diff --git a/.github/workflows/build-firmware.yml b/.github/workflows/build-firmware.yml index 599c0ad3b..9a951fdc2 100644 --- a/.github/workflows/build-firmware.yml +++ b/.github/workflows/build-firmware.yml @@ -6,6 +6,7 @@ on: branches: - master - development + - feature/* pull_request: branches: - master @@ -40,6 +41,49 @@ jobs: name: OpenFFBoard-Firmware-${{ matrix.target }} path: ./Firmware/Output + Build-ESP32SX: + strategy: + fail-fast: false + matrix: + target: ['esp32s2', 'esp32s3'] # Targets to build + runs-on: ubuntu-latest + env: + PROJECT_PATH: Firmware/Targets/ESP32SX + container: espressif/idf:release-v4.4 + steps: + - name: Checkout repo + uses: actions/checkout@v2 + + - name: esp-idf build + run: | + pwd + ls -al + git config --global --add safe.directory $GITHUB_WORKSPACE + git log -n1 + cd /opt/esp/idf + git checkout c29343eb94d + git submodule update --init --recursive + git status + tools/idf_tools.py --non-interactive install cmake + rm $IDF_TOOLS_PATH/idf-env.json + ./install.sh + source ./export.sh + cd $GITHUB_WORKSPACE/$PROJECT_PATH + idf.py set-target ${{ matrix.target }} + idf.py build + esptool.py --chip ${{ matrix.target }} merge_bin -o build/OpenFFBoard-Firmware-${{ matrix.target }}.bin --flash_mode dio --flash_size 4MB 0x1000 build/bootloader/bootloader.bin 0x10000 build/openffboard.bin 0x8000 build/partition_table/partition-table.bin + shell: bash + + - name: Upload a Build Artifact + uses: actions/upload-artifact@v3.0.0 + with: + # Artifact name + name: OpenFFBoard-Firmware-${{ matrix.target }} + # A file, directory or wildcard pattern that describes what to upload + path: | + Firmware/Targets/ESP32SX/build/OpenFFBoard-Firmware-*.bin + retention-days: 90 + # Release: # needs: [Build_firmware] # Requires build first # name: Release if tagged @@ -60,4 +104,5 @@ jobs: # if: startsWith(github.ref, 'refs/tags/') # with: # body_path: ${{ github.workspace }}/CHANGELOG.txt - # body: "Release notes coming soon" \ No newline at end of file + # body: "Release notes coming soon" + diff --git a/Firmware/FFBoard/Inc/CAN.h b/Firmware/FFBoard/Inc/CAN.h index 5715410b6..896a81881 100644 --- a/Firmware/FFBoard/Inc/CAN.h +++ b/Firmware/FFBoard/Inc/CAN.h @@ -24,7 +24,7 @@ typedef struct{ typedef struct{ uint8_t data[8] = {0}; - CAN_RxHeaderTypeDef header = {0,0,0,0,0,0}; + CAN_RxHeaderTypeDef header = {0,0,0,0,0,0,0}; } CAN_rx_msg; diff --git a/Firmware/FFBoard/Inc/CommandInterface.h b/Firmware/FFBoard/Inc/CommandInterface.h index d99ec7b92..a5bd5d0b5 100644 --- a/Firmware/FFBoard/Inc/CommandInterface.h +++ b/Firmware/FFBoard/Inc/CommandInterface.h @@ -14,6 +14,17 @@ #include "thread.hpp" #include "CommandHandler.h" +#ifdef HW_ESP32SX +#define CDC_COMMANDINTERFACE_THREAD_MEM 4096 +#define CDC_COMMANDINTERFACE_THREAD_PRIO (37*25/56) +#define UART_COMMANDINTERFACE_THREAD_MEM CDC_COMMANDINTERFACE_THREAD_MEM +#define UART_COMMANDINTERFACE_THREAD_PRIO CDC_COMMANDINTERFACE_THREAD_PRIO +#else +#define CDC_COMMANDINTERFACE_THREAD_MEM 512 +#define CDC_COMMANDINTERFACE_THREAD_PRIO 36 +#define UART_COMMANDINTERFACE_THREAD_MEM 150 +#define UART_COMMANDINTERFACE_THREAD_PRIO CDC_COMMANDINTERFACE_THREAD_PRIO +#endif class FFBoardMainCommandThread; diff --git a/Firmware/FFBoard/Inc/DebugLog.h b/Firmware/FFBoard/Inc/DebugLog.h new file mode 100644 index 000000000..da74a9cb0 --- /dev/null +++ b/Firmware/FFBoard/Inc/DebugLog.h @@ -0,0 +1,40 @@ + +#ifndef DEBUGLOG_H_ +#define DEBUGLOG_H_ + +#include "target_constants.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// LOG DEBUG +#ifdef HW_ESP32SX +#define FFB_LOGI(format, ...) do { \ + ESP_LOGI(__FUNCTION__, "%s:%d -- " format, __FILE__, __LINE__ __VA_OPT__(,) __VA_ARGS__); \ + } while(0) +#define FFB_LOGW(format, ...) do { \ + ESP_LOGW(__FUNCTION__, "%s:%d -- " format, __FILE__, __LINE__ __VA_OPT__(,) __VA_ARGS__); \ + } while(0) +#define FFB_LOGE(format, ...) do { \ + ESP_LOGE(__FUNCTION__, "%s:%d -- " format, __FILE__, __LINE__ __VA_OPT__(,) __VA_ARGS__); \ + } while(0) +#define FFB_LOGD(format, ...) do { \ + ESP_LOGD(__FUNCTION__, "%s:%d -- " format, __FILE__, __LINE__ __VA_OPT__(,) __VA_ARGS__); \ + } while(0) + +#else + +#define FFB_LOGI(format, ...) +#define FFB_LOGW(format, ...) +#define FFB_LOGE(format, ...) +#define FFB_LOGD(format, ...) + +#endif + + +#ifdef __cplusplus +} +#endif + +#endif /* DEBUGLOG_H_ */ diff --git a/Firmware/FFBoard/Inc/ErrorHandler.h b/Firmware/FFBoard/Inc/ErrorHandler.h index d62f1812a..65af1c9c8 100644 --- a/Firmware/FFBoard/Inc/ErrorHandler.h +++ b/Firmware/FFBoard/Inc/ErrorHandler.h @@ -11,6 +11,15 @@ #include #include "thread.hpp" #include "CommandHandler.h" +#include "target_constants.h" + +#ifdef HW_ESP32SX +#define ERROR_PRINTER_MEM 4096 +#define ERROR_PRINTER_PRIO 19*25/56 +#else +#define ERROR_PRINTER_MEM 512 +#define ERROR_PRINTER_PRIO 19 +#endif /* * Error code definitions diff --git a/Firmware/FFBoard/Inc/FFBoardMainCommandThread.h b/Firmware/FFBoard/Inc/FFBoardMainCommandThread.h index c92b35a08..a8489392c 100644 --- a/Firmware/FFBoard/Inc/FFBoardMainCommandThread.h +++ b/Firmware/FFBoard/Inc/FFBoardMainCommandThread.h @@ -11,9 +11,7 @@ #include #include "cppmain.h" -#include "main.h" #include -#include "cdc_device.h" #include "ChoosableClass.h" #include "CommandHandler.h" #include @@ -24,6 +22,14 @@ #include "CommandInterface.h" +#ifdef HW_ESP32SX +#define FFBOARDMAINCOMMANDTHREAD_MEM 4096 +#define FFBOARDMAINCOMMANDTHREAD_PRIO 32*25/56 +#else +#define FFBOARDMAINCOMMANDTHREAD_MEM 700 +#define FFBOARDMAINCOMMANDTHREAD_PRIO 32 +#endif + class FFBoardMain; //class CommandInterface; diff --git a/Firmware/FFBoard/Inc/HidCommandInterface.h b/Firmware/FFBoard/Inc/HidCommandInterface.h index 36eab6f17..2868eace6 100644 --- a/Firmware/FFBoard/Inc/HidCommandInterface.h +++ b/Firmware/FFBoard/Inc/HidCommandInterface.h @@ -13,6 +13,14 @@ #include "ffb_defs.h" #include "CommandHandler.h" +#ifdef HW_ESP32SX +#define HID_COMMANDINTERFACE_MEM 2048 +#define HID_COMMANDINTERFACE_PRIO 18*25/56 +#else +#define HID_COMMANDINTERFACE_MEM 128 +#define HID_COMMANDINTERFACE_PRIO 18 +#endif + enum class HidCmdType : uint8_t {write = 0, request = 1, info = 2, writeAddr = 3, requestAddr = 4,ACK = 10, notFound = 13, notification = 14, err = 15}; diff --git a/Firmware/FFBoard/Inc/OutputPin.h b/Firmware/FFBoard/Inc/OutputPin.h index 4b69e1683..7875b4b54 100644 --- a/Firmware/FFBoard/Inc/OutputPin.h +++ b/Firmware/FFBoard/Inc/OutputPin.h @@ -37,6 +37,8 @@ class OutputPin { return(this->port == b.port && this->pin == b.pin); } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wmissing-field-initializers" /** * Sets a pin into output mode in case it was previously reconfigured */ @@ -48,7 +50,7 @@ class OutputPin { GPIO_InitStruct.Speed = speed; HAL_GPIO_Init(port, &GPIO_InitStruct); } - +#pragma GCC diagnostic pop //const std::string getName(){return name;} diff --git a/Firmware/FFBoard/Inc/USBdevice.h b/Firmware/FFBoard/Inc/USBdevice.h index 9dc879e62..b98656d91 100644 --- a/Firmware/FFBoard/Inc/USBdevice.h +++ b/Firmware/FFBoard/Inc/USBdevice.h @@ -15,7 +15,13 @@ #define USB_STRING_DESC_BUF_SIZE 32 - +#ifdef HW_ESP32SX +#define USBDEVICE_MEM 4096 +#define USBDEVICE_PRIO 40*25/56 +#else +#define USBDEVICE_MEM 256 +#define USBDEVICE_PRIO 40 +#endif /** * This class defines a usb device and implements callbacks for getting the basic diff --git a/Firmware/FFBoard/Inc/cppmain.h b/Firmware/FFBoard/Inc/cppmain.h index c30f99714..8befe1b65 100644 --- a/Firmware/FFBoard/Inc/cppmain.h +++ b/Firmware/FFBoard/Inc/cppmain.h @@ -22,18 +22,23 @@ extern "C" { #include "eeprom_addresses.h" #include "main.h" -#include "cmsis_compiler.h" - +#include "DebugLog.h" void cppmain(); +#ifndef HW_ESP32SX +#include "cmsis_compiler.h" void usb_init(); void tudThread(void *argument); - +#endif #ifdef __cplusplus } static inline bool inIsr(){ +#ifdef HW_ESP32SX + return xPortInIsrContext(); +#else return (__get_PRIMASK() != 0U) || (__get_IPSR() != 0U); +#endif } template @@ -53,12 +58,14 @@ T clip(T v, C l, C h) { return { v > h ? h : v < l ? l : v }; } - +#ifdef HW_ESP32SX +#define micros() esp_timer_get_time() // Returns microsecond scaled time +#else uint32_t micros(); // Returns microsecond scaled time unsigned long getRunTimeCounterValue(void); // RTOS void refreshWatchdog(); // Refreshes the watchdog - +#endif #endif diff --git a/Firmware/FFBoard/Inc/critical.hpp b/Firmware/FFBoard/Inc/critical.hpp index 80dbac826..71d86db2a 100644 --- a/Firmware/FFBoard/Inc/critical.hpp +++ b/Firmware/FFBoard/Inc/critical.hpp @@ -65,7 +65,9 @@ class CriticalSection { */ static inline void Enter() { + #ifndef HW_ESP32SX taskENTER_CRITICAL(); + #endif } /** @@ -73,7 +75,9 @@ class CriticalSection { */ static inline void Exit() { + #ifndef HW_ESP32SX taskEXIT_CRITICAL(); + #endif } /** diff --git a/Firmware/FFBoard/Inc/tickhook.hpp b/Firmware/FFBoard/Inc/tickhook.hpp index 76ec36600..486fea401 100644 --- a/Firmware/FFBoard/Inc/tickhook.hpp +++ b/Firmware/FFBoard/Inc/tickhook.hpp @@ -44,7 +44,7 @@ #include "task.h" #include -#if ( configUSE_TICK_HOOK == 1 ) +#if ( configUSE_TICK_HOOK == 1) && !defined(HW_ESP32SX) /** * FreeRTOS expects this function to exist and requires it to be diff --git a/Firmware/FFBoard/Src/CAN.cpp b/Firmware/FFBoard/Src/CAN.cpp index f60e8ebbc..cc8d3de35 100644 --- a/Firmware/FFBoard/Src/CAN.cpp +++ b/Firmware/FFBoard/Src/CAN.cpp @@ -205,7 +205,12 @@ void CANPort::setSpeedPreset(uint8_t preset){ takeSemaphore(); HAL_CAN_Stop(this->hcan); HAL_CAN_AbortTxRequest(hcan, txMailbox); +#ifdef HW_ESP32SX + const uint32_t rate[6]={50000, 100000, 125000, 250000, 500000, 1000000}; //bit/s + glue_can_set_speed(this->hcan, rate[preset]); +#else this->hcan->Instance->BTR = canSpeedBTR_preset[preset]; +#endif HAL_CAN_ResetError(hcan); HAL_CAN_Start(this->hcan); diff --git a/Firmware/FFBoard/Src/CDCcomm.cpp b/Firmware/FFBoard/Src/CDCcomm.cpp index 5bea71355..dd5a85845 100644 --- a/Firmware/FFBoard/Src/CDCcomm.cpp +++ b/Firmware/FFBoard/Src/CDCcomm.cpp @@ -29,7 +29,12 @@ CDCcomm::~CDCcomm() { * Global callback if cdc transfer is finished. Used to retry a failed transfer */ void CDCcomm::cdcFinished(uint8_t itf){ - cdcSems[itf].Give(); + bool ret = cdcSems[itf].Give(); + if (true != ret){ + // FFB_LOGE("cdcSem give error\n"); + return; + } + if(CDCcomm::usb_busy_retry && CDCcomm::remainingStrs[itf].length() > 0){ cdcSend(&CDCcomm::remainingStrs[itf], itf); // Retry with remaining string } @@ -67,8 +72,12 @@ uint16_t CDCcomm::cdcSend(std::string* reply,uint8_t itf){ cdcSems[itf].Take(); uint32_t bufferRemaining = tud_cdc_n_write_available(itf); uint32_t cdc_sent = tud_cdc_n_write(itf,reply->c_str(), std::min(reply->length(),bufferRemaining)); - if(!usb_busy_retry) // We did not retransmit so flush now. otherwise TUD will flush if we were in the callback before - tud_cdc_n_write_flush(itf); + if(!usb_busy_retry){ // We did not retransmit so flush now. otherwise TUD will flush if we were in the callback before + int res = tud_cdc_n_write_flush(itf); + if (!res) { + FFB_LOGW("flush failed (res: %d, len=%d)", res, reply->length()); + } + } // If we can't write the whole reply copy remainder to send later if(cdc_sent < reply->length()){ diff --git a/Firmware/FFBoard/Src/CmdParser.cpp b/Firmware/FFBoard/Src/CmdParser.cpp index 73640b9fd..f3c777b2e 100644 --- a/Firmware/FFBoard/Src/CmdParser.cpp +++ b/Firmware/FFBoard/Src/CmdParser.cpp @@ -142,8 +142,8 @@ bool CmdParser::parse(std::vector& commands){ cmd.type = CMDtype::err; }else{ - uint32_t peq = word.find('=', 0); // set - uint32_t pqm = word.find('?', 0); // read with var + int32_t peq = word.find('=', 0); // set + int32_t pqm = word.find('?', 0); // read with var // \n if(pqm == std::string::npos && peq == std::string::npos){ diff --git a/Firmware/FFBoard/Src/CommandHandler.cpp b/Firmware/FFBoard/Src/CommandHandler.cpp index ae7c3fcbc..5dc07e02d 100644 --- a/Firmware/FFBoard/Src/CommandHandler.cpp +++ b/Firmware/FFBoard/Src/CommandHandler.cpp @@ -132,7 +132,7 @@ std::string CommandHandler::getCsvHelpstring(){ for(CmdHandlerCommanddef& cmd : registeredCommands){ if(cmd.helpstring != nullptr && cmd.cmd != nullptr){ char cmdhex[11]; - std::snprintf(cmdhex,11,"0x%lX",cmd.cmdId); + std::snprintf(cmdhex,11,"0x%X",cmd.cmdId); helpstring.append(cmd.cmd); helpstring += ','; helpstring += std::string(cmdhex); diff --git a/Firmware/FFBoard/Src/CommandInterface.cpp b/Firmware/FFBoard/Src/CommandInterface.cpp index 9aa004c91..e2f00bb1c 100644 --- a/Firmware/FFBoard/Src/CommandInterface.cpp +++ b/Firmware/FFBoard/Src/CommandInterface.cpp @@ -240,7 +240,7 @@ void StringCommandInterface::generateReplyFromCmd(std::string& replyPart,const P */ -CDC_CommandInterface::CDC_CommandInterface() : StringCommandInterface(1024), Thread("CDCCMD", 512, 37) { +CDC_CommandInterface::CDC_CommandInterface() : StringCommandInterface(1024), Thread("CDCCMD", CDC_COMMANDINTERFACE_THREAD_MEM, CDC_COMMANDINTERFACE_THREAD_PRIO) { parser.setClearBufferTimeout(parserTimeout); this->Start(); } @@ -273,11 +273,13 @@ void CDC_CommandInterface::sendReplies(const std::vector& results if(HAL_GetTick() - lastSendTime > parserTimeout){ bufferLength = 0; + FFB_LOGE("parserTimeout"); resultsBuffer.clear(); // Empty buffer because we were not able to reply in time to prevent the full buffer from blocking future commands //CDCcomm::clearRemainingBuffer(0); } if( (!enableBroadcastFromOtherInterfaces && originalInterface != this) ){ + FFB_LOGW("not originalInterface"); return; } for(const CommandResult& r : results){ @@ -324,7 +326,7 @@ bool CDC_CommandInterface::readyToSend(){ */ extern UARTPort external_uart; // defined in cpp_target_config.cpp -UART_CommandInterface::UART_CommandInterface(uint32_t baud) : UARTDevice(external_uart),Thread("UARTCMD", 150, 36),StringCommandInterface(512), baud(baud){ // +UART_CommandInterface::UART_CommandInterface(uint32_t baud) : UARTDevice(external_uart),Thread("UARTCMD", UART_COMMANDINTERFACE_THREAD_MEM, UART_COMMANDINTERFACE_THREAD_PRIO),StringCommandInterface(512), baud(baud){ // uartconfig = uartport->getConfig(); if(baud != 0){ uartconfig.BaudRate = this->baud; diff --git a/Firmware/FFBoard/Src/ErrorHandler.cpp b/Firmware/FFBoard/Src/ErrorHandler.cpp index ffb15dc02..f775bcee9 100644 --- a/Firmware/FFBoard/Src/ErrorHandler.cpp +++ b/Firmware/FFBoard/Src/ErrorHandler.cpp @@ -131,7 +131,7 @@ void ErrorHandler::errorCallback(const Error &error, bool cleared){ // return info; //} -ErrorPrinter::ErrorPrinter() : Thread("errprint",256,19){ // Higher than default task but low. +ErrorPrinter::ErrorPrinter() : Thread("errprint",ERROR_PRINTER_MEM,ERROR_PRINTER_PRIO){ // Higher than default task but low. this->Start(); } diff --git a/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp b/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp index 13512c4ac..f7993ba97 100644 --- a/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp +++ b/Firmware/FFBoard/Src/FFBoardMainCommandThread.cpp @@ -30,7 +30,7 @@ Error FFBoardMainCommandThread::cmdExecError = Error(ErrorCode::cmdExecutionErro FFBoardMainCommandThread* commandThread; // Note: allocate enough memory for the command thread to store replies -FFBoardMainCommandThread::FFBoardMainCommandThread(FFBoardMain* mainclass) : Thread("CMD_MAIN",700, 32) { +FFBoardMainCommandThread::FFBoardMainCommandThread(FFBoardMain* mainclass) : Thread("CMD_MAIN",FFBOARDMAINCOMMANDTHREAD_MEM, FFBOARDMAINCOMMANDTHREAD_PRIO) { //main = mainclass; commandThread = this; this->Start(); diff --git a/Firmware/FFBoard/Src/HidCommandInterface.cpp b/Firmware/FFBoard/Src/HidCommandInterface.cpp index c6bcc7a96..8fb08f6a9 100644 --- a/Firmware/FFBoard/Src/HidCommandInterface.cpp +++ b/Firmware/FFBoard/Src/HidCommandInterface.cpp @@ -19,7 +19,7 @@ HID_CommandInterface* HID_CommandInterface::globalInterface = nullptr; -HID_CommandInterface::HID_CommandInterface() : cpp_freertos::Thread("HIDCMD",128,18){ +HID_CommandInterface::HID_CommandInterface() : cpp_freertos::Thread("HIDCMD",HID_COMMANDINTERFACE_MEM,HID_COMMANDINTERFACE_PRIO){ globalInterface = this; this->Start(); } diff --git a/Firmware/FFBoard/Src/SystemCommands.cpp b/Firmware/FFBoard/Src/SystemCommands.cpp index 331f3132a..24d312c6b 100644 --- a/Firmware/FFBoard/Src/SystemCommands.cpp +++ b/Firmware/FFBoard/Src/SystemCommands.cpp @@ -11,8 +11,7 @@ #include "voltagesense.h" #include #include "constants.h" -#include "task.h" -#include "FreeRTOSConfig.h" + extern ClassChooser mainchooser; extern FFBoardMain* mainclass; //extern static const uint8_t SW_VERSION_INT[3]; diff --git a/Firmware/FFBoard/Src/USBdevice.cpp b/Firmware/FFBoard/Src/USBdevice.cpp index 5c2cdc40b..1233bffed 100644 --- a/Firmware/FFBoard/Src/USBdevice.cpp +++ b/Firmware/FFBoard/Src/USBdevice.cpp @@ -11,7 +11,7 @@ uint16_t _desc_str[USB_STRING_DESC_BUF_SIZE]; // String buffer USBdevice::USBdevice(const tusb_desc_device_t* deviceDesc,const uint8_t (*confDesc),const usb_string_desc_t* strings, uint8_t appendSerial) : -Thread("USB", 256, 40), desc_device(deviceDesc), desc_conf(confDesc), string_desc(strings),appendSerial(appendSerial) { +Thread("USB", USBDEVICE_MEM, USBDEVICE_PRIO), desc_device(deviceDesc), desc_conf(confDesc), string_desc(strings),appendSerial(appendSerial) { } diff --git a/Firmware/FFBoard/Src/cppmain.cpp b/Firmware/FFBoard/Src/cppmain.cpp index 0fcf6d8ba..bc7efa58b 100644 --- a/Firmware/FFBoard/Src/cppmain.cpp +++ b/Firmware/FFBoard/Src/cppmain.cpp @@ -5,12 +5,12 @@ #include "global_callbacks.h" #include "cpp_target_config.h" #include "cmsis_os.h" -#include "stm32f4xx_hal_flash.h" +#ifndef HW_ESP32SX #include "tusb.h" - uint32_t clkmhz = HAL_RCC_GetHCLKFreq() / 100000; extern TIM_HandleTypeDef TIM_MICROS; +#endif #ifdef HAL_IWDG_MODULE_ENABLED extern IWDG_HandleTypeDef hiwdg; // Watchdog @@ -21,7 +21,11 @@ bool mainclassChosen = false; uint16_t main_id = 0; +#ifdef HW_ESP32SX +FFBoardMain* mainclass; +#else FFBoardMain* mainclass __attribute__((section (".ccmram"))); +#endif ClassChooser mainchooser(class_registry); @@ -30,6 +34,7 @@ StackType_t usb_device_stack[USBD_STACK_SIZE]; StaticTask_t usb_device_taskdef; +#ifndef HW_ESP32SX void cppmain() { #ifdef FW_DEVID if(HAL_GetDEVID() != FW_DEVID){ @@ -140,3 +145,41 @@ unsigned long getRunTimeCounterValue(void){ return micros(); } +#else // For HW_ESP32SX +static const char *TAG = "cppmain"; +void cppmain() +{ + if ( EE_Init() != HAL_OK) { + Error_Handler(); + } + + startADC(); // enable ADC DMA + + // If switch pressed at boot select failsafe implementation + if (HAL_GPIO_ReadPin(BUTTON_A_GPIO_Port, BUTTON_A_Pin) == 0) { + ESP_LOGI(TAG, "button pressed, boot select failsafe implementation"); + main_id = 0; + } else { + if (!Flash_ReadWriteDefault(ADR_CURRENT_CONFIG, &main_id, 0)) { + Error_Handler(); + } + } + + ESP_LOGI(TAG, "main_id=%d", main_id); + mainclass = mainchooser.Create(main_id); + if (mainclass == nullptr) { // invalid id + mainclass = mainchooser.Create(0); // Baseclass + } + mainclassChosen = true; + + mainclass->usbInit(); // Let mainclass initialize usb + + while (running) { + mainclass->update(); + updateLeds(); + //external_spi.process(); + vTaskDelay(1); + } +} +#endif + diff --git a/Firmware/FFBoard/Src/ctickhook.cpp b/Firmware/FFBoard/Src/ctickhook.cpp index 3702370b3..56d33182b 100644 --- a/Firmware/FFBoard/Src/ctickhook.cpp +++ b/Firmware/FFBoard/Src/ctickhook.cpp @@ -36,10 +36,10 @@ * ***************************************************************************/ - +#include "target_constants.h" #include "tickhook.hpp" -#if ( configUSE_TICK_HOOK == 1 ) +#if ( configUSE_TICK_HOOK == 1) && !defined(HW_ESP32SX) using namespace std; using namespace cpp_freertos; diff --git a/Firmware/FFBoard/Src/eeprom.c b/Firmware/FFBoard/Src/eeprom.c index 3d3305a94..c83208f36 100644 --- a/Firmware/FFBoard/Src/eeprom.c +++ b/Firmware/FFBoard/Src/eeprom.c @@ -49,6 +49,168 @@ /* Includes ------------------------------------------------------------------*/ #include "eeprom.h" #include "eeprom_addresses.h" + +#ifdef HW_ESP32SX + +#include +#include "esp_system.h" +#include "esp_log.h" +#include "nvs_flash.h" + +static const char *TAG = "eeprom"; + +#define _VAL_NAMESPACE "DefParam" + +#define PARAM_CHECK(a, str, label) \ + if (!(a)) { \ + ESP_LOGE(TAG,"%s(%d): %s", __FUNCTION__, __LINE__, str); \ + goto label; \ + } + +static esp_err_t param_save(const char *space_name, const char *key, void *param, uint16_t len) +{ + esp_err_t ret = ESP_ERR_INVALID_ARG; + PARAM_CHECK(NULL != space_name, "Pointer of space_name is invalid", OPEN_FAIL); + PARAM_CHECK(NULL != key, "Pointer of key is invalid", OPEN_FAIL); + PARAM_CHECK(NULL != param, "Pointer of param is invalid", OPEN_FAIL); + nvs_handle_t my_handle; + ret = nvs_open(space_name, NVS_READWRITE, &my_handle); + PARAM_CHECK(ESP_OK == ret, "nvs open failed", OPEN_FAIL); + ret = nvs_set_blob(my_handle, key, param, len); + PARAM_CHECK(ESP_OK == ret, "nvs set blob failed", SAVE_FINISH); + ret = nvs_commit(my_handle); + +SAVE_FINISH: + nvs_close(my_handle); + +OPEN_FAIL: + return ret; +} + +static esp_err_t param_load(const char *space_name, const char *key, void *dest) +{ + esp_err_t ret = ESP_ERR_INVALID_ARG; + PARAM_CHECK(NULL != space_name, "Pointer of space_name is invalid", OPEN_FAIL); + PARAM_CHECK(NULL != key, "Pointer of key is invalid", OPEN_FAIL); + PARAM_CHECK(NULL != dest, "Pointer of dest is invalid", OPEN_FAIL); + nvs_handle_t my_handle; + size_t required_size = 0; + ret = nvs_open(space_name, NVS_READWRITE, &my_handle); + PARAM_CHECK(ESP_OK == ret, "nvs open failed", OPEN_FAIL); + ret = nvs_get_blob(my_handle, key, NULL, &required_size); + PARAM_CHECK(ESP_OK == ret, "nvs get blob failed", LOAD_FINISH); + if (required_size == 0) { + ESP_LOGW(TAG, "the target you want to load has never been saved"); + ret = ESP_FAIL; + goto LOAD_FINISH; + } + ret = nvs_get_blob(my_handle, key, dest, &required_size); + +LOAD_FINISH: + nvs_close(my_handle); + +OPEN_FAIL: + return ret; +} + +__attribute__((__unused__)) +static esp_err_t param_erase(const char *space_name, const char *key) +{ + esp_err_t ret = ESP_ERR_INVALID_ARG; + PARAM_CHECK(NULL != space_name, "Pointer of space_name is invalid", OPEN_FAIL); + PARAM_CHECK(NULL != key, "Pointer of key is invalid", OPEN_FAIL); + nvs_handle_t my_handle; + ret = nvs_open(space_name, NVS_READWRITE, &my_handle); + PARAM_CHECK(ESP_OK == ret, "nvs open failed", OPEN_FAIL); + ret = nvs_erase_key(my_handle, key); + PARAM_CHECK(ESP_OK == ret, "nvs erase failed", ERASE_FINISH); + ret = nvs_commit(my_handle); + +ERASE_FINISH: + nvs_close(my_handle); + +OPEN_FAIL: + return ret; +} + +/* Virtual address defined by the user: 0xFFFF value is prohibited */ +//extern uint16_t VirtAddVarTab[NB_OF_VAR]; + +static void inline make_key(uint16_t addr, char *key) +{ + sprintf(key, "0x%x", addr); +} +/** + * @brief Restore the pages to a known good state in case of page's status + * corruption after a power loss. + * @param None. + * @retval - Flash error code: on write Flash error + * - FLASH_COMPLETE: on success + */ +uint16_t EE_Init(void) +{ + return HAL_OK; +} + + +/** + * @brief Returns the last stored variable data, if found, which correspond to + * the passed virtual address + * @param VirtAddress: Variable virtual address + * @param Data: Global variable contains the read variable value + * @retval Success or error status: + * - 0: if variable was found + * - 1: if the variable was not found + * - NO_VALID_PAGE: if no valid page was found. + */ +uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t *Data) +{ + char key[16] = {0}; + make_key(VirtAddress, key); + esp_err_t ret = param_load(_VAL_NAMESPACE, key, Data); + if (ESP_OK == ret) { + ESP_LOGI(TAG, "read nvs %s=%d", key, *Data); + } else { + ESP_LOGE(TAG, "read nvs %s error", key); + } + /* Return ReadStatus value: (0: variable exist, 1: variable doesn't exist) */ + return ret == ESP_OK ? 0 : 1; +} + +/** + * @brief Writes/upadtes variable data in EEPROM. + * @param VirtAddress: Variable virtual address + * @param Data: 16 bit data to be written + * @retval Success or error status: + * - FLASH_COMPLETE: on success + * - PAGE_FULL: if valid page is full + * - NO_VALID_PAGE: if no valid page was found + * - Flash error code: on write Flash error + */ +uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data) +{ + char key[16] = {0}; + make_key(VirtAddress, key); + ESP_LOGI(TAG, "write nvs %s=%d", key, Data); + esp_err_t ret = param_save(_VAL_NAMESPACE, key, &Data, 2); + + return ret == ESP_OK ? 0 : 1; +} + +/** + * @brief Erases PAGE and PAGE1 and writes VALID_PAGE header to PAGE + * @param None + * @retval Status of the last operation (Flash write or erase) done during + * EEPROM formating + */ +HAL_StatusTypeDef EE_Format(void) +{ + ESP_ERROR_CHECK(nvs_flash_erase()); + return HAL_OK; +} + +#else // STM32 FLASH + /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ @@ -729,6 +891,7 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data) /* Return last operation flash status */ return FlashStatus; } +#endif /** * @} diff --git a/Firmware/FFBoard/Src/flash_helpers.cpp b/Firmware/FFBoard/Src/flash_helpers.cpp index a35f942c5..17d9d92c0 100644 --- a/Firmware/FFBoard/Src/flash_helpers.cpp +++ b/Firmware/FFBoard/Src/flash_helpers.cpp @@ -25,6 +25,7 @@ bool Flash_Write(uint16_t adr,uint16_t dat){ bool res = false; if(readRes == 1 || (readRes == 0 && buf != dat) ){ // Only write if var updated HAL_FLASH_Unlock(); + #ifndef HW_ESP32SX // Clear all the error flags __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPERR); @@ -32,6 +33,7 @@ bool Flash_Write(uint16_t adr,uint16_t dat){ __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_PGAERR); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_PGPERR); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_PGSERR); + #endif if(EE_WriteVariable(adr, dat) == HAL_OK){ res = true; } @@ -60,6 +62,7 @@ bool Flash_ReadWriteDefault(uint16_t adr,uint16_t *buf,uint16_t def){ if(EE_ReadVariable(adr, buf) != 0){ *buf = def; HAL_FLASH_Unlock(); + #ifndef HW_ESP32SX // Clear all the error flags __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPERR); @@ -67,6 +70,7 @@ bool Flash_ReadWriteDefault(uint16_t adr,uint16_t *buf,uint16_t def){ __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_PGAERR); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_PGPERR); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_PGSERR); + #endif EE_WriteVariable(adr, def); HAL_FLASH_Lock(); return false; diff --git a/Firmware/FFBoard/Src/global_callbacks.cpp b/Firmware/FFBoard/Src/global_callbacks.cpp index 1b8262d3c..f33712cb1 100644 --- a/Firmware/FFBoard/Src/global_callbacks.cpp +++ b/Firmware/FFBoard/Src/global_callbacks.cpp @@ -58,9 +58,11 @@ extern ADC_HandleTypeDef hadc3; * Callback after an adc finished conversion */ void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc){ +#ifndef HW_ESP32SX //Pulse braking mosfet if internal voltage is higher than supply. if(hadc == &VSENSE_HADC) brakeCheck(); +#endif uint8_t chans = 0; volatile uint32_t* buf = getAnalogBuffer(hadc,&chans); @@ -76,16 +78,21 @@ void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc){ * Note: this is normally generated in the main.c * A call to HAL_TIM_PeriodElapsedCallback_CPP must be added there instead! */ +#ifndef HW_ESP32SX __weak void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim){ HAL_TIM_PeriodElapsedCallback_CPP(htim); } - +#endif void HAL_TIM_PeriodElapsedCallback_CPP(TIM_HandleTypeDef* htim) { for(TimerHandler* c : TimerHandler::timerHandlers){ c->timerElapsed(htim); } } +#ifdef __cplusplus +extern "C" { +#endif + /** * Callback for GPIO interrupts */ @@ -107,7 +114,7 @@ void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart){ } } -#ifdef CANBUS +#if defined(CANBUS) && !defined(HW_ESP32SX) // CAN uint8_t canRxBuf0[8]; @@ -253,7 +260,9 @@ void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c){ c->I2cError(hi2c); } } - +#ifdef __cplusplus +} +#endif //void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c); //void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c); @@ -365,6 +374,32 @@ void tud_resume_cb(){ mainclass->usbResume(); } +#ifdef HW_ESP32SX +extern "C" volatile uint32_t* getAnalogBuffer(ADC_HandleTypeDef* hadc,uint8_t* chans) +{ + uint8_t result[12] = {0}; + uint32_t ret_num = 0; + esp_err_t ret = adc_digi_read_bytes(result, 12, &ret_num, ADC_MAX_DELAY); + if (ret == ESP_OK || ret == ESP_ERR_INVALID_STATE) + { + for (size_t i = 0; i < ret_num; i+=2) + { + adc_digi_output_data_t *p = (adc_digi_output_data_t *)&result[i]; +#ifdef CONFIG_IDF_TARGET_ESP32S3 + ADC1_BUF[p->type2.channel] = p->type2.data; +#elif defined CONFIG_IDF_TARGET_ESP32S2 + ADC1_BUF[p->type1.channel] = p->type1.data; +#endif + } + ESP_LOGV(__FUNCTION__, "[%d] Channel: 0|%04d, 1|%04d, 2|%04d", ret_num, + ADC1_BUF[0], + ADC1_BUF[1], + ADC1_BUF[2]); + } + *chans = ADC1_CHANNELS; + return ADC1_BUF; +} +#else volatile uint32_t* getAnalogBuffer(ADC_HandleTypeDef* hadc,uint8_t* chans){ #ifdef ADC1_CHANNELS @@ -389,6 +424,7 @@ volatile uint32_t* getAnalogBuffer(ADC_HandleTypeDef* hadc,uint8_t* chans){ #endif return NULL; } +#endif void startADC(){ #ifdef ADC1_CHANNELS diff --git a/Firmware/FFBoard/Src/voltagesense.cpp b/Firmware/FFBoard/Src/voltagesense.cpp index f9feb2244..d26422558 100644 --- a/Firmware/FFBoard/Src/voltagesense.cpp +++ b/Firmware/FFBoard/Src/voltagesense.cpp @@ -40,11 +40,19 @@ void setupBrakePin(uint32_t vdiffAct,uint32_t vdiffDeact,uint32_t vMax){ } uint16_t getIntV(){ +#ifdef HW_ESP32SX + return 20000; // ESP32S2 don't have enough gpio to measure v_int +#else return VSENSE_ADC_BUF[ADC_CHAN_VINT] * vSenseMult; +#endif } uint16_t getExtV(){ +#ifdef HW_ESP32SX + return 20000; // ESP32S2 don't have enough gpio to measure v_ext +#else return VSENSE_ADC_BUF[ADC_CHAN_VEXT] * vSenseMult; +#endif } void brakeCheck(){ diff --git a/Firmware/FFBoard/UserExtensions/Inc/ADS111X.h b/Firmware/FFBoard/UserExtensions/Inc/ADS111X.h index 3989a602d..c755b6cab 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/ADS111X.h +++ b/Firmware/FFBoard/UserExtensions/Inc/ADS111X.h @@ -19,6 +19,14 @@ #ifdef I2C_PORT +#ifdef HW_ESP32SX +#define ADS111X_ANALOG_THREAD_MEM 1024 +#define ADS111X_ANALOG_THREAD_PRIO (25*25/56) +#else +#define ADS111X_ANALOG_THREAD_MEM 64 +#define ADS111X_ANALOG_THREAD_PRIO 25 +#endif + class ADS111X : public I2CDevice { public: diff --git a/Firmware/FFBoard/UserExtensions/Inc/EncoderBissC.h b/Firmware/FFBoard/UserExtensions/Inc/EncoderBissC.h index 1fc47fab9..7118656be 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/EncoderBissC.h +++ b/Firmware/FFBoard/UserExtensions/Inc/EncoderBissC.h @@ -20,6 +20,14 @@ #include "semaphore.hpp" #include "array" +#ifdef HW_ESP32SX +#define ENCODERBISSC_THREAD_MEM 1024 +#define ENCODERBISSC_THREAD_PRIO (42*25/56) +#else +#define ENCODERBISSC_THREAD_MEM 64 +#define ENCODERBISSC_THREAD_PRIO 42 +#endif + class EncoderBissC: public Encoder, public SPIDevice , public CommandHandler,cpp_freertos::Thread,public PersistentStorage { public: diff --git a/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h b/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h index 9107fc4e9..8e762363c 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h +++ b/Firmware/FFBoard/UserExtensions/Inc/FFBHIDMain.h @@ -33,6 +33,14 @@ #include "thread.hpp" +#ifdef HW_ESP32SX +#define FFBHIDMAIN_THREAD_MEM 4096 +#define FFBHIDMAIN_THREAD_PRIO (30*25/56) +#else +#define FFBHIDMAIN_THREAD_MEM 256 +#define FFBHIDMAIN_THREAD_PRIO 30 +#endif + class FFBHIDMain: public FFBoardMain, public cpp_freertos::Thread, PersistentStorage,ExtiHandler,public UsbHidHandler, ErrorHandler{ enum class FFBWheel_commands : uint32_t{ ffbactive,axes,btntypes,lsbtn,addbtn,aintypes,lsain,addain,hidrate,hidsendspd,estop,cfrate diff --git a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h index ef1b26a97..197c45b3b 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h +++ b/Firmware/FFBoard/UserExtensions/Inc/MtEncoderSPI.h @@ -18,6 +18,14 @@ #define MAGNTEK_READ 0x80 +#ifdef HW_ESP32SX +#define MTENCODERSPI_THREAD_MEM 4096 +#define MTENCODERSPI_THREAD_PRIO (42*25/56) +#else +#define MTENCODERSPI_THREAD_MEM 256 +#define MTENCODERSPI_THREAD_PRIO 42 +#endif + class MtEncoderSPI: public Encoder, public SPIDevice, public PersistentStorage, public CommandHandler,cpp_freertos::Thread{ enum class MtEncoderSPI_commands : uint32_t{ cspin,pos,errors diff --git a/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h b/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h index 6685474cc..480754971 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h +++ b/Firmware/FFBoard/UserExtensions/Inc/ODriveCAN.h @@ -17,8 +17,14 @@ #include "PersistentStorage.h" #ifdef ODRIVE + +#ifdef HW_ESP32SX +#define ODRIVE_THREAD_MEM 2048 +#define ODRIVE_THREAD_PRIO 25*25/56 +#else #define ODRIVE_THREAD_MEM 256 #define ODRIVE_THREAD_PRIO 25 // Must be higher than main thread +#endif enum class ODriveState : uint32_t {AXIS_STATE_UNDEFINED=0,AXIS_STATE_IDLE=1,AXIS_STATE_STARTUP_SEQUENCE=2,AXIS_STATE_FULL_CALIBRATION_SEQUENCE=3,AXIS_STATE_MOTOR_CALIBRATION=4,AXIS_STATE_ENCODER_INDEX_SEARCH=6,AXIS_STATE_ENCODER_OFFSET_CALIBRATION=7,AXIS_STATE_CLOSED_LOOP_CONTROL=8,AXIS_STATE_LOCKIN_SPIN=9,AXIS_STATE_ENCODER_DIR_FIND=10,AXIS_STATE_HOMING=11,AXIS_STATE_ENCODER_HALL_POLARITY_CALIBRATION=12,AXIS_STATE_ENCODER_HALL_PHASE_CALIBRATION=13}; enum class ODriveControlMode : uint32_t {CONTROL_MODE_VOLTAGE_CONTROL = 0,CONTROL_MODE_TORQUE_CONTROL = 1,CONTROL_MODE_VELOCITY_CONTROL = 2,CONTROL_MODE_POSITION_CONTROL = 3}; diff --git a/Firmware/FFBoard/UserExtensions/Inc/PCF8574.h b/Firmware/FFBoard/UserExtensions/Inc/PCF8574.h index 7ec1faa4f..8eb760468 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/PCF8574.h +++ b/Firmware/FFBoard/UserExtensions/Inc/PCF8574.h @@ -19,6 +19,15 @@ #include "PersistentStorage.h" #include "thread.hpp" #ifdef I2C_PORT + +#ifdef HW_ESP32SX +#define PCF8574BUTTONS_THREAD_MEM 1024 +#define PCF8574BUTTONS_THREAD_PRIO (20*25/56) +#else +#define PCF8574BUTTONS_THREAD_MEM 64 +#define PCF8574BUTTONS_THREAD_PRIO 20 +#endif + class PCF8574 : public I2CDevice { public: PCF8574(I2CPort &port); diff --git a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h index a28c994a7..5d881878c 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h +++ b/Firmware/FFBoard/UserExtensions/Inc/TMC4671.h @@ -28,8 +28,19 @@ #include "Filters.h" #define SPITIMEOUT 500 + +#ifdef HW_ESP32SX +#define TMC_THREAD_MEM 4096 +#define TMC_THREAD_PRIO 25*25/56 +#define TMCENC_THREAD_MEM 2048 +#define TMCENC_THREAD_PRIO 33*25/56 +#else #define TMC_THREAD_MEM 256 #define TMC_THREAD_PRIO 25 // Must be higher than main thread +#define TMCENC_THREAD_MEM 80 +#define TMCENC_THREAD_PRIO 33 +#endif + #define TMC_ADCOFFSETFAIL 5000 // How much offset from 0x7fff to allow before a calibration is failed extern SPI_HandleTypeDef HSPIDRV; diff --git a/Firmware/FFBoard/UserExtensions/Inc/VescCAN.h b/Firmware/FFBoard/UserExtensions/Inc/VescCAN.h index 4eb8c2509..48859460c 100644 --- a/Firmware/FFBoard/UserExtensions/Inc/VescCAN.h +++ b/Firmware/FFBoard/UserExtensions/Inc/VescCAN.h @@ -18,8 +18,15 @@ #include #ifdef VESC + +#ifdef HW_ESP32SX +#define VESC_THREAD_MEM (1024*3) +#define VESC_THREAD_PRIO 25*25/56 +#else #define VESC_THREAD_MEM 512 #define VESC_THREAD_PRIO 25 // Must be higher than main thread +#endif + #define BUFFER_RX_SIZE 32 #define FW_MIN_RELEASE ((5 << 16) | (3 << 8) | 51) diff --git a/Firmware/FFBoard/UserExtensions/Src/ADS111X.cpp b/Firmware/FFBoard/UserExtensions/Src/ADS111X.cpp index 25e1b10b7..4c2bf8625 100644 --- a/Firmware/FFBoard/UserExtensions/Src/ADS111X.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/ADS111X.cpp @@ -174,7 +174,7 @@ const std::array,4> minMaxValAddr = { }; -ADS111X_AnalogSource::ADS111X_AnalogSource() : ADS111X(i2cport) , CommandHandler("adsAnalog", CLSID_ANALOG_ADS111X, 0),AnalogAxisProcessing(4,this,this, true,true,true,true), Thread("ads111x", 64, 25) { +ADS111X_AnalogSource::ADS111X_AnalogSource() : ADS111X(i2cport) , CommandHandler("adsAnalog", CLSID_ANALOG_ADS111X, 0),AnalogAxisProcessing(4,this,this, true,true,true,true), Thread("ads111x", ADS111X_ANALOG_THREAD_MEM, ADS111X_ANALOG_THREAD_PRIO) { CommandHandler::registerCommands(); setAxes(axes,differentialMode); registerCommand("inputs", ADS111X_AnalogSource_commands::axes, "Amount of inputs (1-4 or 1-2 if differential)",CMDFLAG_GET | CMDFLAG_SET); diff --git a/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp b/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp index ddc111c5d..cbc6ccf23 100644 --- a/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/EncoderBissC.cpp @@ -26,7 +26,7 @@ std::array EncoderBissC::tableCRC6n __attribute__((section (".ccmram EncoderBissC::EncoderBissC() : SPIDevice(ENCODER_SPI_PORT, ENCODER_SPI_PORT.getCsPins()[0]), CommandHandler("bissenc",CLSID_ENCODER_BISS,0), - cpp_freertos::Thread("BISSENC",64,42) { + cpp_freertos::Thread("BISSENC",ENCODERBISSC_THREAD_MEM,ENCODERBISSC_THREAD_PRIO) { EncoderBissC::inUse = true; diff --git a/Firmware/FFBoard/UserExtensions/Src/EncoderLocal.cpp b/Firmware/FFBoard/UserExtensions/Src/EncoderLocal.cpp index 8fdd35583..015371d77 100644 --- a/Firmware/FFBoard/UserExtensions/Src/EncoderLocal.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/EncoderLocal.cpp @@ -20,12 +20,15 @@ const ClassIdentifier EncoderLocal::getInfo(){ EncoderLocal::EncoderLocal() : CommandHandler("localenc",CLSID_ENCODER_LOCAL) { EncoderLocal::inUse = true; this->restoreFlash(); +#ifdef HW_ESP32SX + glue_pcnt_init(); +#else this->htim = &TIM_ENC; HAL_TIM_Base_Start_IT(htim); // May immediately call overflow. Initialize count again this->htim->Instance->CNT = 0x7fff; pos = 0; this->htim->Instance->CR1 = 1; - +#endif if(!useIndex) setPos(0); @@ -40,7 +43,11 @@ void EncoderLocal::registerCommands(){ EncoderLocal::~EncoderLocal() { EncoderLocal::inUse = false; +#ifdef HW_ESP32SX + glue_pcnt_deinit(); +#else this->htim->Instance->CR1 = 0; +#endif } @@ -64,7 +71,12 @@ void EncoderLocal::restoreFlash(){ } int32_t EncoderLocal::getTimerCount(){ +#ifdef HW_ESP32SX + FFB_LOGW("Unsupport %s", __FUNCTION__); + return 0; +#else return (int32_t)htim->Instance->CNT - (int32_t)0x7fff; +#endif } EncoderType EncoderLocal::getEncoderType(){ @@ -73,10 +85,18 @@ EncoderType EncoderLocal::getEncoderType(){ int32_t EncoderLocal::getPos(){ +#ifdef HW_ESP32SX + pos += glue_pcnt_get_delta_value(); + return pos; +#else return getTimerCount() + pos; +#endif } void EncoderLocal::setPos(int32_t pos){ +#ifdef HW_ESP32SX + this->pos=pos; +#else int32_t cnt = getTimerCount(); this->pos = pos - cnt; @@ -86,16 +106,23 @@ void EncoderLocal::setPos(int32_t pos){ } //this->pos = pos - cnt; //htim->Instance->CNT = 0x7fff; // Reset //pos+0x7fff; - +#endif } void EncoderLocal::setPeriod(uint32_t period){ +#ifdef HW_ESP32SX + FFB_LOGW("Unsupport %s", __FUNCTION__); +#else this->htim->Instance->ARR = period-1; +#endif } void EncoderLocal::exti(uint16_t GPIO_Pin){ +#ifdef HW_ESP32SX + FFB_LOGW("Unsupport %s", __FUNCTION__); +#else if(GPIO_Pin == ENCODER_Z_Pin){ if(HAL_GPIO_ReadPin(ENCODER_Z_GPIO_Port, ENCODER_Z_Pin) == GPIO_PIN_RESET){ // Encoder Z pin activated @@ -110,6 +137,7 @@ void EncoderLocal::exti(uint16_t GPIO_Pin){ } } +#endif } void EncoderLocal::timerElapsed(TIM_HandleTypeDef* htim){ @@ -119,11 +147,15 @@ void EncoderLocal::timerElapsed(TIM_HandleTypeDef* htim){ } void EncoderLocal::overflowCallback(){ +#ifdef HW_ESP32SX + // nothing to do +#else if(htim->Instance->CNT > this->htim->Instance->ARR/2){ pos -= htim->Instance->ARR+1; }else{ pos += htim->Instance->ARR+1; } +#endif } void EncoderLocal::setCpr(uint32_t cpr){ diff --git a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp index 795b82cbd..961c16237 100644 --- a/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/FFBHIDMain.cpp @@ -22,7 +22,6 @@ #include "ADS111X.h" #include "cmsis_os.h" -extern osThreadId_t defaultTaskHandle; ////////////////////////////////////////////// /* @@ -69,7 +68,7 @@ const std::vector> analog_sources = * setFFBEffectsCalc must be called in constructor of derived class to finish the setup */ FFBHIDMain::FFBHIDMain(uint8_t axisCount) : - Thread("FFBMAIN", 256, 30),axisCount(axisCount),btn_chooser(button_sources),analog_chooser(analog_sources) + Thread("FFBMAIN", FFBHIDMAIN_THREAD_MEM, FFBHIDMAIN_THREAD_PRIO),axisCount(axisCount),btn_chooser(button_sources),analog_chooser(analog_sources) { restoreFlash(); // Load parameters diff --git a/Firmware/FFBoard/UserExtensions/Src/MotorPWM.cpp b/Firmware/FFBoard/UserExtensions/Src/MotorPWM.cpp index bb2b4619c..1677f572a 100644 --- a/Firmware/FFBoard/UserExtensions/Src/MotorPWM.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/MotorPWM.cpp @@ -156,6 +156,13 @@ void MotorPWM::setPwmSpeed(SpeedPWM_DRV spd){ this->pwmspeed = spd; tFreq = (float)(timerConfig.timerFreq/1000000)/(float)(prescaler+1); +#ifdef HW_ESP32SX + glue_ledc_config((ledc_channel_t)timerConfig.channel_1, LEDC_TIMER_11_BIT, timerConfig.timerFreq/(prescaler+1)/period); + glue_ledc_config((ledc_channel_t)timerConfig.channel_2, LEDC_TIMER_11_BIT, timerConfig.timerFreq/(prescaler+1)/period); + glue_ledc_config((ledc_channel_t)timerConfig.channel_3, LEDC_TIMER_11_BIT, timerConfig.timerFreq/(prescaler+1)/period); + glue_ledc_config((ledc_channel_t)timerConfig.channel_4, LEDC_TIMER_11_BIT, timerConfig.timerFreq/(prescaler+1)/period); + period = 1UL << LEDC_TIMER_11_BIT; +#else pwmInitTimer(timerConfig.timer, timerConfig.channel_1,period,prescaler); pwmInitTimer(timerConfig.timer, timerConfig.channel_2,period,prescaler); pwmInitTimer(timerConfig.timer, timerConfig.channel_3,period,prescaler); @@ -164,6 +171,7 @@ void MotorPWM::setPwmSpeed(SpeedPWM_DRV spd){ // setPWM_HAL(0, timer, channel_1, period); // pwmInitTimer(timer, channel_2,period,prescaler); // setPWM_HAL(0, timer, channel_2, period); +#endif turn(0); } } @@ -172,6 +180,17 @@ void MotorPWM::setPwmSpeed(SpeedPWM_DRV spd){ * Updates pwm pulse length */ void MotorPWM::setPWM(uint32_t value,uint8_t ccr){ +#ifdef HW_ESP32SX + if(ccr == 1){ + glue_ledc_set_duty(LEDC_CHANNEL_0, value); // Set next CCR for channel 1 + }else if(ccr == 2){ + glue_ledc_set_duty(LEDC_CHANNEL_1, value); // Set next CCR for channel 2 + }else if(ccr == 3){ + glue_ledc_set_duty(LEDC_CHANNEL_2, value); // Set next CCR for channel 3 + }else if(ccr == 4){ + glue_ledc_set_duty(LEDC_CHANNEL_3, value); // Set next CCR for channel 4 + } +#else if(ccr == 1){ timerConfig.timer->Instance->CCR1 = value; // Set next CCR for channel 1 }else if(ccr == 2){ @@ -181,7 +200,7 @@ void MotorPWM::setPWM(uint32_t value,uint8_t ccr){ }else if(ccr == 4){ timerConfig.timer->Instance->CCR4 = value; // Set next CCR for channel 4 } - +#endif } SpeedPWM_DRV MotorPWM::getPwmSpeed(){ @@ -277,7 +296,7 @@ CommandStatus MotorPWM::command(const ParsedCommand& cmd,std::vectorInstance->ARR = period; @@ -296,7 +315,6 @@ void pwmInitTimer(TIM_HandleTypeDef* timer,uint32_t channel,uint32_t period,uint //setPWM_HAL(0,timer,channel,period); HAL_TIM_PWM_Start(timer, channel); pwmTimMutex.Unlock(); - } @@ -314,4 +332,6 @@ void setPWM_HAL(uint32_t value,TIM_HandleTypeDef* timer,uint32_t channel,uint32_ HAL_TIM_PWM_ConfigChannel(timer, &sConfigOC, channel); HAL_TIM_PWM_Start(timer, channel); } +#endif // end of HW_ESP32SX + #endif diff --git a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp index 20ce36135..581daa699 100644 --- a/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/MtEncoderSPI.cpp @@ -18,7 +18,7 @@ const ClassIdentifier MtEncoderSPI::getInfo(){ return info; } -MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFreeCsPins()[0]), CommandHandler("mtenc",CLSID_ENCODER_MTSPI,0),cpp_freertos::Thread("MTENC",256,42) { +MtEncoderSPI::MtEncoderSPI() : SPIDevice(ENCODER_SPI_PORT,ENCODER_SPI_PORT.getFreeCsPins()[0]), CommandHandler("mtenc",CLSID_ENCODER_MTSPI,0),cpp_freertos::Thread("MTENC",MTENCODERSPI_THREAD_MEM,MTENCODERSPI_THREAD_PRIO) { MtEncoderSPI::inUse = true; this->spiConfig.peripheral.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4; // 4 = 10MHz 8 = 5MHz this->spiConfig.peripheral.FirstBit = SPI_FIRSTBIT_MSB; diff --git a/Firmware/FFBoard/UserExtensions/Src/PCF8574.cpp b/Firmware/FFBoard/UserExtensions/Src/PCF8574.cpp index fda28269f..5c375b695 100644 --- a/Firmware/FFBoard/UserExtensions/Src/PCF8574.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/PCF8574.cpp @@ -68,7 +68,7 @@ const ClassIdentifier PCF8574Buttons::getInfo(){ } -PCF8574Buttons::PCF8574Buttons() : PCF8574(i2cport) , CommandHandler("pcfbtn", CLSID_BTN_PCF, 0), Thread("pcfbtn", 64, 20) { +PCF8574Buttons::PCF8574Buttons() : PCF8574(i2cport) , CommandHandler("pcfbtn", CLSID_BTN_PCF, 0), Thread("pcfbtn", PCF8574BUTTONS_THREAD_MEM, PCF8574BUTTONS_THREAD_PRIO) { CommandHandler::registerCommands(); ButtonSource::btnnum=8; registerCommand("btnnum", PCF8574Buttons_commands::btnnum, "Amount of buttons",CMDFLAG_GET | CMDFLAG_SET); diff --git a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp index 2badfe365..c45214401 100644 --- a/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/TMC4671.cpp @@ -9,7 +9,6 @@ #ifdef TMC4671DRIVER #include "ledEffects.h" #include "voltagesense.h" -#include "stm32f4xx_hal_spi.h" #include #include #include "ErrorHandler.h" @@ -2997,7 +2996,7 @@ void TMC4671::setUpExtEncTimer(){ /** * Medium priority task to update external encoders */ -TMC4671::TMC_ExternalEncoderUpdateThread::TMC_ExternalEncoderUpdateThread(TMC4671* tmc) : cpp_freertos::Thread("TMCENC",80,33),tmc(tmc){ +TMC4671::TMC_ExternalEncoderUpdateThread::TMC_ExternalEncoderUpdateThread(TMC4671* tmc) : cpp_freertos::Thread("TMCENC",TMCENC_THREAD_MEM,TMCENC_THREAD_PRIO),tmc(tmc){ this->Start(); } diff --git a/Firmware/FFBoard/UserExtensions/Src/VescCAN.cpp b/Firmware/FFBoard/UserExtensions/Src/VescCAN.cpp index 4cb9e15f4..2dd4ca7fb 100644 --- a/Firmware/FFBoard/UserExtensions/Src/VescCAN.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/VescCAN.cpp @@ -558,6 +558,15 @@ void VescCAN::decode_buffer(uint8_t *buffer, uint8_t len) { } +#ifdef HW_ESP32SX +extern "C" void glue_can_receive_msg(CAN_HandleTypeDef *hcan, uint8_t *rxBuf, CAN_RxHeaderTypeDef *rxHeader) +{ + for(CanHandler* c : CanHandler::canHandlers){ + c->canRxPendCallback(hcan,rxBuf,rxHeader,CAN_RX_FIFO0); + } +} +#endif + /* * Process the received message on the CAN bus. * Message have several struct, depends on message type, check the comm_can.c and diff --git a/Firmware/FFBoard/UserExtensions/Src/usb_descriptors.cpp b/Firmware/FFBoard/UserExtensions/Src/usb_descriptors.cpp index df7cb284d..c7046cd05 100644 --- a/Firmware/FFBoard/UserExtensions/Src/usb_descriptors.cpp +++ b/Firmware/FFBoard/UserExtensions/Src/usb_descriptors.cpp @@ -6,7 +6,6 @@ */ #include "tusb.h" #include "usb_descriptors.h" -#include "usbd.h" #include "stm32f4xx_hal.h" #include "main.h" #include "usb_hid_ffb_desc.h" diff --git a/Firmware/Targets/ESP32SX/.gitignore b/Firmware/Targets/ESP32SX/.gitignore new file mode 100644 index 000000000..76831bdb2 --- /dev/null +++ b/Firmware/Targets/ESP32SX/.gitignore @@ -0,0 +1,10 @@ +/*Debug/ +**/Release/ +**/.settings/ +*.launch +.vscode +.metadata +.mxproject +/build/ +sdkconfig.old +sdkconfig diff --git a/Firmware/Targets/ESP32SX/CMakeLists.txt b/Firmware/Targets/ESP32SX/CMakeLists.txt new file mode 100644 index 000000000..9d0c21325 --- /dev/null +++ b/Firmware/Targets/ESP32SX/CMakeLists.txt @@ -0,0 +1,6 @@ +# The following five lines of boilerplate have to be in your project's +# CMakeLists in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.5) +add_compile_options(-fdiagnostics-color=always) +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +project(openffboard) diff --git a/Firmware/Targets/ESP32SX/README.md b/Firmware/Targets/ESP32SX/README.md new file mode 100644 index 000000000..d6396c068 --- /dev/null +++ b/Firmware/Targets/ESP32SX/README.md @@ -0,0 +1,95 @@ +## OpenFFBoard for ESP32S2/ESP32S3 + +ESP32-S serial chips allows all of us to DIY at a lower cost. You can get the same fantastic experience as STM32 in some functions. See this [PR](https://github.com/Ultrawipf/OpenFFBoard/pull/46) for supported features and progress. + +| Supported Targets | Status | +| ----------------- | ------------------- | +| ESP32-S2 | Basically OK | +| ESP32-S3 | Testing is required | + +### Hardware + +Any ESP board that supports USB is OK, but you need enough experience to handle the connection with peripherals. + +[openffboard-esp32](https://github.com/TDA-2030/OpenFFBoard/tree/openffb/esp32s2%2Bvesc/openffboard-esp32s2) is a simple board for ESP32S2 and ESp32S3. It is designed with [KiCad](https://www.kicad.org/) and realizes most of the functions of OpenFFBoard. You may start your journey from this board. + +#### Pin Assignment + +| OpenFFBoard signals | ESP32S2 PINs | ESP32S3 PINs | +| ------------------- | ------------ | ------------ | +| Switch | IO0 | IO0 | +| AIN0 | IO1 | IO1 | +| AIN1 | IO2 | IO2 | +| AIN2 | IO3 | IO3 | +| ENCODER_A | IO4 | IO4 | +| PWM1 | IO5 | IO5 | +| ENCODER_B | IO6 | IO6 | +| PWM2 | IO7 | IO7 | +| DIN3 | IO8 | IO8 | +| DIN2 | IO9 | IO9 | +| DIN1 | IO10 | IO10 | +| DIN0 | IO11 | IO11 | +| GPIO_DRV | IO12 | IO12 | +| DRV_ENABLE | IO13 | IO13 | +| DRV_FLAG | IO14 | IO14 | +| ENCODER_Z | IO15 | IO15 | +| PWM3 | IO16 | IO16 | +| PWM4 | IO17 | IO17 | +| USB_D- | IO19 | IO19 | +| USB_D+ | IO20 | IO20 | +| BRAKE_CTRL | IO21 | IO21 | +| SPI1_CS2 | IO33 | IO47 | +| SPI1_CS3 | IO34 | IO48 | +| CAN_RX | IO35 | IO35 | +| CAN_TX | IO36 | IO36 | +| CAN_S | IO37 | IO37 | +| SPI1_MISO | IO38 | IO38 | +| SPI1_MOSI | IO39 | IO39 | +| LED_ERR | IO40 | IO40 | +| LED_CLIP | IO41 | IO41 | +| LED_SYS | IO42 | IO42 | +| SPI1_CS1 | IO45 | IO45 | +| SPI1_SCK | IO46 | IO46 | + + + +### Quick Download Firmware + +`Firmware/Targets/ESP32SX/firmware` contains a set of compiled firmware. You can write it from 0x00 address to esp32s2/esp32s3. + +1. First connect IO0 to GND, and then connect USB to the computer + +2. Download [flash_download_tool](https://www.espressif.com/en/support/download/other-tools) from Espressif + +3. Run the `flash_download_tool` + + ![download_1](https://s1.328888.xyz/2022/04/29/AHV7v.png) + + ![download_2](https://s1.328888.xyz/2022/04/29/AHF2i.png) + + Complete the configuration as shown in the figure, and finally click the `START` button. Please select the correct COM port before downloading. + +### How to Build + +1. Checkout esp-idf to commit: `c29343eb94d` + +``` bash +cd esp-idf +git fetch +git checkout c29343eb94d +git submodule update --init --recursive +``` + +2. Build the project and flash it to the board, then run monitor tool to view serial output: + +``` bash +cd /OpenFFBoard/Firmware/Targets/ESP32SX +idf.py set-target esp32s2 +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +(To exit the serial monitor, type ``Ctrl-]``.) + +See the Getting Started Guide for full steps to configure and use ESP-IDF to build projects. \ No newline at end of file diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/CMakeLists.txt b/Firmware/Targets/ESP32SX/components/tinyusb/CMakeLists.txt new file mode 100644 index 000000000..f30a66490 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/CMakeLists.txt @@ -0,0 +1,84 @@ +idf_build_get_property(target IDF_TARGET) + +set(srcs) +set(includes_public) +set(includes_private) +set(compile_options) + +if(CONFIG_TINYUSB) + if(target STREQUAL "esp32s3") + set(tusb_mcu "OPT_MCU_ESP32S3") + set(tusb_family "esp32sx") + elseif(target STREQUAL "esp32s2") + set(tusb_mcu "OPT_MCU_ESP32S2") + set(tusb_family "esp32sx") + else() + # CONFIG_TINYUSB dependency has been garanteed by Kconfig logic, + # So it's not possible that cmake goes here + message(FATAL_ERROR "TinyUSB is not support on ${target}.") + return() + endif() + + list(APPEND compile_options + "-DCFG_TUSB_MCU=${tusb_mcu}" + "-DCFG_TUSB_DEBUG=${CONFIG_TINYUSB_DEBUG_LEVEL}" + ) + + idf_component_get_property(freertos_component_dir freertos COMPONENT_DIR) + + list(APPEND includes_private + # "tinyusb/hw/bsp/" + "tinyusb/src/" + "tinyusb/src/device" + # "additions/include_private" + ) + + list(APPEND includes_public + "tinyusb/src/" + "additions/include" + "tinyusb/src/class/cdc" + "tinyusb/src/class/hid" + "tinyusb/src/class/midi" + # The FreeRTOS API include convention in tinyusb is different from esp-idf + "${freertos_component_dir}/include/freertos" + ) + + list(APPEND srcs + "tinyusb/src/portable/espressif/${tusb_family}/dcd_${tusb_family}.c" + "tinyusb/src/class/cdc/cdc_device.c" + "tinyusb/src/class/hid/hid_device.c" + "tinyusb/src/class/midi/midi_device.c" + "tinyusb/src/class/msc/msc_device.c" + "tinyusb/src/class/vendor/vendor_device.c" + "tinyusb/src/common/tusb_fifo.c" + "tinyusb/src/device/usbd_control.c" + "tinyusb/src/device/usbd.c" + "tinyusb/src/tusb.c" + # "additions/src/descriptors_control.c" + # "additions/src/tinyusb.c" + # "additions/src/tusb_tasks.c" + # "additions/src/usb_descriptors.c" + ) + + # when no builtin class driver is enabled, an uint8_t data compared with `BUILTIN_DRIVER_COUNT` will always be false + set_source_files_properties("tinyusb/src/device/usbd.c" PROPERTIES COMPILE_FLAGS "-Wno-type-limits") + + if(CONFIG_TINYUSB_CDC_ENABLED) + list(APPEND srcs + # "additions/src/cdc.c" + # "additions/src/tusb_cdc_acm.c" + # "additions/src/tusb_console.c" + # "additions/src/vfs_tinyusb.c" + ) + endif() # CONFIG_TINYUSB_CDC_ENABLED +endif() # CONFIG_TINYUSB + +idf_component_register(SRCS ${srcs} + INCLUDE_DIRS ${includes_public} + PRIV_INCLUDE_DIRS ${includes_private} + PRIV_REQUIRES "vfs" "usb" + ) + +if(CONFIG_TINYUSB) + target_compile_options(${COMPONENT_LIB} PRIVATE ${compile_options}) +endif() diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/Kconfig b/Firmware/Targets/ESP32SX/components/tinyusb/Kconfig new file mode 100644 index 000000000..192f7741f --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/Kconfig @@ -0,0 +1,192 @@ +menu "TinyUSB Stack" + visible if USB_OTG_SUPPORTED + + config TINYUSB + bool "Use TinyUSB Stack" + depends on USB_OTG_SUPPORTED + default n + help + Enable TinyUSB stack support. + Note that, esp-idf only uses the device stack provided by TinyUSB. + + if TINYUSB + config TINYUSB_DEBUG_LEVEL + int "TinyUSB log level (0-3)" + default 0 + range 0 3 + help + Specify verbosity of TinyUSB log output. + + menu "TinyUSB task configuration" + config TINYUSB_NO_DEFAULT_TASK + bool "Do not create a TinyUSB task" + default n + help + This option allows to not create the FreeRTOS task during the driver initialization. + User will have to handle TinyUSB events manually. + + config TINYUSB_TASK_PRIORITY + int "TinyUSB task priority" + default 5 + depends on !TINYUSB_NO_DEFAULT_TASK + help + Set the priority of the default TinyUSB main task. + + config TINYUSB_TASK_STACK_SIZE + int "TinyUSB task stack size (bytes)" + default 4096 + depends on !TINYUSB_NO_DEFAULT_TASK + help + Set the stack size of the default TinyUSB main task. + endmenu + + menu "Descriptor configuration" + config TINYUSB_DESC_USE_ESPRESSIF_VID + bool "VID: Use Espressif's vendor ID" + default y + help + Enable this option, USB device will use Espressif's vendor ID as its VID. + This is helpful at product develop stage. + + config TINYUSB_DESC_CUSTOM_VID + hex "VID: Custom vendor ID" + default 0x1234 + depends on !TINYUSB_DESC_USE_ESPRESSIF_VID + help + Custom Vendor ID. + + config TINYUSB_DESC_USE_DEFAULT_PID + bool "PID: Use a default PID assigned to TinyUSB" + default y + help + Default TinyUSB PID assigning uses values 0x4000...0x4007. + + config TINYUSB_DESC_CUSTOM_PID + hex "PID: Custom product ID" + default 0x5678 + depends on !TINYUSB_DESC_USE_DEFAULT_PID + help + Custom Product ID. + + config TINYUSB_DESC_BCD_DEVICE + hex "bcdDevice" + default 0x0100 + help + Version of the firmware of the USB device. + + config TINYUSB_DESC_MANUFACTURER_STRING + string "Manufacturer name" + default "Espressif Systems" + help + Name of the manufacturer of the USB device. + + config TINYUSB_DESC_PRODUCT_STRING + string "Product name" + default "Espressif Device" + help + Name of the USB device. + + config TINYUSB_DESC_SERIAL_STRING + string "Serial string" + default "123456" + help + Serial number of the USB device. + + config TINYUSB_DESC_CDC_STRING + depends on TINYUSB_CDC_ENABLED + string "CDC Device String" + default "Espressif CDC Device" + help + Name of the CDC device. + + config TINYUSB_DESC_MSC_STRING + depends on TINYUSB_MSC_ENABLED + string "MSC Device String" + default "Espressif MSC Device" + help + Name of the MSC device. + + config TINYUSB_DESC_HID_STRING + depends on TINYUSB_HID_ENABLED + string "HID Device String" + default "Espressif HID Device" + help + Name of the HID device + endmenu # "Descriptor configuration" + + menu "Massive Storage Class (MSC)" + config TINYUSB_MSC_ENABLED + bool "Enable TinyUSB MSC feature" + default n + help + Enable TinyUSB MSC feature. + + config TINYUSB_MSC_BUFSIZE + depends on TINYUSB_MSC_ENABLED + int "MSC FIFO size" + default 512 + help + MSC FIFO size, in bytes. + endmenu # "Massive Storage Class" + + menu "HID configuration" + config TINYUSB_HID_ENABLED + bool "Enable TinyUSB HID feature" + default n + help + Enable TinyUSB HID feature. + + config TINYUSB_HID_BUFSIZE + depends on TINYUSB_HID_ENABLED + int "HID Buffer size" + default 64 + help + in bytes + endmenu # "HID configuration" + + menu "Communication Device Class (CDC)" + config TINYUSB_CDC_ENABLED + bool "Enable TinyUSB CDC feature" + default n + help + Enable TinyUSB CDC feature. + + config TINYUSB_CDC_RX_BUFSIZE + depends on TINYUSB_CDC_ENABLED + int "CDC FIFO size of RX channel" + default 64 + help + CDC FIFO size of RX channel. + + config TINYUSB_CDC_TX_BUFSIZE + depends on TINYUSB_CDC_ENABLED + int "CDC FIFO size of TX channel" + default 64 + help + CDC FIFO size of TX channel. + endmenu # "Communication Device Class" + + menu "USB MIDI" + config TINYUSB_MIDI_ENABLED + bool "Enable TinyUSB MIDI feature" + default n + help + Enable TinyUSB MIDI feature. + + config TINYUSB_MIDI_RX_BUFSIZE + depends on TINYUSB_MIDI_ENABLED + int "MIDI FIFO size of RX channel" + default 64 + help + MIDI FIFO size of RX channel. + + config TINYUSB_MIDI_TX_BUFSIZE + depends on TINYUSB_MIDI_ENABLED + int "MIDI FIFO size of TX channel" + default 64 + help + MIDI FIFO size of TX channel. + endmenu + endif # TINYUSB + +endmenu # "TinyUSB Stack" diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/additions/include/tinyusb.h b/Firmware/Targets/ESP32SX/components/tinyusb/additions/include/tinyusb.h new file mode 100644 index 000000000..aaa48fa72 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/additions/include/tinyusb.h @@ -0,0 +1,102 @@ +// Copyright 2020 Espressif Systems (Shanghai) PTE LTD +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include "tusb.h" +#include "tusb_option.h" +#include "tusb_config.h" +#include "tinyusb_types.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +/* tinyusb uses buffers with type of uint8_t[] but in our driver we are reading them as a 32-bit word */ +#if (CFG_TUD_ENDPOINT0_SIZE < 4) +# define CFG_TUD_ENDPOINT0_SIZE 4 +# warning "CFG_TUD_ENDPOINT0_SIZE was too low and was set to 4" +#endif + +#if TUSB_OPT_DEVICE_ENABLED + +# if CFG_TUD_HID +# if (CFG_TUD_HID_BUFSIZE < 4) +# define CFG_TUD_HID_BUFSIZE 4 +# warning "CFG_TUD_HID_BUFSIZE was too low and was set to 4" +# endif +# endif + +# if CFG_TUD_CDC +# if (CFG_TUD_CDC_EP_BUFSIZE < 4) +# define CFG_TUD_CDC_EP_BUFSIZE 4 +# warning "CFG_TUD_CDC_EP_BUFSIZE was too low and was set to 4" +# endif +# endif + +# if CFG_TUD_MSC +# if (CFG_TUD_MSC_BUFSIZE < 4) +# define CFG_TUD_MSC_BUFSIZE 4 +# warning "CFG_TUD_MSC_BUFSIZE was too low and was set to 4" +# endif +# endif + +# if CFG_TUD_MIDI +# if (CFG_TUD_MIDI_EPSIZE < 4) +# define CFG_TUD_MIDI_EPSIZE 4 +# warning "CFG_TUD_MIDI_EPSIZE was too low and was set to 4" +# endif +# endif + +# if CFG_TUD_CUSTOM_CLASS +# warning "Please check that the buffer is more then 4 bytes" +# endif +#endif + +/** + * @brief Configuration structure of the tinyUSB core + */ +typedef struct { + tusb_desc_device_t *descriptor; /*!< Pointer to a device descriptor */ + const char **string_descriptor; /*!< Pointer to an array of string descriptors */ + bool external_phy; /*!< Should USB use an external PHY */ +} tinyusb_config_t; + +/** + * @brief This is an all-in-one helper function, including: + * 1. USB device driver initialization + * 2. Descriptors preparation + * 3. TinyUSB stack initialization + * 4. Creates and start a task to handle usb events + * + * @note Don't change Custom descriptor, but if it has to be done, + * Suggest to define as follows in order to match the Interface Association Descriptor (IAD): + * bDeviceClass = TUSB_CLASS_MISC, + * bDeviceSubClass = MISC_SUBCLASS_COMMON, + * + * @param config tinyusb stack specific configuration + * @retval ESP_ERR_INVALID_ARG Install driver and tinyusb stack failed because of invalid argument + * @retval ESP_FAIL Install driver and tinyusb stack failed because of internal error + * @retval ESP_OK Install driver and tinyusb stack successfully + */ +esp_err_t tinyusb_driver_install(const tinyusb_config_t *config); + +// TODO esp_err_t tinyusb_driver_uninstall(void); (IDF-1474) + +#ifdef __cplusplus +} +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/additions/include/tusb_config.h b/Firmware/Targets/ESP32SX/components/tinyusb/additions/include/tusb_config.h new file mode 100644 index 000000000..bb035a1e3 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/additions/include/tusb_config.h @@ -0,0 +1,101 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org), + * Additions Copyright (c) 2020, Espressif Systems (Shanghai) PTE LTD + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + */ + +#pragma once + +#include "tusb_option.h" +#include "sdkconfig.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef CONFIG_TINYUSB_CDC_ENABLED +# define CONFIG_TINYUSB_CDC_ENABLED 0 +#endif + +#ifndef CONFIG_TINYUSB_MSC_ENABLED +# define CONFIG_TINYUSB_MSC_ENABLED 0 +#endif + +#ifndef CONFIG_TINYUSB_HID_ENABLED +# define CONFIG_TINYUSB_HID_ENABLED 0 +#endif + +#ifndef CONFIG_TINYUSB_MIDI_ENABLED +# define CONFIG_TINYUSB_MIDI_ENABLED 0 +#endif + +#ifndef CONFIG_TINYUSB_CUSTOM_CLASS_ENABLED +# define CONFIG_TINYUSB_CUSTOM_CLASS_ENABLED 0 +#endif + +#define CFG_TUSB_RHPORT0_MODE OPT_MODE_DEVICE | OPT_MODE_FULL_SPEED +#define CFG_TUSB_OS OPT_OS_FREERTOS + +/* USB DMA on some MCUs can only access a specific SRAM region with restriction on alignment. + * Tinyusb use follows macros to declare transferring memory so that they can be put + * into those specific section. + * e.g + * - CFG_TUSB_MEM SECTION : __attribute__ (( section(".usb_ram") )) + * - CFG_TUSB_MEM_ALIGN : __attribute__ ((aligned(4))) + */ +#ifndef CFG_TUSB_MEM_SECTION +# define CFG_TUSB_MEM_SECTION +#endif + +#ifndef CFG_TUSB_MEM_ALIGN +# define CFG_TUSB_MEM_ALIGN TU_ATTR_ALIGNED(4) +#endif + +#ifndef CFG_TUD_ENDPOINT0_SIZE +#define CFG_TUD_ENDPOINT0_SIZE 64 +#endif + +// CDC FIFO size of TX and RX +#define CFG_TUD_CDC_RX_BUFSIZE CONFIG_TINYUSB_CDC_RX_BUFSIZE +#define CFG_TUD_CDC_TX_BUFSIZE CONFIG_TINYUSB_CDC_TX_BUFSIZE + +// MSC Buffer size of Device Mass storage +#define CFG_TUD_MSC_BUFSIZE CONFIG_TINYUSB_MSC_BUFSIZE + +// MIDI FIFO size of TX and RX +#define CFG_TUD_MIDI_RX_BUFSIZE CONFIG_TINYUSB_MIDI_RX_BUFSIZE +#define CFG_TUD_MIDI_TX_BUFSIZE CONFIG_TINYUSB_MIDI_TX_BUFSIZE + +// HID buffer size Should be sufficient to hold ID (if any) + Data +#define CFG_TUD_HID_BUFSIZE CONFIG_TINYUSB_HID_BUFSIZE + +// Enabled device class driver +#define CFG_TUD_CDC CONFIG_TINYUSB_CDC_ENABLED +#define CFG_TUD_MSC CONFIG_TINYUSB_MSC_ENABLED +#define CFG_TUD_HID CONFIG_TINYUSB_HID_ENABLED +#define CFG_TUD_MIDI CONFIG_TINYUSB_MIDI_ENABLED +#define CFG_TUD_CUSTOM_CLASS CONFIG_TINYUSB_CUSTOM_CLASS_ENABLED + +#ifdef __cplusplus +} +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/sdkconfig.rename b/Firmware/Targets/ESP32SX/components/tinyusb/sdkconfig.rename new file mode 100644 index 000000000..6aea8579a --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/sdkconfig.rename @@ -0,0 +1,22 @@ +# sdkconfig replacement configurations for deprecated options formatted as +# CONFIG_DEPRECATED_OPTION CONFIG_NEW_OPTION +CONFIG_USB_ENABLED CONFIG_TINYUSB +CONFIG_USB_DO_NOT_CREATE_TASK CONFIG_TINYUSB_NO_DEFAULT_TASK +CONFIG_USB_TASK_PRIORITY CONFIG_TINYUSB_TASK_PRIORITY +CONFIG_USB_DESC_USE_ESPRESSIF_VID CONFIG_TINYUSB_DESC_USE_ESPRESSIF_VID +CONFIG_USB_DESC_CUSTOM_VID CONFIG_TINYUSB_DESC_CUSTOM_VID +CONFIG_USB_DESC_USE_DEFAULT_PID CONFIG_TINYUSB_DESC_USE_DEFAULT_PID +CONFIG_USB_DESC_CUSTOM_PID CONFIG_TINYUSB_DESC_CUSTOM_PID +CONFIG_USB_DESC_BCDDEVICE CONFIG_TINYUSB_DESC_BCD_DEVICE +CONFIG_USB_DESC_MANUFACTURER_STRING CONFIG_TINYUSB_DESC_MANUFACTURER_STRING +CONFIG_USB_DESC_PRODUCT_STRING CONFIG_TINYUSB_DESC_PRODUCT_STRING +CONFIG_USB_DESC_SERIAL_STRING CONFIG_TINYUSB_DESC_SERIAL_STRING +CONFIG_USB_DESC_CDC_STRING CONFIG_TINYUSB_DESC_CDC_STRING +CONFIG_USB_DESC_MSC_STRING CONFIG_TINYUSB_DESC_MSC_STRING +CONFIG_USB_DESC_HID_STRING CONFIG_TINYUSB_DESC_HID_STRING +CONFIG_USB_MSC_ENABLED CONFIG_TINYUSB_MSC_ENABLED +CONFIG_USB_MSC_BUFSIZE CONFIG_TINYUSB_MSC_BUFSIZE +CONFIG_USB_CDC_ENABLED CONFIG_TINYUSB_CDC_ENABLED +CONFIG_USB_CDC_RX_BUFSIZE CONFIG_TINYUSB_CDC_RX_BUFSIZE +CONFIG_USB_CDC_TX_BUFSIZE CONFIG_TINYUSB_CDC_TX_BUFSIZE +CONFIG_USB_DEBUG_LEVEL CONFIG_TINYUSB_DEBUG_LEVEL diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/LICENSE b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/LICENSE new file mode 100644 index 000000000..ddd4ab410 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/LICENSE @@ -0,0 +1,21 @@ +The MIT License (MIT) + +Copyright (c) 2018, hathach (tinyusb.org) + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/README.rst b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/README.rst new file mode 100644 index 000000000..5b994c357 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/README.rst @@ -0,0 +1,146 @@ +.. figure:: docs/assets/logo.svg + :alt: TinyUSB + +|Build Status| |Documentation Status| |License| + +TinyUSB is an open-source cross-platform USB Host/Device stack for +embedded system, designed to be memory-safe with no dynamic allocation +and thread-safe with all interrupt events are deferred then handled in +the non-ISR task function. + +Please take a look at the online `documentation `__. + +.. figure:: docs/assets/stack.svg + :width: 500px + :alt: stackup + +:: + + . + ├── docs # Documentation + ├── examples # Sample with Makefile build support + ├── hw + │   ├── bsp # Supported boards source files + │   └── mcu # Low level mcu core & peripheral drivers + ├── lib # Sources from 3rd party such as freeRTOS, fatfs ... + ├── src # All sources files for TinyUSB stack itself. + ├── test # Unit tests for the stack + └── tools # Files used internally + +Supported MCUs +============== + +The stack supports the following MCUs: + +- **Allwinner:** F1C100s/F1C200s +- **Broadcom:** BCM2837, BCM2711 +- **Dialog:** DA1469x +- **Espressif:** ESP32-S2, ESP32-S3 +- **GigaDevice:** GD32VF103 +- **Infineon:** XMC4500 +- **MicroChip:** SAMD11, SAMD21, SAMD51, SAME5x, SAMG55, SAML21, SAML22, SAME7x +- **NordicSemi:** nRF52833, nRF52840, nRF5340 +- **Nuvoton:** NUC120, NUC121/NUC125, NUC126, NUC505 +- **NXP:** + + - iMX RT Series: RT1011, RT1015, RT1021, RT1052, RT1062, RT1064 + - Kinetis: KL25, K32L2 + - LPC Series: 11u, 13, 15, 17, 18, 40, 43, 51u, 54, 55 + +- **Raspberry Pi:** RP2040 +- **Renesas:** RX63N, RX65N, RX72N +- **Silabs:** EFM32GG +- **Sony:** CXD56 +- **ST:** STM32 series: F0, F1, F2, F3, F4, F7, H7, G4, L0, L1, L4, L4+ +- **TI:** MSP430, MSP432E4, TM4C123 +- **ValentyUSB:** eptri + +Here is the list of `Supported Devices`_ that can be used with provided examples. + +Device Stack +============ + +Supports multiple device configurations by dynamically changing USB descriptors, low power functions such like suspend, resume, and remote wakeup. The following device classes are supported: + +- Audio Class 2.0 (UAC2) +- Bluetooth Host Controller Interface (BTH HCI) +- Communication Device Class (CDC) +- Device Firmware Update (DFU): DFU mode (WIP) and Runtime +- Human Interface Device (HID): Generic (In & Out), Keyboard, Mouse, Gamepad etc ... +- Mass Storage Class (MSC): with multiple LUNs +- Musical Instrument Digital Interface (MIDI) +- Network with RNDIS, Ethernet Control Model (ECM), Network Control Model (NCM) +- Test and Measurement Class (USBTMC) +- Video class 1.5 (UVC): work in progress +- Vendor-specific class support with generic In & Out endpoints. Can be used with MS OS 2.0 compatible descriptor to load winUSB driver without INF file. +- `WebUSB `__ with vendor-specific class + +If you have a special requirement, `usbd_app_driver_get_cb()` can be used to write your own class driver without modifying the stack. Here is how the RPi team added their reset interface `raspberrypi/pico-sdk#197 `_ + +Host Stack +========== + +- Human Interface Device (HID): Keyboard, Mouse, Generic +- Mass Storage Class (MSC) +- Hub currently only supports 1 level of hub (due to my laziness) + +OS Abstraction layer +==================== + +TinyUSB is completely thread-safe by pushing all Interrupt Service Request (ISR) events into a central queue, then processing them later in the non-ISR context task function. It also uses semaphore/mutex to access shared resources such as Communication Device Class (CDC) FIFO. Therefore the stack needs to use some of the OS's basic APIs. Following OSes are already supported out of the box. + +- **No OS** +- **FreeRTOS** +- `RT-Thread `_: `repo `_ +- **Mynewt** Due to the newt package build system, Mynewt examples are better to be on its `own repo `_ + +Local Docs +========== + +- Info + + - `Uses`_ + - `Changelog`_ + - `Contributors`_ + +- `Reference`_ + + - `Supported Devices`_ + - `Getting Started`_ + - `Concurrency`_ + +- `Contributing`_ + + - `Code of Conduct`_ + - `Structure`_ + - `Porting`_ + +License +======= + +All TinyUSB sources in the ``src`` folder are licensed under MIT +license, the `Full license is here `__. However, each file can be +individually licensed especially those in ``lib`` and ``hw/mcu`` folder. +Please make sure you understand all the license term for files you use +in your project. + + +.. |Build Status| image:: https://github.com/hathach/tinyusb/workflows/Build/badge.svg + :target: https://github.com/hathach/tinyusb/actions +.. |Documentation Status| image:: https://readthedocs.org/projects/tinyusb/badge/?version=latest + :target: https://docs.tinyusb.org/en/latest/?badge=latest +.. |License| image:: https://img.shields.io/badge/license-MIT-brightgreen.svg + :target: https://opensource.org/licenses/MIT + + +.. _Uses: docs/info/uses.rst +.. _Changelog: docs/info/changelog.rst +.. _Contributors: CONTRIBUTORS.rst +.. _Reference: docs/reference/index.rst +.. _Supported Devices: docs/reference/supported.rst +.. _Getting Started: docs/reference/getting_started.rst +.. _Concurrency: docs/reference/concurrency.rst +.. _Contributing: docs/contributing/index.rst +.. _Code of Conduct: CODE_OF_CONDUCT.rst +.. _Structure: docs/contributing/structure.rst +.. _Porting: docs/contributing/porting.rst diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio.h new file mode 100644 index 000000000..6f9c1a6b5 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio.h @@ -0,0 +1,933 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * Copyright (c) 2020 Reinhard Panhuber + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup group_class + * \defgroup ClassDriver_Audio Audio + * Currently only MIDI subclass is supported + * @{ */ + +#ifndef _TUSB_AUDIO_H__ +#define _TUSB_AUDIO_H__ + +#include "common/tusb_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/// Audio Device Class Codes + +/// A.2 - Audio Function Subclass Codes +typedef enum +{ + AUDIO_FUNCTION_SUBCLASS_UNDEFINED = 0x00, +} audio_function_subclass_type_t; + +/// A.3 - Audio Function Protocol Codes +typedef enum +{ + AUDIO_FUNC_PROTOCOL_CODE_UNDEF = 0x00, + AUDIO_FUNC_PROTOCOL_CODE_V2 = 0x20, ///< Version 2.0 +} audio_function_protocol_code_t; + +/// A.5 - Audio Interface Subclass Codes +typedef enum +{ + AUDIO_SUBCLASS_UNDEFINED = 0x00, + AUDIO_SUBCLASS_CONTROL , ///< Audio Control + AUDIO_SUBCLASS_STREAMING , ///< Audio Streaming + AUDIO_SUBCLASS_MIDI_STREAMING , ///< MIDI Streaming +} audio_subclass_type_t; + +/// A.6 - Audio Interface Protocol Codes +typedef enum +{ + AUDIO_INT_PROTOCOL_CODE_UNDEF = 0x00, + AUDIO_INT_PROTOCOL_CODE_V2 = 0x20, ///< Version 2.0 +} audio_interface_protocol_code_t; + +/// A.7 - Audio Function Category Codes +typedef enum +{ + AUDIO_FUNC_UNDEF = 0x00, + AUDIO_FUNC_DESKTOP_SPEAKER = 0x01, + AUDIO_FUNC_HOME_THEATER = 0x02, + AUDIO_FUNC_MICROPHONE = 0x03, + AUDIO_FUNC_HEADSET = 0x04, + AUDIO_FUNC_TELEPHONE = 0x05, + AUDIO_FUNC_CONVERTER = 0x06, + AUDIO_FUNC_SOUND_RECODER = 0x07, + AUDIO_FUNC_IO_BOX = 0x08, + AUDIO_FUNC_MUSICAL_INSTRUMENT = 0x09, + AUDIO_FUNC_PRO_AUDIO = 0x0A, + AUDIO_FUNC_AUDIO_VIDEO = 0x0B, + AUDIO_FUNC_CONTROL_PANEL = 0x0C, + AUDIO_FUNC_OTHER = 0xFF, +} audio_function_code_t; + +/// A.9 - Audio Class-Specific AC Interface Descriptor Subtypes UAC2 +typedef enum +{ + AUDIO_CS_AC_INTERFACE_AC_DESCRIPTOR_UNDEF = 0x00, + AUDIO_CS_AC_INTERFACE_HEADER = 0x01, + AUDIO_CS_AC_INTERFACE_INPUT_TERMINAL = 0x02, + AUDIO_CS_AC_INTERFACE_OUTPUT_TERMINAL = 0x03, + AUDIO_CS_AC_INTERFACE_MIXER_UNIT = 0x04, + AUDIO_CS_AC_INTERFACE_SELECTOR_UNIT = 0x05, + AUDIO_CS_AC_INTERFACE_FEATURE_UNIT = 0x06, + AUDIO_CS_AC_INTERFACE_EFFECT_UNIT = 0x07, + AUDIO_CS_AC_INTERFACE_PROCESSING_UNIT = 0x08, + AUDIO_CS_AC_INTERFACE_EXTENSION_UNIT = 0x09, + AUDIO_CS_AC_INTERFACE_CLOCK_SOURCE = 0x0A, + AUDIO_CS_AC_INTERFACE_CLOCK_SELECTOR = 0x0B, + AUDIO_CS_AC_INTERFACE_CLOCK_MULTIPLIER = 0x0C, + AUDIO_CS_AC_INTERFACE_SAMPLE_RATE_CONVERTER = 0x0D, +} audio_cs_ac_interface_subtype_t; + +/// A.10 - Audio Class-Specific AS Interface Descriptor Subtypes UAC2 +typedef enum +{ + AUDIO_CS_AS_INTERFACE_AS_DESCRIPTOR_UNDEF = 0x00, + AUDIO_CS_AS_INTERFACE_AS_GENERAL = 0x01, + AUDIO_CS_AS_INTERFACE_FORMAT_TYPE = 0x02, + AUDIO_CS_AS_INTERFACE_ENCODER = 0x03, + AUDIO_CS_AS_INTERFACE_DECODER = 0x04, +} audio_cs_as_interface_subtype_t; + +/// A.11 - Effect Unit Effect Types +typedef enum +{ + AUDIO_EFFECT_TYPE_UNDEF = 0x00, + AUDIO_EFFECT_TYPE_PARAM_EQ_SECTION = 0x01, + AUDIO_EFFECT_TYPE_REVERBERATION = 0x02, + AUDIO_EFFECT_TYPE_MOD_DELAY = 0x03, + AUDIO_EFFECT_TYPE_DYN_RANGE_COMP = 0x04, +} audio_effect_unit_effect_type_t; + +/// A.12 - Processing Unit Process Types +typedef enum +{ + AUDIO_PROCESS_TYPE_UNDEF = 0x00, + AUDIO_PROCESS_TYPE_UP_DOWN_MIX = 0x01, + AUDIO_PROCESS_TYPE_DOLBY_PROLOGIC = 0x02, + AUDIO_PROCESS_TYPE_STEREO_EXTENDER = 0x03, +} audio_processing_unit_process_type_t; + +/// A.13 - Audio Class-Specific EP Descriptor Subtypes UAC2 +typedef enum +{ + AUDIO_CS_EP_SUBTYPE_UNDEF = 0x00, + AUDIO_CS_EP_SUBTYPE_GENERAL = 0x01, +} audio_cs_ep_subtype_t; + +/// A.14 - Audio Class-Specific Request Codes +typedef enum +{ + AUDIO_CS_REQ_UNDEF = 0x00, + AUDIO_CS_REQ_CUR = 0x01, + AUDIO_CS_REQ_RANGE = 0x02, + AUDIO_CS_REQ_MEM = 0x03, +} audio_cs_req_t; + +/// A.17 - Control Selector Codes + +/// A.17.1 - Clock Source Control Selectors +typedef enum +{ + AUDIO_CS_CTRL_UNDEF = 0x00, + AUDIO_CS_CTRL_SAM_FREQ = 0x01, + AUDIO_CS_CTRL_CLK_VALID = 0x02, +} audio_clock_src_control_selector_t; + +/// A.17.2 - Clock Selector Control Selectors +typedef enum +{ + AUDIO_CX_CTRL_UNDEF = 0x00, + AUDIO_CX_CTRL_CONTROL = 0x01, +} audio_clock_sel_control_selector_t; + +/// A.17.3 - Clock Multiplier Control Selectors +typedef enum +{ + AUDIO_CM_CTRL_UNDEF = 0x00, + AUDIO_CM_CTRL_NUMERATOR_CONTROL = 0x01, + AUDIO_CM_CTRL_DENOMINATOR_CONTROL = 0x02, +} audio_clock_mul_control_selector_t; + +/// A.17.4 - Terminal Control Selectors +typedef enum +{ + AUDIO_TE_CTRL_UNDEF = 0x00, + AUDIO_TE_CTRL_COPY_PROTECT = 0x01, + AUDIO_TE_CTRL_CONNECTOR = 0x02, + AUDIO_TE_CTRL_OVERLOAD = 0x03, + AUDIO_TE_CTRL_CLUSTER = 0x04, + AUDIO_TE_CTRL_UNDERFLOW = 0x05, + AUDIO_TE_CTRL_OVERFLOW = 0x06, + AUDIO_TE_CTRL_LATENCY = 0x07, +} audio_terminal_control_selector_t; + +/// A.17.5 - Mixer Control Selectors +typedef enum +{ + AUDIO_MU_CTRL_UNDEF = 0x00, + AUDIO_MU_CTRL_MIXER = 0x01, + AUDIO_MU_CTRL_CLUSTER = 0x02, + AUDIO_MU_CTRL_UNDERFLOW = 0x03, + AUDIO_MU_CTRL_OVERFLOW = 0x04, + AUDIO_MU_CTRL_LATENCY = 0x05, +} audio_mixer_control_selector_t; + +/// A.17.6 - Selector Control Selectors +typedef enum +{ + AUDIO_SU_CTRL_UNDEF = 0x00, + AUDIO_SU_CTRL_SELECTOR = 0x01, + AUDIO_SU_CTRL_LATENCY = 0x02, +} audio_sel_control_selector_t; + +/// A.17.7 - Feature Unit Control Selectors +typedef enum +{ + AUDIO_FU_CTRL_UNDEF = 0x00, + AUDIO_FU_CTRL_MUTE = 0x01, + AUDIO_FU_CTRL_VOLUME = 0x02, + AUDIO_FU_CTRL_BASS = 0x03, + AUDIO_FU_CTRL_MID = 0x04, + AUDIO_FU_CTRL_TREBLE = 0x05, + AUDIO_FU_CTRL_GRAPHIC_EQUALIZER = 0x06, + AUDIO_FU_CTRL_AGC = 0x07, + AUDIO_FU_CTRL_DELAY = 0x08, + AUDIO_FU_CTRL_BASS_BOOST = 0x09, + AUDIO_FU_CTRL_LOUDNESS = 0x0A, + AUDIO_FU_CTRL_INPUT_GAIN = 0x0B, + AUDIO_FU_CTRL_GAIN_PAD = 0x0C, + AUDIO_FU_CTRL_INVERTER = 0x0D, + AUDIO_FU_CTRL_UNDERFLOW = 0x0E, + AUDIO_FU_CTRL_OVERVLOW = 0x0F, + AUDIO_FU_CTRL_LATENCY = 0x10, +} audio_feature_unit_control_selector_t; + +/// A.17.8 Effect Unit Control Selectors + +/// A.17.8.1 Parametric Equalizer Section Effect Unit Control Selectors +typedef enum +{ + AUDIO_PE_CTRL_UNDEF = 0x00, + AUDIO_PE_CTRL_ENABLE = 0x01, + AUDIO_PE_CTRL_CENTERFREQ = 0x02, + AUDIO_PE_CTRL_QFACTOR = 0x03, + AUDIO_PE_CTRL_GAIN = 0x04, + AUDIO_PE_CTRL_UNDERFLOW = 0x05, + AUDIO_PE_CTRL_OVERFLOW = 0x06, + AUDIO_PE_CTRL_LATENCY = 0x07, +} audio_parametric_equalizer_control_selector_t; + +/// A.17.8.2 Reverberation Effect Unit Control Selectors +typedef enum +{ + AUDIO_RV_CTRL_UNDEF = 0x00, + AUDIO_RV_CTRL_ENABLE = 0x01, + AUDIO_RV_CTRL_TYPE = 0x02, + AUDIO_RV_CTRL_LEVEL = 0x03, + AUDIO_RV_CTRL_TIME = 0x04, + AUDIO_RV_CTRL_FEEDBACK = 0x05, + AUDIO_RV_CTRL_PREDELAY = 0x06, + AUDIO_RV_CTRL_DENSITY = 0x07, + AUDIO_RV_CTRL_HIFREQ_ROLLOFF = 0x08, + AUDIO_RV_CTRL_UNDERFLOW = 0x09, + AUDIO_RV_CTRL_OVERFLOW = 0x0A, + AUDIO_RV_CTRL_LATENCY = 0x0B, +} audio_reverberation_effect_control_selector_t; + +/// A.17.8.3 Modulation Delay Effect Unit Control Selectors +typedef enum +{ + AUDIO_MD_CTRL_UNDEF = 0x00, + AUDIO_MD_CTRL_ENABLE = 0x01, + AUDIO_MD_CTRL_BALANCE = 0x02, + AUDIO_MD_CTRL_RATE = 0x03, + AUDIO_MD_CTRL_DEPTH = 0x04, + AUDIO_MD_CTRL_TIME = 0x05, + AUDIO_MD_CTRL_FEEDBACK = 0x06, + AUDIO_MD_CTRL_UNDERFLOW = 0x07, + AUDIO_MD_CTRL_OVERFLOW = 0x08, + AUDIO_MD_CTRL_LATENCY = 0x09, +} audio_modulation_delay_control_selector_t; + +/// A.17.8.4 Dynamic Range Compressor Effect Unit Control Selectors +typedef enum +{ + AUDIO_DR_CTRL_UNDEF = 0x00, + AUDIO_DR_CTRL_ENABLE = 0x01, + AUDIO_DR_CTRL_COMPRESSION_RATE = 0x02, + AUDIO_DR_CTRL_MAXAMPL = 0x03, + AUDIO_DR_CTRL_THRESHOLD = 0x04, + AUDIO_DR_CTRL_ATTACK_TIME = 0x05, + AUDIO_DR_CTRL_RELEASE_TIME = 0x06, + AUDIO_DR_CTRL_UNDERFLOW = 0x07, + AUDIO_DR_CTRL_OVERFLOW = 0x08, + AUDIO_DR_CTRL_LATENCY = 0x09, +} audio_dynamic_range_compression_control_selector_t; + +/// A.17.9 Processing Unit Control Selectors + +/// A.17.9.1 Up/Down-mix Processing Unit Control Selectors +typedef enum +{ + AUDIO_UD_CTRL_UNDEF = 0x00, + AUDIO_UD_CTRL_ENABLE = 0x01, + AUDIO_UD_CTRL_MODE_SELECT = 0x02, + AUDIO_UD_CTRL_CLUSTER = 0x03, + AUDIO_UD_CTRL_UNDERFLOW = 0x04, + AUDIO_UD_CTRL_OVERFLOW = 0x05, + AUDIO_UD_CTRL_LATENCY = 0x06, +} audio_up_down_mix_control_selector_t; + +/// A.17.9.2 Dolby Prologic â„¢ Processing Unit Control Selectors +typedef enum +{ + AUDIO_DP_CTRL_UNDEF = 0x00, + AUDIO_DP_CTRL_ENABLE = 0x01, + AUDIO_DP_CTRL_MODE_SELECT = 0x02, + AUDIO_DP_CTRL_CLUSTER = 0x03, + AUDIO_DP_CTRL_UNDERFLOW = 0x04, + AUDIO_DP_CTRL_OVERFLOW = 0x05, + AUDIO_DP_CTRL_LATENCY = 0x06, +} audio_dolby_prologic_control_selector_t; + +/// A.17.9.3 Stereo Extender Processing Unit Control Selectors +typedef enum +{ + AUDIO_ST_EXT_CTRL_UNDEF = 0x00, + AUDIO_ST_EXT_CTRL_ENABLE = 0x01, + AUDIO_ST_EXT_CTRL_WIDTH = 0x02, + AUDIO_ST_EXT_CTRL_UNDERFLOW = 0x03, + AUDIO_ST_EXT_CTRL_OVERFLOW = 0x04, + AUDIO_ST_EXT_CTRL_LATENCY = 0x05, +} audio_stereo_extender_control_selector_t; + +/// A.17.10 Extension Unit Control Selectors +typedef enum +{ + AUDIO_XU_CTRL_UNDEF = 0x00, + AUDIO_XU_CTRL_ENABLE = 0x01, + AUDIO_XU_CTRL_CLUSTER = 0x02, + AUDIO_XU_CTRL_UNDERFLOW = 0x03, + AUDIO_XU_CTRL_OVERFLOW = 0x04, + AUDIO_XU_CTRL_LATENCY = 0x05, +} audio_extension_unit_control_selector_t; + +/// A.17.11 AudioStreaming Interface Control Selectors +typedef enum +{ + AUDIO_AS_CTRL_UNDEF = 0x00, + AUDIO_AS_CTRL_ACT_ALT_SETTING = 0x01, + AUDIO_AS_CTRL_VAL_ALT_SETTINGS = 0x02, + AUDIO_AS_CTRL_AUDIO_DATA_FORMAT = 0x03, +} audio_audiostreaming_interface_control_selector_t; + +/// A.17.12 Encoder Control Selectors +typedef enum +{ + AUDIO_EN_CTRL_UNDEF = 0x00, + AUDIO_EN_CTRL_BIT_RATE = 0x01, + AUDIO_EN_CTRL_QUALITY = 0x02, + AUDIO_EN_CTRL_VBR = 0x03, + AUDIO_EN_CTRL_TYPE = 0x04, + AUDIO_EN_CTRL_UNDERFLOW = 0x05, + AUDIO_EN_CTRL_OVERFLOW = 0x06, + AUDIO_EN_CTRL_ENCODER_ERROR = 0x07, + AUDIO_EN_CTRL_PARAM1 = 0x08, + AUDIO_EN_CTRL_PARAM2 = 0x09, + AUDIO_EN_CTRL_PARAM3 = 0x0A, + AUDIO_EN_CTRL_PARAM4 = 0x0B, + AUDIO_EN_CTRL_PARAM5 = 0x0C, + AUDIO_EN_CTRL_PARAM6 = 0x0D, + AUDIO_EN_CTRL_PARAM7 = 0x0E, + AUDIO_EN_CTRL_PARAM8 = 0x0F, +} audio_encoder_control_selector_t; + +/// A.17.13 Decoder Control Selectors + +/// A.17.13.1 MPEG Decoder Control Selectors +typedef enum +{ + AUDIO_MPD_CTRL_UNDEF = 0x00, + AUDIO_MPD_CTRL_DUAL_CHANNEL = 0x01, + AUDIO_MPD_CTRL_SECOND_STEREO = 0x02, + AUDIO_MPD_CTRL_MULTILINGUAL = 0x03, + AUDIO_MPD_CTRL_DYN_RANGE = 0x04, + AUDIO_MPD_CTRL_SCALING = 0x05, + AUDIO_MPD_CTRL_HILO_SCALING = 0x06, + AUDIO_MPD_CTRL_UNDERFLOW = 0x07, + AUDIO_MPD_CTRL_OVERFLOW = 0x08, + AUDIO_MPD_CTRL_DECODER_ERROR = 0x09, +} audio_MPEG_decoder_control_selector_t; + +/// A.17.13.2 AC-3 Decoder Control Selectors +typedef enum +{ + AUDIO_AD_CTRL_UNDEF = 0x00, + AUDIO_AD_CTRL_MODE = 0x01, + AUDIO_AD_CTRL_DYN_RANGE = 0x02, + AUDIO_AD_CTRL_SCALING = 0x03, + AUDIO_AD_CTRL_HILO_SCALING = 0x04, + AUDIO_AD_CTRL_UNDERFLOW = 0x05, + AUDIO_AD_CTRL_OVERFLOW = 0x06, + AUDIO_AD_CTRL_DECODER_ERROR = 0x07, +} audio_AC3_decoder_control_selector_t; + +/// A.17.13.3 WMA Decoder Control Selectors +typedef enum +{ + AUDIO_WD_CTRL_UNDEF = 0x00, + AUDIO_WD_CTRL_UNDERFLOW = 0x01, + AUDIO_WD_CTRL_OVERFLOW = 0x02, + AUDIO_WD_CTRL_DECODER_ERROR = 0x03, +} audio_WMA_decoder_control_selector_t; + +/// A.17.13.4 DTS Decoder Control Selectors +typedef enum +{ + AUDIO_DD_CTRL_UNDEF = 0x00, + AUDIO_DD_CTRL_UNDERFLOW = 0x01, + AUDIO_DD_CTRL_OVERFLOW = 0x02, + AUDIO_DD_CTRL_DECODER_ERROR = 0x03, +} audio_DTS_decoder_control_selector_t; + +/// A.17.14 Endpoint Control Selectors +typedef enum +{ + AUDIO_EP_CTRL_UNDEF = 0x00, + AUDIO_EP_CTRL_PITCH = 0x01, + AUDIO_EP_CTRL_DATA_OVERRUN = 0x02, + AUDIO_EP_CTRL_DATA_UNDERRUN = 0x03, +} audio_EP_control_selector_t; + +/// Terminal Types + +/// 2.1 - Audio Class-Terminal Types UAC2 +typedef enum +{ + AUDIO_TERM_TYPE_USB_UNDEFINED = 0x0100, + AUDIO_TERM_TYPE_USB_STREAMING = 0x0101, + AUDIO_TERM_TYPE_USB_VENDOR_SPEC = 0x01FF, +} audio_terminal_type_t; + +/// 2.2 - Audio Class-Input Terminal Types UAC2 +typedef enum +{ + AUDIO_TERM_TYPE_IN_UNDEFINED = 0x0200, + AUDIO_TERM_TYPE_IN_GENERIC_MIC = 0x0201, + AUDIO_TERM_TYPE_IN_DESKTOP_MIC = 0x0202, + AUDIO_TERM_TYPE_IN_PERSONAL_MIC = 0x0203, + AUDIO_TERM_TYPE_IN_OMNI_MIC = 0x0204, + AUDIO_TERM_TYPE_IN_ARRAY_MIC = 0x0205, + AUDIO_TERM_TYPE_IN_PROC_ARRAY_MIC = 0x0206, +} audio_terminal_input_type_t; + +/// 2.3 - Audio Class-Output Terminal Types UAC2 +typedef enum +{ + AUDIO_TERM_TYPE_OUT_UNDEFINED = 0x0300, + AUDIO_TERM_TYPE_OUT_GENERIC_SPEAKER = 0x0301, + AUDIO_TERM_TYPE_OUT_HEADPHONES = 0x0302, + AUDIO_TERM_TYPE_OUT_HEAD_MNT_DISP_AUIDO = 0x0303, + AUDIO_TERM_TYPE_OUT_DESKTOP_SPEAKER = 0x0304, + AUDIO_TERM_TYPE_OUT_ROOM_SPEAKER = 0x0305, + AUDIO_TERM_TYPE_OUT_COMMUNICATION_SPEAKER = 0x0306, + AUDIO_TERM_TYPE_OUT_LOW_FRQ_EFFECTS_SPEAKER = 0x0307, +} audio_terminal_output_type_t; + +/// Rest is yet to be implemented + +/// Additional Audio Device Class Codes - Source: Audio Data Formats + +/// A.1 - Audio Class-Format Type Codes UAC2 +typedef enum +{ + AUDIO_FORMAT_TYPE_UNDEFINED = 0x00, + AUDIO_FORMAT_TYPE_I = 0x01, + AUDIO_FORMAT_TYPE_II = 0x02, + AUDIO_FORMAT_TYPE_III = 0x03, + AUDIO_FORMAT_TYPE_IV = 0x04, + AUDIO_EXT_FORMAT_TYPE_I = 0x81, + AUDIO_EXT_FORMAT_TYPE_II = 0x82, + AUDIO_EXT_FORMAT_TYPE_III = 0x83, +} audio_format_type_t; + +// A.2.1 - Audio Class-Audio Data Format Type I UAC2 +typedef enum +{ + AUDIO_DATA_FORMAT_TYPE_I_PCM = (uint32_t) (1 << 0), + AUDIO_DATA_FORMAT_TYPE_I_PCM8 = (uint32_t) (1 << 1), + AUDIO_DATA_FORMAT_TYPE_I_IEEE_FLOAT = (uint32_t) (1 << 2), + AUDIO_DATA_FORMAT_TYPE_I_ALAW = (uint32_t) (1 << 3), + AUDIO_DATA_FORMAT_TYPE_I_MULAW = (uint32_t) (1 << 4), + AUDIO_DATA_FORMAT_TYPE_I_RAW_DATA = 0x80000000, +} audio_data_format_type_I_t; + +/// All remaining definitions are taken from the descriptor descriptions in the UAC2 main specification + +/// Audio Class-Control Values UAC2 +typedef enum +{ + AUDIO_CTRL_NONE = 0x00, ///< No Host access + AUDIO_CTRL_R = 0x01, ///< Host read access only + AUDIO_CTRL_RW = 0x03, ///< Host read write access +} audio_control_t; + +/// Audio Class-Specific AC Interface Descriptor Controls UAC2 +typedef enum +{ + AUDIO_CS_AS_INTERFACE_CTRL_LATENCY_POS = 0, +} audio_cs_ac_interface_control_pos_t; + +/// Audio Class-Specific AS Interface Descriptor Controls UAC2 +typedef enum +{ + AUDIO_CS_AS_INTERFACE_CTRL_ACTIVE_ALT_SET_POS = 0, + AUDIO_CS_AS_INTERFACE_CTRL_VALID_ALT_SET_POS = 2, +} audio_cs_as_interface_control_pos_t; + +/// Audio Class-Specific AS Isochronous Data EP Attributes UAC2 +typedef enum +{ + AUDIO_CS_AS_ISO_DATA_EP_ATT_MAX_PACKETS_ONLY = 0x80, + AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK = 0x00, +} audio_cs_as_iso_data_ep_attribute_t; + +/// Audio Class-Specific AS Isochronous Data EP Controls UAC2 +typedef enum +{ + AUDIO_CS_AS_ISO_DATA_EP_CTRL_PITCH_POS = 0, + AUDIO_CS_AS_ISO_DATA_EP_CTRL_DATA_OVERRUN_POS = 2, + AUDIO_CS_AS_ISO_DATA_EP_CTRL_DATA_UNDERRUN_POS = 4, +} audio_cs_as_iso_data_ep_control_pos_t; + +/// Audio Class-Specific AS Isochronous Data EP Lock Delay Units UAC2 +typedef enum +{ + AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED = 0x00, + AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_MILLISEC = 0x01, + AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_PCM_SAMPLES = 0x02, +} audio_cs_as_iso_data_ep_lock_delay_unit_t; + +/// Audio Class-Clock Source Attributes UAC2 +typedef enum +{ + AUDIO_CLOCK_SOURCE_ATT_EXT_CLK = 0x00, + AUDIO_CLOCK_SOURCE_ATT_INT_FIX_CLK = 0x01, + AUDIO_CLOCK_SOURCE_ATT_INT_VAR_CLK = 0x02, + AUDIO_CLOCK_SOURCE_ATT_INT_PRO_CLK = 0x03, + AUDIO_CLOCK_SOURCE_ATT_CLK_SYC_SOF = 0x04, +} audio_clock_source_attribute_t; + +/// Audio Class-Clock Source Controls UAC2 +typedef enum +{ + AUDIO_CLOCK_SOURCE_CTRL_CLK_FRQ_POS = 0, + AUDIO_CLOCK_SOURCE_CTRL_CLK_VAL_POS = 2, +} audio_clock_source_control_pos_t; + +/// Audio Class-Clock Selector Controls UAC2 +typedef enum +{ + AUDIO_CLOCK_SELECTOR_CTRL_POS = 0, +} audio_clock_selector_control_pos_t; + +/// Audio Class-Clock Multiplier Controls UAC2 +typedef enum +{ + AUDIO_CLOCK_MULTIPLIER_CTRL_NUMERATOR_POS = 0, + AUDIO_CLOCK_MULTIPLIER_CTRL_DENOMINATOR_POS = 2, +} audio_clock_multiplier_control_pos_t; + +/// Audio Class-Input Terminal Controls UAC2 +typedef enum +{ + AUDIO_IN_TERM_CTRL_CPY_PROT_POS = 0, + AUDIO_IN_TERM_CTRL_CONNECTOR_POS = 2, + AUDIO_IN_TERM_CTRL_OVERLOAD_POS = 4, + AUDIO_IN_TERM_CTRL_CLUSTER_POS = 6, + AUDIO_IN_TERM_CTRL_UNDERFLOW_POS = 8, + AUDIO_IN_TERM_CTRL_OVERFLOW_POS = 10, +} audio_terminal_input_control_pos_t; + +/// Audio Class-Output Terminal Controls UAC2 +typedef enum +{ + AUDIO_OUT_TERM_CTRL_CPY_PROT_POS = 0, + AUDIO_OUT_TERM_CTRL_CONNECTOR_POS = 2, + AUDIO_OUT_TERM_CTRL_OVERLOAD_POS = 4, + AUDIO_OUT_TERM_CTRL_UNDERFLOW_POS = 6, + AUDIO_OUT_TERM_CTRL_OVERFLOW_POS = 8, +} audio_terminal_output_control_pos_t; + +/// Audio Class-Feature Unit Controls UAC2 +typedef enum +{ + AUDIO_FEATURE_UNIT_CTRL_MUTE_POS = 0, + AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS = 2, + AUDIO_FEATURE_UNIT_CTRL_BASS_POS = 4, + AUDIO_FEATURE_UNIT_CTRL_MID_POS = 6, + AUDIO_FEATURE_UNIT_CTRL_TREBLE_POS = 8, + AUDIO_FEATURE_UNIT_CTRL_GRAPHIC_EQU_POS = 10, + AUDIO_FEATURE_UNIT_CTRL_AGC_POS = 12, + AUDIO_FEATURE_UNIT_CTRL_DELAY_POS = 14, + AUDIO_FEATURE_UNIT_CTRL_BASS_BOOST_POS = 16, + AUDIO_FEATURE_UNIT_CTRL_LOUDNESS_POS = 18, + AUDIO_FEATURE_UNIT_CTRL_INPUT_GAIN_POS = 20, + AUDIO_FEATURE_UNIT_CTRL_INPUT_GAIN_PAD_POS = 22, + AUDIO_FEATURE_UNIT_CTRL_PHASE_INV_POS = 24, + AUDIO_FEATURE_UNIT_CTRL_UNDERFLOW_POS = 26, + AUDIO_FEATURE_UNIT_CTRL_OVERFLOW_POS = 28, +} audio_feature_unit_control_pos_t; + +/// Audio Class-Audio Channel Configuration UAC2 +typedef enum +{ + AUDIO_CHANNEL_CONFIG_NON_PREDEFINED = 0x00000000, + AUDIO_CHANNEL_CONFIG_FRONT_LEFT = 0x00000001, + AUDIO_CHANNEL_CONFIG_FRONT_RIGHT = 0x00000002, + AUDIO_CHANNEL_CONFIG_FRONT_CENTER = 0x00000004, + AUDIO_CHANNEL_CONFIG_LOW_FRQ_EFFECTS = 0x00000008, + AUDIO_CHANNEL_CONFIG_BACK_LEFT = 0x00000010, + AUDIO_CHANNEL_CONFIG_BACK_RIGHT = 0x00000020, + AUDIO_CHANNEL_CONFIG_FRONT_LEFT_OF_CENTER = 0x00000040, + AUDIO_CHANNEL_CONFIG_FRONT_RIGHT_OF_CENTER = 0x00000080, + AUDIO_CHANNEL_CONFIG_BACK_CENTER = 0x00000100, + AUDIO_CHANNEL_CONFIG_SIDE_LEFT = 0x00000200, + AUDIO_CHANNEL_CONFIG_SIDE_RIGHT = 0x00000400, + AUDIO_CHANNEL_CONFIG_TOP_CENTER = 0x00000800, + AUDIO_CHANNEL_CONFIG_TOP_FRONT_LEFT = 0x00001000, + AUDIO_CHANNEL_CONFIG_TOP_FRONT_CENTER = 0x00002000, + AUDIO_CHANNEL_CONFIG_TOP_FRONT_RIGHT = 0x00004000, + AUDIO_CHANNEL_CONFIG_TOP_BACK_LEFT = 0x00008000, + AUDIO_CHANNEL_CONFIG_TOP_BACK_CENTER = 0x00010000, + AUDIO_CHANNEL_CONFIG_TOP_BACK_RIGHT = 0x00020000, + AUDIO_CHANNEL_CONFIG_TOP_FRONT_LEFT_OF_CENTER = 0x00040000, + AUDIO_CHANNEL_CONFIG_TOP_FRONT_RIGHT_OF_CENTER = 0x00080000, + AUDIO_CHANNEL_CONFIG_LEFT_LOW_FRQ_EFFECTS = 0x00100000, + AUDIO_CHANNEL_CONFIG_RIGHT_LOW_FRQ_EFFECTS = 0x00200000, + AUDIO_CHANNEL_CONFIG_TOP_SIDE_LEFT = 0x00400000, + AUDIO_CHANNEL_CONFIG_TOP_SIDE_RIGHT = 0x00800000, + AUDIO_CHANNEL_CONFIG_BOTTOM_CENTER = 0x01000000, + AUDIO_CHANNEL_CONFIG_BACK_LEFT_OF_CENTER = 0x02000000, + AUDIO_CHANNEL_CONFIG_BACK_RIGHT_OF_CENTER = 0x04000000, + AUDIO_CHANNEL_CONFIG_RAW_DATA = 0x80000000, +} audio_channel_config_t; + +/// AUDIO Channel Cluster Descriptor (4.1) +typedef struct TU_ATTR_PACKED { + uint8_t bNrChannels; ///< Number of channels currently connected. + audio_channel_config_t bmChannelConfig; ///< Bitmap according to 'audio_channel_config_t' with a 1 set if channel is connected and 0 else. In case channels are non-predefined ignore them here (see UAC2 specification 4.1 Audio Channel Cluster Descriptor. + uint8_t iChannelNames; ///< Index of a string descriptor, describing the name of the first inserted channel with a non-predefined spatial location. +} audio_desc_channel_cluster_t; + +/// AUDIO Class-Specific AC Interface Header Descriptor (4.7.2) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes: 9. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_HEADER. + uint16_t bcdADC ; ///< Audio Device Class Specification Release Number in Binary-Coded Decimal. Value: U16_TO_U8S_LE(0x0200). + uint8_t bCategory ; ///< Constant, indicating the primary use of this audio function, as intended by the manufacturer. See: audio_function_t. + uint16_t wTotalLength ; ///< Total number of bytes returned for the class-specific AudioControl interface descriptor. Includes the combined length of this descriptor header and all Clock Source, Unit and Terminal descriptors. + uint8_t bmControls ; ///< See: audio_cs_ac_interface_control_pos_t. +} audio_desc_cs_ac_interface_t; + +/// AUDIO Clock Source Descriptor (4.7.2.1) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes: 8. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_CLOCK_SOURCE. + uint8_t bClockID ; ///< Constant uniquely identifying the Clock Source Entity within the audio function. This value is used in all requests to address this Entity. + uint8_t bmAttributes ; ///< See: audio_clock_source_attribute_t. + uint8_t bmControls ; ///< See: audio_clock_source_control_pos_t. + uint8_t bAssocTerminal ; ///< Terminal ID of the Terminal that is associated with this Clock Source. + uint8_t iClockSource ; ///< Index of a string descriptor, describing the Clock Source Entity. +} audio_desc_clock_source_t; + +/// AUDIO Clock Selector Descriptor (4.7.2.2) for ONE pin +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 7+p. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_CLOCK_SELECTOR. + uint8_t bClockID ; ///< Constant uniquely identifying the Clock Selector Entity within the audio function. This value is used in all requests to address this Entity. + uint8_t bNrInPins ; ///< Number of Input Pins of this Unit: p = 1 thus bNrInPins = 1. + uint8_t baCSourceID ; ///< ID of the Clock Entity to which the first Clock Input Pin of this Clock Selector Entity is connected.. + uint8_t bmControls ; ///< See: audio_clock_selector_control_pos_t. + uint8_t iClockSource ; ///< Index of a string descriptor, describing the Clock Selector Entity. +} audio_desc_clock_selector_t; + +/// AUDIO Clock Selector Descriptor (4.7.2.2) for multiple pins +#define audio_desc_clock_selector_n_t(source_num) \ + struct TU_ATTR_PACKED { \ + uint8_t bLength ; \ + uint8_t bDescriptorType ; \ + uint8_t bDescriptorSubType ; \ + uint8_t bClockID ; \ + uint8_t bNrInPins ; \ + struct TU_ATTR_PACKED { \ + uint8_t baSourceID ; \ + } sourceID[source_num] ; \ + uint8_t bmControls ; \ + uint8_t iClockSource ; \ +} + +/// AUDIO Clock Multiplier Descriptor (4.7.2.3) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 7. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_CLOCK_MULTIPLIER. + uint8_t bClockID ; ///< Constant uniquely identifying the Clock Multiplier Entity within the audio function. This value is used in all requests to address this Entity. + uint8_t bCSourceID ; ///< ID of the Clock Entity to which the last Clock Input Pin of this Clock Selector Entity is connected. + uint8_t bmControls ; ///< See: audio_clock_multiplier_control_pos_t. + uint8_t iClockSource ; ///< Index of a string descriptor, describing the Clock Multiplier Entity. +} audio_desc_clock_multiplier_t; + +/// AUDIO Input Terminal Descriptor(4.7.2.4) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 17. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_INPUT_TERMINAL. + uint16_t wTerminalType ; ///< Constant characterizing the type of Terminal. See: audio_terminal_type_t for USB streaming and audio_terminal_input_type_t for other input types. + uint8_t bAssocTerminal ; ///< ID of the Output Terminal to which this Input Terminal is associated. + uint8_t bCSourceID ; ///< ID of the Clock Entity to which this Input Terminal is connected. + uint8_t bNrChannels ; ///< Number of logical output channels in the Terminal’s output audio channel cluster. + uint32_t bmChannelConfig ; ///< Describes the spatial location of the logical channels. See:audio_channel_config_t. + uint16_t bmControls ; ///< See: audio_terminal_input_control_pos_t. + uint8_t iTerminal ; ///< Index of a string descriptor, describing the Input Terminal. +} audio_desc_input_terminal_t; + +/// AUDIO Output Terminal Descriptor(4.7.2.5) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 12. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_OUTPUT_TERMINAL. + uint8_t bTerminalID ; ///< Constant uniquely identifying the Terminal within the audio function. This value is used in all requests to address this Terminal. + uint16_t wTerminalType ; ///< Constant characterizing the type of Terminal. See: audio_terminal_type_t for USB streaming and audio_terminal_output_type_t for other output types. + uint8_t bAssocTerminal ; ///< Constant, identifying the Input Terminal to which this Output Terminal is associated. + uint8_t bSourceID ; ///< ID of the Unit or Terminal to which this Terminal is connected. + uint8_t bCSourceID ; ///< ID of the Clock Entity to which this Output Terminal is connected. + uint16_t bmControls ; ///< See: audio_terminal_output_type_t. + uint8_t iTerminal ; ///< Index of a string descriptor, describing the Output Terminal. +} audio_desc_output_terminal_t; + +/// AUDIO Feature Unit Descriptor(4.7.2.8) for ONE channel +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 14. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_FEATURE_UNIT. + uint8_t bUnitID ; ///< Constant uniquely identifying the Unit within the audio function. This value is used in all requests to address this Unit. + uint8_t bSourceID ; ///< ID of the Unit or Terminal to which this Feature Unit is connected. + struct TU_ATTR_PACKED { + uint32_t bmaControls ; ///< See: audio_feature_unit_control_pos_t. Controls0 is master channel 0 (always present) and Controls1 is logical channel 1. + } controls[2] ; + uint8_t iTerminal ; ///< Index of a string descriptor, describing this Feature Unit. +} audio_desc_feature_unit_t; + +/// AUDIO Feature Unit Descriptor(4.7.2.8) for multiple channels +#define audio_desc_feature_unit_n_t(ch_num)\ + struct TU_ATTR_PACKED { \ + uint8_t bLength ; /* 6+(ch_num+1)*4 */\ + uint8_t bDescriptorType ; \ + uint8_t bDescriptorSubType ; \ + uint8_t bUnitID ; \ + uint8_t bSourceID ; \ + struct TU_ATTR_PACKED { \ + uint32_t bmaControls ; \ + } controls[ch_num+1] ; \ + uint8_t iTerminal ; \ +} + +/// AUDIO Class-Specific AS Interface Descriptor(4.9.2) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 16. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AS_INTERFACE_AS_GENERAL. + uint8_t bTerminalLink ; ///< The Terminal ID of the Terminal to which this interface is connected. + uint8_t bmControls ; ///< See: audio_cs_as_interface_control_pos_t. + uint8_t bFormatType ; ///< Constant identifying the Format Type the AudioStreaming interface is using. See: audio_format_type_t. + uint32_t bmFormats ; ///< The Audio Data Format(s) that can be used to communicate with this interface.See: audio_data_format_type_I_t. + uint8_t bNrChannels ; ///< Number of physical channels in the AS Interface audio channel cluster. + uint32_t bmChannelConfig ; ///< Describes the spatial location of the physical channels. See: audio_channel_config_t. + uint8_t iChannelNames ; ///< Index of a string descriptor, describing the name of the first physical channel. +} audio_desc_cs_as_interface_t; + +/// AUDIO Type I Format Type Descriptor(2.3.1.6 - Audio Formats) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 6. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AS_INTERFACE_FORMAT_TYPE. + uint8_t bFormatType ; ///< Constant identifying the Format Type the AudioStreaming interface is using. Value: AUDIO_FORMAT_TYPE_I. + uint8_t bSubslotSize ; ///< The number of bytes occupied by one audio subslot. Can be 1, 2, 3 or 4. + uint8_t bBitResolution ; ///< The number of effectively used bits from the available bits in an audio subslot. +} audio_desc_type_I_format_t; + +/// AUDIO Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor, in bytes: 8. + uint8_t bDescriptorType ; ///< Descriptor Type. Value: TUSB_DESC_CS_ENDPOINT. + uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_EP_SUBTYPE_GENERAL. + uint8_t bmAttributes ; ///< See: audio_cs_as_iso_data_ep_attribute_t. + uint8_t bmControls ; ///< See: audio_cs_as_iso_data_ep_control_pos_t. + uint8_t bLockDelayUnits ; ///< Indicates the units used for the wLockDelay field. See: audio_cs_as_iso_data_ep_lock_delay_unit_t. + uint16_t wLockDelay ; ///< Indicates the time it takes this endpoint to reliably lock its internal clock recovery circuitry. Units used depend on the value of the bLockDelayUnits field. +} audio_desc_cs_as_iso_data_ep_t; + +// 5.2.2 Control Request Layout +typedef struct TU_ATTR_PACKED +{ + union + { + struct TU_ATTR_PACKED + { + uint8_t recipient : 5; ///< Recipient type tusb_request_recipient_t. + uint8_t type : 2; ///< Request type tusb_request_type_t. + uint8_t direction : 1; ///< Direction type. tusb_dir_t + } bmRequestType_bit; + + uint8_t bmRequestType; + }; + + uint8_t bRequest; ///< Request type audio_cs_req_t + uint8_t bChannelNumber; + uint8_t bControlSelector; + union + { + uint8_t bInterface; + uint8_t bEndpoint; + }; + uint8_t bEntityID; + uint16_t wLength; +} audio_control_request_t; + +//// 5.2.3 Control Request Parameter Block Layout + +// 5.2.3.1 1-byte Control CUR Parameter Block +typedef struct TU_ATTR_PACKED +{ + int8_t bCur ; ///< The setting for the CUR attribute of the addressed Control +} audio_control_cur_1_t; + +// 5.2.3.2 2-byte Control CUR Parameter Block +typedef struct TU_ATTR_PACKED +{ + int16_t bCur ; ///< The setting for the CUR attribute of the addressed Control +} audio_control_cur_2_t; + +// 5.2.3.3 4-byte Control CUR Parameter Block +typedef struct TU_ATTR_PACKED +{ + int32_t bCur ; ///< The setting for the CUR attribute of the addressed Control +} audio_control_cur_4_t; + +// Use the following ONLY for RECEIVED data - compiler does not know how many subranges are defined! Use the one below for predefined lengths - or if you know what you are doing do what you like +// 5.2.3.1 1-byte Control RANGE Parameter Block +typedef struct TU_ATTR_PACKED { + uint16_t wNumSubRanges; + struct TU_ATTR_PACKED { + int8_t bMin ; /*The setting for the MIN attribute of the nth subrange of the addressed Control*/ + int8_t bMax ; /*The setting for the MAX attribute of the nth subrange of the addressed Control*/ + uint8_t bRes ; /*The setting for the RES attribute of the nth subrange of the addressed Control*/ + } subrange[] ; +} audio_control_range_1_t; + +// 5.2.3.2 2-byte Control RANGE Parameter Block +typedef struct TU_ATTR_PACKED { + uint16_t wNumSubRanges; + struct TU_ATTR_PACKED { + int16_t bMin ; /*The setting for the MIN attribute of the nth subrange of the addressed Control*/ + int16_t bMax ; /*The setting for the MAX attribute of the nth subrange of the addressed Control*/ + uint16_t bRes ; /*The setting for the RES attribute of the nth subrange of the addressed Control*/ + } subrange[] ; +} audio_control_range_2_t; + +// 5.2.3.3 4-byte Control RANGE Parameter Block +typedef struct TU_ATTR_PACKED { + uint16_t wNumSubRanges; + struct TU_ATTR_PACKED { + int32_t bMin ; /*The setting for the MIN attribute of the nth subrange of the addressed Control*/ + int32_t bMax ; /*The setting for the MAX attribute of the nth subrange of the addressed Control*/ + uint32_t bRes ; /*The setting for the RES attribute of the nth subrange of the addressed Control*/ + } subrange[] ; +} audio_control_range_4_t; + +// 5.2.3.1 1-byte Control RANGE Parameter Block +#define audio_control_range_1_n_t(numSubRanges) \ + struct TU_ATTR_PACKED { \ + uint16_t wNumSubRanges; \ + struct TU_ATTR_PACKED { \ + int8_t bMin ; /*The setting for the MIN attribute of the nth subrange of the addressed Control*/\ + int8_t bMax ; /*The setting for the MAX attribute of the nth subrange of the addressed Control*/\ + uint8_t bRes ; /*The setting for the RES attribute of the nth subrange of the addressed Control*/\ + } subrange[numSubRanges] ; \ +} + +/// 5.2.3.2 2-byte Control RANGE Parameter Block +#define audio_control_range_2_n_t(numSubRanges) \ + struct TU_ATTR_PACKED { \ + uint16_t wNumSubRanges; \ + struct TU_ATTR_PACKED { \ + int16_t bMin ; /*The setting for the MIN attribute of the nth subrange of the addressed Control*/\ + int16_t bMax ; /*The setting for the MAX attribute of the nth subrange of the addressed Control*/\ + uint16_t bRes ; /*The setting for the RES attribute of the nth subrange of the addressed Control*/\ + } subrange[numSubRanges]; \ +} + +// 5.2.3.3 4-byte Control RANGE Parameter Block +#define audio_control_range_4_n_t(numSubRanges) \ + struct TU_ATTR_PACKED { \ + uint16_t wNumSubRanges; \ + struct TU_ATTR_PACKED { \ + int32_t bMin ; /*The setting for the MIN attribute of the nth subrange of the addressed Control*/\ + int32_t bMax ; /*The setting for the MAX attribute of the nth subrange of the addressed Control*/\ + uint32_t bRes ; /*The setting for the RES attribute of the nth subrange of the addressed Control*/\ + } subrange[numSubRanges]; \ +} + +/** @} */ + +#ifdef __cplusplus +} +#endif + +#endif + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio_device.c new file mode 100644 index 000000000..d21980060 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio_device.c @@ -0,0 +1,2294 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Reinhard Panhuber, Jerzy Kasenberg + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/* + * This driver supports at most one out EP, one in EP, one control EP, and one feedback EP and one alternative interface other than zero. Hence, only one input terminal and one output terminal are support, if you need more adjust the driver! + * It supports multiple TX and RX channels. + * + * In case you need more alternate interfaces, you need to define additional defines for this specific alternate interface. Just define them and set them in the set_interface function. + * + * There are three data flow structures currently implemented, where at least one SW-FIFO is used to decouple the asynchronous processes MCU vs. host + * + * 1. Input data -> SW-FIFO -> MCU USB + * + * The most easiest version, available in case the target MCU can handle the software FIFO (SW-FIFO) and if it is implemented in the device driver (if yes then dcd_edpt_xfer_fifo() is available) + * + * 2. Input data -> SW-FIFO -> Linear buffer -> MCU USB + * + * In case the target MCU can not handle a SW-FIFO, a linear buffer is used. This uses the default function dcd_edpt_xfer(). In this case more memory is required. + * + * 3. (Input data 1 | Input data 2 | ... | Input data N) -> (SW-FIFO 1 | SW-FIFO 2 | ... | SW-FIFO N) -> Linear buffer -> MCU USB + * + * This case is used if you have more channels which need to be combined into one stream. Every channel has its own SW-FIFO. All data is encoded into an Linear buffer. + * + * The same holds in the RX case. + * + * */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_AUDIO) + +//--------------------------------------------------------------------+ +// INCLUDE +//--------------------------------------------------------------------+ +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "audio_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +// Use ring buffer if it's available, some MCUs need extra RAM requirements +#ifndef TUD_AUDIO_PREFER_RING_BUFFER +#if CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX +#define TUD_AUDIO_PREFER_RING_BUFFER 0 +#else +#define TUD_AUDIO_PREFER_RING_BUFFER 1 +#endif +#endif + +// Linear buffer in case target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer +// is available or driver is would need to be changed dramatically + +// Only STM32 synopsys and dcd_transdimension use non-linear buffer for now +// Synopsys detection copied from dcd_synopsys.c (refactor later on) +#if defined (STM32F105x8) || defined (STM32F105xB) || defined (STM32F105xC) || \ + defined (STM32F107xB) || defined (STM32F107xC) +#define STM32F1_SYNOPSYS +#endif + +#if defined (STM32L475xx) || defined (STM32L476xx) || \ + defined (STM32L485xx) || defined (STM32L486xx) || defined (STM32L496xx) || \ + defined (STM32L4R5xx) || defined (STM32L4R7xx) || defined (STM32L4R9xx) || \ + defined (STM32L4S5xx) || defined (STM32L4S7xx) || defined (STM32L4S9xx) +#define STM32L4_SYNOPSYS +#endif + +#if (CFG_TUSB_MCU == OPT_MCU_STM32F1 && defined(STM32F1_SYNOPSYS)) || \ + CFG_TUSB_MCU == OPT_MCU_STM32F2 || \ + CFG_TUSB_MCU == OPT_MCU_STM32F4 || \ + CFG_TUSB_MCU == OPT_MCU_STM32F7 || \ + CFG_TUSB_MCU == OPT_MCU_STM32H7 || \ + (CFG_TUSB_MCU == OPT_MCU_STM32L4 && defined(STM32L4_SYNOPSYS)) || \ + CFG_TUSB_MCU == OPT_MCU_RX63X || \ + CFG_TUSB_MCU == OPT_MCU_RX65X || \ + CFG_TUSB_MCU == OPT_MCU_RX72N || \ + CFG_TUSB_MCU == OPT_MCU_GD32VF103 || \ + CFG_TUSB_MCU == OPT_MCU_LPC18XX || \ + CFG_TUSB_MCU == OPT_MCU_LPC43XX || \ + CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX || \ + CFG_TUSB_MCU == OPT_MCU_MSP432E4 +#if TUD_AUDIO_PREFER_RING_BUFFER +#define USE_LINEAR_BUFFER 0 +#else +#define USE_LINEAR_BUFFER 1 +#endif +#else +#define USE_LINEAR_BUFFER 1 +#endif + +// Declaration of buffers + +// Check for maximum supported numbers +#if CFG_TUD_AUDIO > 3 +#error Maximum number of audio functions restricted to three! +#endif + +// EP IN software buffers and mutexes +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING +#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_in_sw_buf_1[CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t ep_in_ff_mutex_wr_1; // No need for read mutex as only USB driver reads from FIFO +#endif +#endif // CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0 +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_in_sw_buf_2[CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t ep_in_ff_mutex_wr_2; // No need for read mutex as only USB driver reads from FIFO +#endif +#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0 +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_in_sw_buf_3[CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t ep_in_ff_mutex_wr_3; // No need for read mutex as only USB driver reads from FIFO +#endif +#endif // CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0 +#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING + +// Linear buffer TX in case: +// - target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer is available or driver is would need to be changed dramatically OR +// - the software encoding is used - in this case the linear buffers serve as a target memory where logical channels are encoded into +#if CFG_TUD_AUDIO_ENABLE_EP_IN && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_ENCODING) +#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_in_1[CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX]; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_in_2[CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX]; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_in_3[CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX]; +#endif +#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING) + +// EP OUT software buffers and mutexes +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING +#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_out_sw_buf_1[CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t ep_out_ff_mutex_rd_1; // No need for write mutex as only USB driver writes into FIFO +#endif +#endif // CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0 +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_out_sw_buf_2[CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t ep_out_ff_mutex_rd_2; // No need for write mutex as only USB driver writes into FIFO +#endif +#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0 +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_out_sw_buf_3[CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t ep_out_ff_mutex_rd_3; // No need for write mutex as only USB driver writes into FIFO +#endif +#endif // CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0 +#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING + +// Linear buffer RX in case: +// - target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer is available or driver is would need to be changed dramatically OR +// - the software encoding is used - in this case the linear buffers serve as a target memory where logical channels are encoded into +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING) +#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_out_1[CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX]; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_out_2[CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX]; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_out_3[CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX]; +#endif +#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING) + +// Control buffers +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t ctrl_buf_1[CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ]; +#if CFG_TUD_AUDIO > 1 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t ctrl_buf_2[CFG_TUD_AUDIO_FUNC_2_CTRL_BUF_SZ]; +#endif +#if CFG_TUD_AUDIO > 2 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t ctrl_buf_3[CFG_TUD_AUDIO_FUNC_3_CTRL_BUF_SZ]; +#endif + +// Active alternate setting of interfaces +uint8_t alt_setting_1[CFG_TUD_AUDIO_FUNC_1_N_AS_INT]; +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_N_AS_INT > 0 +uint8_t alt_setting_2[CFG_TUD_AUDIO_FUNC_2_N_AS_INT]; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_N_AS_INT > 0 +uint8_t alt_setting_3[CFG_TUD_AUDIO_FUNC_3_N_AS_INT]; +#endif + +// Software encoding/decoding support FIFOs +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING +#if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t tx_supp_ff_buf_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ]; +tu_fifo_t tx_supp_ff_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t tx_supp_ff_mutex_wr_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO]; // No need for read mutex as only USB driver reads from FIFO +#endif +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t tx_supp_ff_buf_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ]; +tu_fifo_t tx_supp_ff_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t tx_supp_ff_mutex_wr_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO]; // No need for read mutex as only USB driver reads from FIFO +#endif +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t tx_supp_ff_buf_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ]; +tu_fifo_t tx_supp_ff_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t tx_supp_ff_mutex_wr_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO]; // No need for read mutex as only USB driver reads from FIFO +#endif +#endif +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING +#if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t rx_supp_ff_buf_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ]; +tu_fifo_t rx_supp_ff_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t rx_supp_ff_mutex_rd_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO]; // No need for write mutex as only USB driver writes into FIFO +#endif +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t rx_supp_ff_buf_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ]; +tu_fifo_t rx_supp_ff_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t rx_supp_ff_mutex_rd_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO]; // No need for write mutex as only USB driver writes into FIFO +#endif +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0 +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t rx_supp_ff_buf_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ]; +tu_fifo_t rx_supp_ff_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO]; +#if CFG_FIFO_MUTEX +osal_mutex_def_t rx_supp_ff_mutex_rd_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO]; // No need for write mutex as only USB driver writes into FIFO +#endif +#endif +#endif + +typedef struct +{ + uint8_t rhport; + uint8_t const * p_desc; // Pointer pointing to Standard AC Interface Descriptor(4.7.1) - Audio Control descriptor defining audio function + +#if CFG_TUD_AUDIO_ENABLE_EP_IN + uint8_t ep_in; // TX audio data EP. + uint16_t ep_in_sz; // Current size of TX EP + uint8_t ep_in_as_intf_num; // Corresponding Standard AS Interface Descriptor (4.9.1) belonging to output terminal to which this EP belongs - 0 is invalid (this fits to UAC2 specification since AS interfaces can not have interface number equal to zero) +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + uint8_t ep_out; // Incoming (into uC) audio data EP. + uint16_t ep_out_sz; // Current size of RX EP + uint8_t ep_out_as_intf_num; // Corresponding Standard AS Interface Descriptor (4.9.1) belonging to input terminal to which this EP belongs - 0 is invalid (this fits to UAC2 specification since AS interfaces can not have interface number equal to zero) + +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + uint8_t ep_fb; // Feedback EP. +#endif + +#endif + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN + uint8_t ep_int_ctr; // Audio control interrupt EP. +#endif + + /*------------- From this point, data is not cleared by bus reset -------------*/ + + uint16_t desc_length; // Length of audio function descriptor + + // Buffer for control requests + uint8_t * ctrl_buf; + uint8_t ctrl_buf_sz; + + // Current active alternate settings + uint8_t * alt_setting; // We need to save the current alternate setting this way, because it is possible that there are AS interfaces which do not have an EP! + + // EP Transfer buffers and FIFOs +#if CFG_TUD_AUDIO_ENABLE_EP_OUT +#if !CFG_TUD_AUDIO_ENABLE_DECODING + tu_fifo_t ep_out_ff; +#endif + +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + uint32_t fb_val; // Feedback value for asynchronous mode (in 16.16 format). +#endif +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING + tu_fifo_t ep_in_ff; +#endif + + // Audio control interrupt buffer - no FIFO - 6 Bytes according to UAC 2 specification (p. 74) +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN + CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t ep_int_ctr_buf[CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE]; +#endif + + // Decoding parameters - parameters are set when alternate AS interface is set by host + // Coding is currently only supported for EP. Software coding corresponding to AS interfaces without EPs are not supported currently. +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING + audio_format_type_t format_type_rx; + uint8_t n_channels_rx; + +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING + audio_data_format_type_I_t format_type_I_rx; + uint8_t n_bytes_per_sampe_rx; + uint8_t n_channels_per_ff_rx; + uint8_t n_ff_used_rx; +#endif +#endif + + // Encoding parameters - parameters are set when alternate AS interface is set by host +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING + audio_format_type_t format_type_tx; + uint8_t n_channels_tx; + +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING + audio_data_format_type_I_t format_type_I_tx; + uint8_t n_bytes_per_sampe_tx; + uint8_t n_channels_per_ff_tx; + uint8_t n_ff_used_tx; +#endif +#endif + + // Support FIFOs for software encoding and decoding +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING + tu_fifo_t * rx_supp_ff; + uint8_t n_rx_supp_ff; + uint16_t rx_supp_ff_sz_max; +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING + tu_fifo_t * tx_supp_ff; + uint8_t n_tx_supp_ff; + uint16_t tx_supp_ff_sz_max; +#endif + + // Linear buffer in case target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer is available or driver is would need to be changed dramatically OR the support FIFOs are used +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING) + uint8_t * lin_buf_out; +#define USE_LINEAR_BUFFER_RX 1 +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_ENCODING) + uint8_t * lin_buf_in; +#define USE_LINEAR_BUFFER_TX 1 +#endif + +} audiod_function_t; + +#ifndef USE_LINEAR_BUFFER_TX +#define USE_LINEAR_BUFFER_TX 0 +#endif + +#ifndef USE_LINEAR_BUFFER_RX +#define USE_LINEAR_BUFFER_RX 0 +#endif + +#define ITF_MEM_RESET_SIZE offsetof(audiod_function_t, ctrl_buf) + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +CFG_TUSB_MEM_SECTION audiod_function_t _audiod_fct[CFG_TUD_AUDIO]; + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT +static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received); +#endif + +#if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT +static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN +static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t* audio); +#endif + +#if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN +static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t* audio); +#endif + +static bool audiod_get_interface(uint8_t rhport, tusb_control_request_t const * p_request); +static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const * p_request); + +static bool audiod_get_AS_interface_index_global(uint8_t itf, uint8_t *func_id, uint8_t *idxItf, uint8_t const **pp_desc_int); +static bool audiod_get_AS_interface_index(uint8_t itf, audiod_function_t * audio, uint8_t *idxItf, uint8_t const **pp_desc_int); +static bool audiod_verify_entity_exists(uint8_t itf, uint8_t entityID, uint8_t *func_id); +static bool audiod_verify_itf_exists(uint8_t itf, uint8_t *func_id); +static bool audiod_verify_ep_exists(uint8_t ep, uint8_t *func_id); +static uint8_t audiod_get_audio_fct_idx(audiod_function_t * audio); + +#if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_ENABLE_DECODING +static void audiod_parse_for_AS_params(audiod_function_t* audio, uint8_t const * p_desc, uint8_t const * p_desc_end, uint8_t const as_itf); + +static inline uint8_t tu_desc_subtype(void const* desc) +{ + return ((uint8_t const*) desc)[2]; +} +#endif + +bool tud_audio_n_mounted(uint8_t func_id) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO); + audiod_function_t* audio = &_audiod_fct[func_id]; + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + if (audio->ep_out == 0) return false; +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN + if (audio->ep_in == 0) return false; +#endif + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN + if (audio->ep_int_ctr == 0) return false; +#endif + +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + if (audio->ep_fb == 0) return false; +#endif + + return true; +} + +//--------------------------------------------------------------------+ +// READ API +//--------------------------------------------------------------------+ + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING + +uint16_t tud_audio_n_available(uint8_t func_id) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + return tu_fifo_count(&_audiod_fct[func_id].ep_out_ff); +} + +uint16_t tud_audio_n_read(uint8_t func_id, void* buffer, uint16_t bufsize) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + return tu_fifo_read_n(&_audiod_fct[func_id].ep_out_ff, buffer, bufsize); +} + +bool tud_audio_n_clear_ep_out_ff(uint8_t func_id) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + return tu_fifo_clear(&_audiod_fct[func_id].ep_out_ff); +} + +tu_fifo_t* tud_audio_n_get_ep_out_ff(uint8_t func_id) +{ + if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL) return &_audiod_fct[func_id].ep_out_ff; + return NULL; +} + +#endif + +#if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT +// Delete all content in the support RX FIFOs +bool tud_audio_n_clear_rx_support_ff(uint8_t func_id, uint8_t ff_idx) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff); + return tu_fifo_clear(&_audiod_fct[func_id].rx_supp_ff[ff_idx]); +} + +uint16_t tud_audio_n_available_support_ff(uint8_t func_id, uint8_t ff_idx) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff); + return tu_fifo_count(&_audiod_fct[func_id].rx_supp_ff[ff_idx]); +} + +uint16_t tud_audio_n_read_support_ff(uint8_t func_id, uint8_t ff_idx, void* buffer, uint16_t bufsize) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff); + return tu_fifo_read_n(&_audiod_fct[func_id].rx_supp_ff[ff_idx], buffer, bufsize); +} + +tu_fifo_t* tud_audio_n_get_rx_support_ff(uint8_t func_id, uint8_t ff_idx) +{ + if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff) return &_audiod_fct[func_id].rx_supp_ff[ff_idx]; + return NULL; +} +#endif + +// This function is called once an audio packet is received by the USB and is responsible for putting data from USB memory into EP_OUT_FIFO (or support FIFOs + decoding of received stream into audio channels). +// If you prefer your own (more efficient) implementation suiting your purpose set CFG_TUD_AUDIO_ENABLE_DECODING = 0. + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + +static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received) +{ + uint8_t idxItf; + uint8_t const *dummy2; + uint8_t idx_audio_fct = 0; + + if (tud_audio_rx_done_pre_read_cb || tud_audio_rx_done_post_read_cb) + { + idx_audio_fct = audiod_get_audio_fct_idx(audio); + TU_VERIFY(audiod_get_AS_interface_index(audio->ep_out_as_intf_num, audio, &idxItf, &dummy2)); + } + + // Call a weak callback here - a possibility for user to get informed an audio packet was received and data gets now loaded into EP FIFO (or decoded into support RX software FIFO) + if (tud_audio_rx_done_pre_read_cb) TU_VERIFY(tud_audio_rx_done_pre_read_cb(rhport, n_bytes_received, idx_audio_fct, audio->ep_out, audio->alt_setting[idxItf])); + +#if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT + + switch (audio->format_type_rx) + { + case AUDIO_FORMAT_TYPE_UNDEFINED: + // INDIVIDUAL DECODING PROCEDURE REQUIRED HERE! + TU_LOG2(" Desired CFG_TUD_AUDIO_FORMAT encoding not implemented!\r\n"); + TU_BREAKPOINT(); + break; + + case AUDIO_FORMAT_TYPE_I: + + switch (audio->format_type_I_tx) + { + case AUDIO_DATA_FORMAT_TYPE_I_PCM: + TU_VERIFY(audiod_decode_type_I_pcm(rhport, audio, n_bytes_received)); + break; + + default: + // DESIRED CFG_TUD_AUDIO_FORMAT_TYPE_I_RX NOT IMPLEMENTED! + TU_LOG2(" Desired CFG_TUD_AUDIO_FORMAT_TYPE_I_RX encoding not implemented!\r\n"); + TU_BREAKPOINT(); + break; + } + break; + + default: + // Desired CFG_TUD_AUDIO_FORMAT_TYPE_RX not implemented! + TU_LOG2(" Desired CFG_TUD_AUDIO_FORMAT_TYPE_RX not implemented!\r\n"); + TU_BREAKPOINT(); + break; + } + + // Prepare for next transmission + TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_out, audio->lin_buf_out, audio->ep_out_sz), false); + +#else + +#if USE_LINEAR_BUFFER_RX + // Data currently is in linear buffer, copy into EP OUT FIFO + TU_VERIFY(tu_fifo_write_n(&audio->ep_out_ff, audio->lin_buf_out, n_bytes_received)); + + // Schedule for next receive + TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_out, audio->lin_buf_out, audio->ep_out_sz), false); +#else + // Data is already placed in EP FIFO, schedule for next receive + TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_out, &audio->ep_out_ff, audio->ep_out_sz), false); +#endif + +#endif + + // Call a weak callback here - a possibility for user to get informed decoding was completed + if (tud_audio_rx_done_post_read_cb) TU_VERIFY(tud_audio_rx_done_post_read_cb(rhport, n_bytes_received, idx_audio_fct, audio->ep_out, audio->alt_setting[idxItf])); + + return true; +} + +#endif //CFG_TUD_AUDIO_ENABLE_EP_OUT + +// The following functions are used in case CFG_TUD_AUDIO_ENABLE_DECODING != 0 +#if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT + +// Decoding according to 2.3.1.5 Audio Streams + +// Helper function +static inline uint8_t * audiod_interleaved_copy_bytes_fast_decode(uint16_t const nBytesToCopy, void * dst, uint8_t * dst_end, uint8_t * src, uint8_t const n_ff_used) +{ + + // This function is an optimized version of + // while((uint8_t *)dst < dst_end) + // { + // memcpy(dst, src, nBytesToCopy); + // dst = (uint8_t *)dst + nBytesToCopy; + // src += nBytesToCopy * n_ff_used; + // } + + // Optimize for fast half word copies + typedef struct{ + uint16_t val; + } __attribute((__packed__)) unaligned_uint16_t; + + // Optimize for fast word copies + typedef struct{ + uint32_t val; + } __attribute((__packed__)) unaligned_uint32_t; + + switch (nBytesToCopy) + { + case 1: + while((uint8_t *)dst < dst_end) + { + *(uint8_t *)dst++ = *src; + src += n_ff_used; + } + break; + + case 2: + while((uint8_t *)dst < dst_end) + { + *(unaligned_uint16_t*)dst = *(unaligned_uint16_t*)src; + dst += 2; + src += 2 * n_ff_used; + } + break; + + case 3: + while((uint8_t *)dst < dst_end) + { + // memcpy(dst, src, 3); + // dst = (uint8_t *)dst + 3; + // src += 3 * n_ff_used; + + // TODO: Is there a faster way to copy 3 bytes? + *(uint8_t *)dst++ = *src++; + *(uint8_t *)dst++ = *src++; + *(uint8_t *)dst++ = *src++; + + src += 3 * (n_ff_used - 1); + } + break; + + case 4: + while((uint8_t *)dst < dst_end) + { + *(unaligned_uint32_t*)dst = *(unaligned_uint32_t*)src; + dst += 4; + src += 4 * n_ff_used; + } + break; + } + + return src; +} + +static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received) +{ + (void) rhport; + + // Determine amount of samples + uint8_t const n_ff_used = audio->n_ff_used_rx; + uint16_t const nBytesPerFFToRead = n_bytes_received / n_ff_used; + uint8_t cnt_ff; + + // Decode + uint8_t * src; + uint8_t * dst_end; + + tu_fifo_buffer_info_t info; + + for (cnt_ff = 0; cnt_ff < n_ff_used; cnt_ff++) + { + tu_fifo_get_write_info(&audio->rx_supp_ff[cnt_ff], &info); + + if (info.len_lin != 0) + { + info.len_lin = tu_min16(nBytesPerFFToRead, info.len_lin); + src = &audio->lin_buf_out[cnt_ff*audio->n_channels_per_ff_rx * audio->n_bytes_per_sampe_rx]; + dst_end = info.ptr_lin + info.len_lin; + src = audiod_interleaved_copy_bytes_fast_decode(audio->n_bytes_per_sampe_rx, info.ptr_lin, dst_end, src, n_ff_used); + + // Handle wrapped part of FIFO + info.len_wrap = tu_min16(nBytesPerFFToRead - info.len_lin, info.len_wrap); + if (info.len_wrap != 0) + { + dst_end = info.ptr_wrap + info.len_wrap; + audiod_interleaved_copy_bytes_fast_decode(audio->n_bytes_per_sampe_rx, info.ptr_wrap, dst_end, src, n_ff_used); + } + tu_fifo_advance_write_pointer(&audio->rx_supp_ff[cnt_ff], info.len_lin + info.len_wrap); + } + } + + // Number of bytes should be a multiple of CFG_TUD_AUDIO_N_BYTES_PER_SAMPLE_RX * CFG_TUD_AUDIO_N_CHANNELS_RX but checking makes no sense - no way to correct it + // TU_VERIFY(cnt != n_bytes); + + return true; +} +#endif //CFG_TUD_AUDIO_ENABLE_DECODING + +//--------------------------------------------------------------------+ +// WRITE API +//--------------------------------------------------------------------+ + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING + +/** + * \brief Write data to EP in buffer + * + * Write data to buffer. If it is full, new data can be inserted once a transmit was scheduled. See audiod_tx_done_cb(). + * If TX FIFOs are used, this function is not available in order to not let the user mess up the encoding process. + * + * \param[in] func_id: Index of audio function interface + * \param[in] data: Pointer to data array to be copied from + * \param[in] len: # of array elements to copy + * \return Number of bytes actually written + */ +uint16_t tud_audio_n_write(uint8_t func_id, const void * data, uint16_t len) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + return tu_fifo_write_n(&_audiod_fct[func_id].ep_in_ff, data, len); +} + +bool tud_audio_n_clear_ep_in_ff(uint8_t func_id) // Delete all content in the EP IN FIFO +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + return tu_fifo_clear(&_audiod_fct[func_id].ep_in_ff); +} + +tu_fifo_t* tud_audio_n_get_ep_in_ff(uint8_t func_id) +{ + if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL) return &_audiod_fct[func_id].ep_in_ff; + return NULL; +} + +#endif + +#if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN + +uint16_t tud_audio_n_flush_tx_support_ff(uint8_t func_id) // Force all content in the support TX FIFOs to be written into linear buffer and schedule a transmit +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + audiod_function_t* audio = &_audiod_fct[func_id]; + + uint16_t n_bytes_copied = tu_fifo_count(&audio->tx_supp_ff[0]); + + TU_VERIFY(audiod_tx_done_cb(audio->rhport, audio)); + + n_bytes_copied -= tu_fifo_count(&audio->tx_supp_ff[0]); + n_bytes_copied = n_bytes_copied*audio->tx_supp_ff[0].item_size; + + return n_bytes_copied; +} + +bool tud_audio_n_clear_tx_support_ff(uint8_t func_id, uint8_t ff_idx) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_tx_supp_ff); + return tu_fifo_clear(&_audiod_fct[func_id].tx_supp_ff[ff_idx]); +} + +uint16_t tud_audio_n_write_support_ff(uint8_t func_id, uint8_t ff_idx, const void * data, uint16_t len) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_tx_supp_ff); + return tu_fifo_write_n(&_audiod_fct[func_id].tx_supp_ff[ff_idx], data, len); +} + +tu_fifo_t* tud_audio_n_get_tx_support_ff(uint8_t func_id, uint8_t ff_idx) +{ + if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_tx_supp_ff) return &_audiod_fct[func_id].tx_supp_ff[ff_idx]; + return NULL; +} + +#endif + + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN + +// If no interrupt transmit is pending bytes get written into buffer and a transmit is scheduled - once transmit completed tud_audio_int_ctr_done_cb() is called in inform user +uint16_t tud_audio_int_ctr_n_write(uint8_t func_id, uint8_t const* buffer, uint16_t len) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + + // We write directly into the EP's buffer - abort if previous transfer not complete + TU_VERIFY(!usbd_edpt_busy(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int_ctr)); + + // Check length + TU_VERIFY(len <= CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE); + + memcpy(_audiod_fct[func_id].ep_int_ctr_buf, buffer, len); + + // Schedule transmit + TU_VERIFY(usbd_edpt_xfer(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int_ctr, _audiod_fct[func_id].ep_int_ctr_buf, len)); + + return true; +} + +#endif + + +// This function is called once a transmit of an audio packet was successfully completed. Here, we encode samples and place it in IN EP's buffer for next transmission. +// If you prefer your own (more efficient) implementation suiting your purpose set CFG_TUD_AUDIO_ENABLE_ENCODING = 0 and use tud_audio_n_write. + +// n_bytes_copied - Informs caller how many bytes were loaded. In case n_bytes_copied = 0, a ZLP is scheduled to inform host no data is available for current frame. +#if CFG_TUD_AUDIO_ENABLE_EP_IN +static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t * audio) +{ + uint8_t idxItf; + uint8_t const *dummy2; + + uint8_t idx_audio_fct = audiod_get_audio_fct_idx(audio); + TU_VERIFY(audiod_get_AS_interface_index(audio->ep_in_as_intf_num, audio, &idxItf, &dummy2)); + + // Only send something if current alternate interface is not 0 as in this case nothing is to be sent due to UAC2 specifications + if (audio->alt_setting[idxItf] == 0) return false; + + // Call a weak callback here - a possibility for user to get informed former TX was completed and data gets now loaded into EP in buffer (in case FIFOs are used) or + // if no FIFOs are used the user may use this call back to load its data into the EP IN buffer by use of tud_audio_n_write_ep_in_buffer(). + if (tud_audio_tx_done_pre_load_cb) TU_VERIFY(tud_audio_tx_done_pre_load_cb(rhport, idx_audio_fct, audio->ep_in, audio->alt_setting[idxItf])); + + // Send everything in ISO EP FIFO + uint16_t n_bytes_tx; + + // If support FIFOs are used, encode and schedule transmit +#if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN + switch (audio->format_type_tx) + { + case AUDIO_FORMAT_TYPE_UNDEFINED: + // INDIVIDUAL ENCODING PROCEDURE REQUIRED HERE! + TU_LOG2(" Desired CFG_TUD_AUDIO_FORMAT encoding not implemented!\r\n"); + TU_BREAKPOINT(); + n_bytes_tx = 0; + break; + + case AUDIO_FORMAT_TYPE_I: + + switch (audio->format_type_I_tx) + { + case AUDIO_DATA_FORMAT_TYPE_I_PCM: + + n_bytes_tx = audiod_encode_type_I_pcm(rhport, audio); + break; + + default: + // YOUR ENCODING IS REQUIRED HERE! + TU_LOG2(" Desired CFG_TUD_AUDIO_FORMAT_TYPE_I_TX encoding not implemented!\r\n"); + TU_BREAKPOINT(); + n_bytes_tx = 0; + break; + } + break; + + default: + // Desired CFG_TUD_AUDIO_FORMAT_TYPE_TX not implemented! + TU_LOG2(" Desired CFG_TUD_AUDIO_FORMAT_TYPE_TX not implemented!\r\n"); + TU_BREAKPOINT(); + n_bytes_tx = 0; + break; + } + + TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_in, audio->lin_buf_in, n_bytes_tx)); + +#else + // No support FIFOs, if no linear buffer required schedule transmit, else put data into linear buffer and schedule + + n_bytes_tx = tu_min16(tu_fifo_count(&audio->ep_in_ff), audio->ep_in_sz); // Limit up to max packet size, more can not be done for ISO + +#if USE_LINEAR_BUFFER_TX + tu_fifo_read_n(&audio->ep_in_ff, audio->lin_buf_in, n_bytes_tx); + TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_in, audio->lin_buf_in, n_bytes_tx)); +#else + // Send everything in ISO EP FIFO + TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_in, &audio->ep_in_ff, n_bytes_tx)); +#endif + +#endif + + // Call a weak callback here - a possibility for user to get informed former TX was completed and how many bytes were loaded for the next frame + if (tud_audio_tx_done_post_load_cb) TU_VERIFY(tud_audio_tx_done_post_load_cb(rhport, n_bytes_tx, idx_audio_fct, audio->ep_in, audio->alt_setting[idxItf])); + + return true; +} + +#endif //CFG_TUD_AUDIO_ENABLE_EP_IN + +#if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN +// Take samples from the support buffer and encode them into the IN EP software FIFO +// Returns number of bytes written into linear buffer + +/* 2.3.1.7.1 PCM Format +The PCM (Pulse Coded Modulation) format is the most commonly used audio format to represent audio +data streams. The audio data is not compressed and uses a signed two’s-complement fixed point format. It +is left-justified (the sign bit is the Msb) and data is padded with trailing zeros to fill the remaining unused +bits of the subslot. The binary point is located to the right of the sign bit so that all values lie within the +range [-1, +1) + */ + +/* + * This function encodes channels saved within the support FIFOs into one stream by interleaving the PCM samples + * in the support FIFOs according to 2.3.1.5 Audio Streams. It does not control justification (left or right) and + * does not change the number of bytes per sample. + * */ + +// Helper function +static inline uint8_t * audiod_interleaved_copy_bytes_fast_encode(uint16_t const nBytesToCopy, uint8_t * src, uint8_t * src_end, uint8_t * dst, uint8_t const n_ff_used) +{ + // Optimize for fast half word copies + typedef struct{ + uint16_t val; + } __attribute((__packed__)) unaligned_uint16_t; + + // Optimize for fast word copies + typedef struct{ + uint32_t val; + } __attribute((__packed__)) unaligned_uint32_t; + + switch (nBytesToCopy) + { + case 1: + while(src < src_end) + { + *dst = *src++; + dst += n_ff_used; + } + break; + + case 2: + while(src < src_end) + { + *(unaligned_uint16_t*)dst = *(unaligned_uint16_t*)src; + src += 2; + dst += 2 * n_ff_used; + } + break; + + case 3: + while(src < src_end) + { + // memcpy(dst, src, 3); + // src = (uint8_t *)src + 3; + // dst += 3 * n_ff_used; + + // TODO: Is there a faster way to copy 3 bytes? + *dst++ = *src++; + *dst++ = *src++; + *dst++ = *src++; + + dst += 3 * (n_ff_used - 1); + } + break; + + case 4: + while(src < src_end) + { + *(unaligned_uint32_t*)dst = *(unaligned_uint32_t*)src; + src += 4; + dst += 4 * n_ff_used; + } + break; + } + + return dst; +} + +static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t* audio) +{ + // This function relies on the fact that the length of the support FIFOs was configured to be a multiple of the active sample size in bytes s.t. no sample is split within a wrap + // This is ensured within set_interface, where the FIFOs are reconfigured according to this size + + // We encode directly into IN EP's linear buffer - abort if previous transfer not complete + TU_VERIFY(!usbd_edpt_busy(rhport, audio->ep_in)); + + // Determine amount of samples + uint8_t const n_ff_used = audio->n_ff_used_tx; + uint16_t const nBytesToCopy = audio->n_channels_per_ff_tx * audio->n_bytes_per_sampe_tx; + uint16_t const capPerFF = audio->ep_in_sz / n_ff_used; // Sample capacity per FIFO in bytes + uint16_t nBytesPerFFToSend = tu_fifo_count(&audio->tx_supp_ff[0]); + uint8_t cnt_ff; + + for (cnt_ff = 1; cnt_ff < n_ff_used; cnt_ff++) + { + uint16_t const count = tu_fifo_count(&audio->tx_supp_ff[cnt_ff]); + if (count < nBytesPerFFToSend) + { + nBytesPerFFToSend = count; + } + } + + // Check if there is enough + if (nBytesPerFFToSend == 0) return 0; + + // Limit to maximum sample number - THIS IS A POSSIBLE ERROR SOURCE IF TOO MANY SAMPLE WOULD NEED TO BE SENT BUT CAN NOT! + nBytesPerFFToSend = tu_min16(nBytesPerFFToSend, capPerFF); + + // Round to full number of samples (flooring) + nBytesPerFFToSend = (nBytesPerFFToSend / nBytesToCopy) * nBytesToCopy; + + // Encode + uint8_t * dst; + uint8_t * src_end; + + tu_fifo_buffer_info_t info; + + for (cnt_ff = 0; cnt_ff < n_ff_used; cnt_ff++) + { + dst = &audio->lin_buf_in[cnt_ff*audio->n_channels_per_ff_tx*audio->n_bytes_per_sampe_tx]; + + tu_fifo_get_read_info(&audio->tx_supp_ff[cnt_ff], &info); + + if (info.len_lin != 0) + { + info.len_lin = tu_min16(nBytesPerFFToSend, info.len_lin); // Limit up to desired length + src_end = (uint8_t *)info.ptr_lin + info.len_lin; + dst = audiod_interleaved_copy_bytes_fast_encode(audio->n_bytes_per_sampe_tx, info.ptr_lin, src_end, dst, n_ff_used); + + // Limit up to desired length + info.len_wrap = tu_min16(nBytesPerFFToSend - info.len_lin, info.len_wrap); + + // Handle wrapped part of FIFO + if (info.len_wrap != 0) + { + src_end = (uint8_t *)info.ptr_wrap + info.len_wrap; + audiod_interleaved_copy_bytes_fast_encode(audio->n_bytes_per_sampe_tx, info.ptr_wrap, src_end, dst, n_ff_used); + } + + tu_fifo_advance_read_pointer(&audio->tx_supp_ff[cnt_ff], info.len_lin + info.len_wrap); + } + } + + return nBytesPerFFToSend * n_ff_used; +} +#endif //CFG_TUD_AUDIO_ENABLE_ENCODING + +// This function is called once a transmit of a feedback packet was successfully completed. Here, we get the next feedback value to be sent + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP +static inline bool audiod_fb_send(uint8_t rhport, audiod_function_t *audio) +{ + return usbd_edpt_xfer(rhport, audio->ep_fb, (uint8_t *) &audio->fb_val, 4); +} +#endif + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void audiod_init(void) +{ + tu_memclr(_audiod_fct, sizeof(_audiod_fct)); + + for(uint8_t i=0; ictrl_buf = ctrl_buf_1; + audio->ctrl_buf_sz = CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ; + break; +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_CTRL_BUF_SZ > 0 + case 1: + audio->ctrl_buf = ctrl_buf_2; + audio->ctrl_buf_sz = CFG_TUD_AUDIO_FUNC_2_CTRL_BUF_SZ; + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_CTRL_BUF_SZ > 0 + case 2: + audio->ctrl_buf = ctrl_buf_3; + audio->ctrl_buf_sz = CFG_TUD_AUDIO_FUNC_3_CTRL_BUF_SZ; + break; +#endif + } + + // Initialize active alternate interface buffers + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_N_AS_INT > 0 + case 0: + audio->alt_setting = alt_setting_1; + break; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_N_AS_INT > 0 + case 1: + audio->alt_setting = alt_setting_2; + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_N_AS_INT > 0 + case 2: + audio->alt_setting = alt_setting_3; + break; +#endif + } + + // Initialize IN EP FIFO if required +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING + + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0 + case 0: + tu_fifo_config(&audio->ep_in_ff, audio_ep_in_sw_buf_1, CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&audio->ep_in_ff, osal_mutex_create(&ep_in_ff_mutex_wr_1), NULL); +#endif + break; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0 + case 1: + tu_fifo_config(&audio->ep_in_ff, audio_ep_in_sw_buf_2, CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&audio->ep_in_ff, osal_mutex_create(&ep_in_ff_mutex_wr_2), NULL); +#endif + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0 + case 2: + tu_fifo_config(&audio->ep_in_ff, audio_ep_in_sw_buf_3, CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&audio->ep_in_ff, osal_mutex_create(&ep_in_ff_mutex_wr_3), NULL); +#endif + break; +#endif + } +#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING + + // Initialize linear buffers +#if USE_LINEAR_BUFFER_TX + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX > 0 + case 0: + audio->lin_buf_in = lin_buf_in_1; + break; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX > 0 + case 1: + audio->lin_buf_in = lin_buf_in_2; + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX > 0 + case 2: + audio->lin_buf_in = lin_buf_in_3; + break; +#endif + } +#endif // USE_LINEAR_BUFFER_TX + + // Initialize OUT EP FIFO if required +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING + + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0 + case 0: + tu_fifo_config(&audio->ep_out_ff, audio_ep_out_sw_buf_1, CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&audio->ep_out_ff, NULL, osal_mutex_create(&ep_out_ff_mutex_rd_1)); +#endif + break; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0 + case 1: + tu_fifo_config(&audio->ep_out_ff, audio_ep_out_sw_buf_2, CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&audio->ep_out_ff, NULL, osal_mutex_create(&ep_out_ff_mutex_rd_2)); +#endif + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0 + case 2: + tu_fifo_config(&audio->ep_out_ff, audio_ep_out_sw_buf_3, CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&audio->ep_out_ff, NULL, osal_mutex_create(&ep_out_ff_mutex_rd_3)); +#endif + break; +#endif + } +#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING + + // Initialize linear buffers +#if USE_LINEAR_BUFFER_RX + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0 + case 0: + audio->lin_buf_out = lin_buf_out_1; + break; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0 + case 1: + audio->lin_buf_out = lin_buf_out_2; + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0 + case 2: + audio->lin_buf_out = lin_buf_out_3; + break; +#endif + } +#endif // USE_LINEAR_BUFFER_TX + + // Initialize TX support FIFOs if required +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING + + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0 + case 0: + audio->tx_supp_ff = tx_supp_ff_1; + audio->n_tx_supp_ff = CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO; + audio->tx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ; + for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO; cnt++) + { + tu_fifo_config(&tx_supp_ff_1[cnt], tx_supp_ff_buf_1[cnt], CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&tx_supp_ff_1[cnt], osal_mutex_create(&tx_supp_ff_mutex_wr_1[cnt]), NULL); +#endif + } + + break; +#endif // CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0 + +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0 + case 1: + audio->tx_supp_ff = tx_supp_ff_2; + audio->n_tx_supp_ff = CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO; + audio->tx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ; + for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO; cnt++) + { + tu_fifo_config(&tx_supp_ff_2[cnt], tx_supp_ff_buf_2[cnt], CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&tx_supp_ff_2[cnt], osal_mutex_create(&tx_supp_ff_mutex_wr_2[cnt]), NULL); +#endif + } + + break; +#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0 + +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0 + case 2: + audio->tx_supp_ff = tx_supp_ff_3; + audio->n_tx_supp_ff = CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO; + audio->tx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ; + for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO; cnt++) + { + tu_fifo_config(&tx_supp_ff_3[cnt], tx_supp_ff_buf_3[cnt], CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&tx_supp_ff_3[cnt], osal_mutex_create(&tx_supp_ff_mutex_wr_3[cnt]), NULL); +#endif + } + + break; +#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0 + } +#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING + + // Set encoding parameters for Type_I formats +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0 + case 0: + audio->n_channels_per_ff_tx = CFG_TUD_AUDIO_FUNC_1_CHANNEL_PER_FIFO_TX; + break; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0 + case 1: + audio->n_channels_per_ff_tx = CFG_TUD_AUDIO_FUNC_2_CHANNEL_PER_FIFO_TX; + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0 + case 2: + audio->n_channels_per_ff_tx = CFG_TUD_AUDIO_FUNC_3_CHANNEL_PER_FIFO_TX; + break; +#endif + } +#endif // CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING + + // Initialize RX support FIFOs if required +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING + + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0 + case 0: + audio->rx_supp_ff = rx_supp_ff_1; + audio->n_rx_supp_ff = CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO; + audio->rx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ; + for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO; cnt++) + { + tu_fifo_config(&rx_supp_ff_1[cnt], rx_supp_ff_buf_1[cnt], CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&rx_supp_ff_1[cnt], osal_mutex_create(&rx_supp_ff_mutex_rd_1[cnt]), NULL); +#endif + } + + break; +#endif // CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0 + +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0 + case 1: + audio->rx_supp_ff = rx_supp_ff_2; + audio->n_rx_supp_ff = CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO; + audio->rx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ; + for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO; cnt++) + { + tu_fifo_config(&rx_supp_ff_2[cnt], rx_supp_ff_buf_2[cnt], CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&rx_supp_ff_2[cnt], osal_mutex_create(&rx_supp_ff_mutex_rd_2[cnt]), NULL); +#endif + } + + break; +#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0 + +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0 + case 2: + audio->rx_supp_ff = rx_supp_ff_3; + audio->n_rx_supp_ff = CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO; + audio->rx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ; + for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO; cnt++) + { + tu_fifo_config(&rx_supp_ff_3[cnt], rx_supp_ff_buf_3[cnt], CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ, 1, true); +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&rx_supp_ff_3[cnt], osal_mutex_create(&rx_supp_ff_mutex_rd_3[cnt]), NULL); +#endif + } + + break; +#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0 + } +#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING + + // Set encoding parameters for Type_I formats +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING + switch (i) + { +#if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0 + case 0: + audio->n_channels_per_ff_rx = CFG_TUD_AUDIO_FUNC_1_CHANNEL_PER_FIFO_RX; + break; +#endif +#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0 + case 1: + audio->n_channels_per_ff_rx = CFG_TUD_AUDIO_FUNC_2_CHANNEL_PER_FIFO_RX; + break; +#endif +#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0 + case 2: + audio->n_channels_per_ff_rx = CFG_TUD_AUDIO_FUNC_3_CHANNEL_PER_FIFO_RX; + break; +#endif + } +#endif // CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING + } +} + +void audiod_reset(uint8_t rhport) +{ + (void) rhport; + + for(uint8_t i=0; iep_in_ff); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING + tu_fifo_clear(&audio->ep_out_ff); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING + for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++) + { + tu_fifo_clear(&audio->tx_supp_ff[cnt]); + } +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING + for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++) + { + tu_fifo_clear(&audio->rx_supp_ff[cnt]); + } +#endif + } +} + +uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + (void) max_len; + + TU_VERIFY ( TUSB_CLASS_AUDIO == itf_desc->bInterfaceClass && + AUDIO_SUBCLASS_CONTROL == itf_desc->bInterfaceSubClass); + + // Verify version is correct - this check can be omitted + TU_VERIFY(itf_desc->bInterfaceProtocol == AUDIO_INT_PROTOCOL_CODE_V2); + + // Verify interrupt control EP is enabled if demanded by descriptor - this should be best some static check however - this check can be omitted + if (itf_desc->bNumEndpoints == 1) // 0 or 1 EPs are allowed + { + TU_VERIFY(CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN > 0); + } + + // Alternate setting MUST be zero - this check can be omitted + TU_VERIFY(itf_desc->bAlternateSetting == 0); + + // Find available audio driver interface + uint8_t i; + for (i = 0; i < CFG_TUD_AUDIO; i++) + { + if (!_audiod_fct[i].p_desc) + { + _audiod_fct[i].p_desc = (uint8_t const *)itf_desc; // Save pointer to AC descriptor which is by specification always the first one + _audiod_fct[i].rhport = rhport; + + // Setup descriptor lengths + switch (i) + { + case 0: + _audiod_fct[i].desc_length = CFG_TUD_AUDIO_FUNC_1_DESC_LEN; + break; +#if CFG_TUD_AUDIO > 1 + case 1: + _audiod_fct[i].desc_length = CFG_TUD_AUDIO_FUNC_2_DESC_LEN; + break; +#endif +#if CFG_TUD_AUDIO > 2 + case 2: + _audiod_fct[i].desc_length = CFG_TUD_AUDIO_FUNC_3_DESC_LEN; + break; +#endif + } + + break; + } + } + + // Verify we found a free one + TU_ASSERT( i < CFG_TUD_AUDIO ); + + // This is all we need so far - the EPs are setup by a later set_interface request (as per UAC2 specification) + uint16_t drv_len = _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN; // - TUD_AUDIO_DESC_IAD_LEN since tinyUSB already handles the IAD descriptor + + return drv_len; +} + +static bool audiod_get_interface(uint8_t rhport, tusb_control_request_t const * p_request) +{ + uint8_t const itf = tu_u16_low(p_request->wIndex); + + // Find index of audio streaming interface + uint8_t func_id, idxItf; + uint8_t const *dummy; + + TU_VERIFY(audiod_get_AS_interface_index_global(itf, &func_id, &idxItf, &dummy)); + TU_VERIFY(tud_control_xfer(rhport, p_request, &_audiod_fct[func_id].alt_setting[idxItf], 1)); + + TU_LOG2(" Get itf: %u - current alt: %u\r\n", itf, _audiod_fct[func_id].alt_setting[idxItf]); + + return true; +} + +static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const * p_request) +{ + (void) rhport; + + // Here we need to do the following: + + // 1. Find the audio driver assigned to the given interface to be set + // Since one audio driver interface has to be able to cover an unknown number of interfaces (AC, AS + its alternate settings), the best memory efficient way to solve this is to always search through the descriptors. + // The audio driver is mapped to an audio function by a reference pointer to the corresponding AC interface of this audio function which serves as a starting point for searching + + // 2. Close EPs which are currently open + // To do so it is not necessary to know the current active alternate interface since we already save the current EP addresses - we simply close them + + // 3. Open new EP + + uint8_t const itf = tu_u16_low(p_request->wIndex); + uint8_t const alt = tu_u16_low(p_request->wValue); + + TU_LOG2(" Set itf: %u - alt: %u\r\n", itf, alt); + + // Find index of audio streaming interface and index of interface + uint8_t func_id, idxItf; + uint8_t const *p_desc; + TU_VERIFY(audiod_get_AS_interface_index_global(itf, &func_id, &idxItf, &p_desc)); + + audiod_function_t* audio = &_audiod_fct[func_id]; + + // Look if there is an EP to be closed - for this driver, there are only 3 possible EPs which may be closed (only AS related EPs can be closed, AC EP (if present) is always open) +#if CFG_TUD_AUDIO_ENABLE_EP_IN + if (audio->ep_in_as_intf_num == itf) + { + audio->ep_in_as_intf_num = 0; + usbd_edpt_close(rhport, audio->ep_in); + + // Clear FIFOs, since data is no longer valid +#if !CFG_TUD_AUDIO_ENABLE_ENCODING + tu_fifo_clear(&audio->ep_in_ff); +#else + for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++) + { + tu_fifo_clear(&audio->tx_supp_ff[cnt]); + } +#endif + + // Invoke callback - can be used to stop data sampling + if (tud_audio_set_itf_close_EP_cb) TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request)); + + audio->ep_in = 0; // Necessary? + + } +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + if (audio->ep_out_as_intf_num == itf) + { + audio->ep_out_as_intf_num = 0; + usbd_edpt_close(rhport, audio->ep_out); + + // Clear FIFOs, since data is no longer valid +#if !CFG_TUD_AUDIO_ENABLE_DECODING + tu_fifo_clear(&audio->ep_out_ff); +#else + for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++) + { + tu_fifo_clear(&audio->rx_supp_ff[cnt]); + } +#endif + + // Invoke callback - can be used to stop data sampling + if (tud_audio_set_itf_close_EP_cb) TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request)); + + audio->ep_out = 0; // Necessary? + + // Close corresponding feedback EP +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + usbd_edpt_close(rhport, audio->ep_fb); + audio->ep_fb = 0; // Necessary? +#endif + } +#endif + + // Save current alternative interface setting + audio->alt_setting[idxItf] = alt; + + // Open new EP if necessary - EPs are only to be closed or opened for AS interfaces - Look for AS interface with correct alternate interface + // Get pointer at end + uint8_t const *p_desc_end = audio->p_desc + audio->desc_length - TUD_AUDIO_DESC_IAD_LEN; + + // p_desc starts at required interface with alternate setting zero + while (p_desc < p_desc_end) + { + // Find correct interface + if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const * )p_desc)->bInterfaceNumber == itf && ((tusb_desc_interface_t const * )p_desc)->bAlternateSetting == alt) + { +#if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_ENABLE_DECODING + uint8_t const * p_desc_parse_for_params = p_desc; +#endif + // From this point forward follow the EP descriptors associated to the current alternate setting interface - Open EPs if necessary + uint8_t foundEPs = 0, nEps = ((tusb_desc_interface_t const * )p_desc)->bNumEndpoints; + while (foundEPs < nEps && p_desc < p_desc_end) + { + if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT) + { + tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const *) p_desc; + TU_ASSERT(usbd_edpt_open(rhport, desc_ep)); + + uint8_t const ep_addr = desc_ep->bEndpointAddress; + + //TODO: We need to set EP non busy since this is not taken care of right now in ep_close() - THIS IS A WORKAROUND! + usbd_edpt_clear_stall(rhport, ep_addr); + +#if CFG_TUD_AUDIO_ENABLE_EP_IN + if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.usage == 0x00) // Check if usage is data EP + { + // Save address + audio->ep_in = ep_addr; + audio->ep_in_as_intf_num = itf; + audio->ep_in_sz = tu_edpt_packet_size(desc_ep); + + // If software encoding is enabled, parse for the corresponding parameters - doing this here means only AS interfaces with EPs get scanned for parameters +#if CFG_TUD_AUDIO_ENABLE_ENCODING + audiod_parse_for_AS_params(audio, p_desc_parse_for_params, p_desc_end, itf); + + // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING + const uint16_t active_fifo_depth = (audio->tx_supp_ff_sz_max / audio->n_bytes_per_sampe_tx) * audio->n_bytes_per_sampe_tx; + for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++) + { + tu_fifo_config(&audio->tx_supp_ff[cnt], audio->tx_supp_ff[cnt].buffer, active_fifo_depth, 1, true); + } + audio->n_ff_used_tx = audio->n_channels_tx / audio->n_channels_per_ff_tx; + TU_ASSERT( audio->n_ff_used_tx <= audio->n_tx_supp_ff ); +#endif + +#endif + // Invoke callback - can be used to trigger data sampling if not already running + if (tud_audio_set_itf_cb) TU_VERIFY(tud_audio_set_itf_cb(rhport, p_request)); + + // Schedule first transmit if alternate interface is not zero i.e. streaming is disabled - in case no sample data is available a ZLP is loaded + // It is necessary to trigger this here since the refill is done with an RX FIFO empty interrupt which can only trigger if something was in there + TU_VERIFY(audiod_tx_done_cb(rhport, &_audiod_fct[func_id])); + } +#endif // CFG_TUD_AUDIO_ENABLE_EP_IN + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + + if (tu_edpt_dir(ep_addr) == TUSB_DIR_OUT) // Checking usage not necessary + { + // Save address + audio->ep_out = ep_addr; + audio->ep_out_as_intf_num = itf; + audio->ep_out_sz = tu_edpt_packet_size(desc_ep); + +#if CFG_TUD_AUDIO_ENABLE_DECODING + audiod_parse_for_AS_params(audio, p_desc_parse_for_params, p_desc_end, itf); + + // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING + const uint16_t active_fifo_depth = (audio->rx_supp_ff_sz_max / audio->n_bytes_per_sampe_rx) * audio->n_bytes_per_sampe_rx; + for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++) + { + tu_fifo_config(&audio->rx_supp_ff[cnt], audio->rx_supp_ff[cnt].buffer, active_fifo_depth, 1, true); + } + audio->n_ff_used_rx = audio->n_channels_rx / audio->n_channels_per_ff_rx; + TU_ASSERT( audio->n_ff_used_rx <= audio->n_rx_supp_ff ); +#endif +#endif + +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + // In case of asynchronous EP, call Cb after ep_fb is set + if ( !(desc_ep->bmAttributes.sync == 0x01 && audio->ep_fb == 0) ) + { + if (tud_audio_set_itf_cb) TU_VERIFY(tud_audio_set_itf_cb(rhport, p_request)); + } +#else + // Invoke callback + if (tud_audio_set_itf_cb) TU_VERIFY(tud_audio_set_itf_cb(rhport, p_request)); +#endif + // Prepare for incoming data +#if USE_LINEAR_BUFFER_RX + TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_out, audio->lin_buf_out, audio->ep_out_sz), false); +#else + TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_out, &audio->ep_out_ff, audio->ep_out_sz), false); +#endif + } + +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.usage == 1) // Check if usage is explicit data feedback + { + audio->ep_fb = ep_addr; + + // Invoke callback after ep_out is set + if (audio->ep_out != 0) + { + if (tud_audio_set_itf_cb) TU_VERIFY(tud_audio_set_itf_cb(rhport, p_request)); + } + } +#endif +#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT + + foundEPs += 1; + } + p_desc = tu_desc_next(p_desc); + } + + TU_VERIFY(foundEPs == nEps); + + // We are done - abort loop + break; + } + + // Moving forward + p_desc = tu_desc_next(p_desc); + } + + tud_control_status(rhport, p_request); + + return true; +} + +// Invoked when class request DATA stage is finished. +// return false to stall control EP (e.g Host send non-sense DATA) +static bool audiod_control_complete(uint8_t rhport, tusb_control_request_t const * p_request) +{ + // Handle audio class specific set requests + if(p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS && p_request->bmRequestType_bit.direction == TUSB_DIR_OUT) + { + uint8_t func_id; + + switch (p_request->bmRequestType_bit.recipient) + { + case TUSB_REQ_RCPT_INTERFACE: + { + uint8_t itf = TU_U16_LOW(p_request->wIndex); + uint8_t entityID = TU_U16_HIGH(p_request->wIndex); + + if (entityID != 0) + { + if (tud_audio_set_req_entity_cb) + { + // Check if entity is present and get corresponding driver index + TU_VERIFY(audiod_verify_entity_exists(itf, entityID, &func_id)); + + // Invoke callback + return tud_audio_set_req_entity_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf); + } + else + { + TU_LOG2(" No entity set request callback available!\r\n"); + return false; // In case no callback function is present or request can not be conducted we stall it + } + } + else + { + if (tud_audio_set_req_itf_cb) + { + // Find index of audio driver structure and verify interface really exists + TU_VERIFY(audiod_verify_itf_exists(itf, &func_id)); + + // Invoke callback + return tud_audio_set_req_itf_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf); + } + else + { + TU_LOG2(" No interface set request callback available!\r\n"); + return false; // In case no callback function is present or request can not be conducted we stall it + } + } + } + break; + + case TUSB_REQ_RCPT_ENDPOINT: + { + uint8_t ep = TU_U16_LOW(p_request->wIndex); + + if (tud_audio_set_req_ep_cb) + { + // Check if entity is present and get corresponding driver index + TU_VERIFY(audiod_verify_ep_exists(ep, &func_id)); + + // Invoke callback + return tud_audio_set_req_ep_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf); + } + else + { + TU_LOG2(" No EP set request callback available!\r\n"); + return false; // In case no callback function is present or request can not be conducted we stall it + } + } + break; + // Unknown/Unsupported recipient + default: TU_BREAKPOINT(); return false; + } + } + return true; +} + +// Handle class control request +// return false to stall control endpoint (e.g unsupported request) +static bool audiod_control_request(uint8_t rhport, tusb_control_request_t const * p_request) +{ + (void) rhport; + + // Handle standard requests - standard set requests usually have no data stage so we also handle set requests here + if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) + { + switch (p_request->bRequest) + { + case TUSB_REQ_GET_INTERFACE: + return audiod_get_interface(rhport, p_request); + + case TUSB_REQ_SET_INTERFACE: + return audiod_set_interface(rhport, p_request); + + // Unknown/Unsupported request + default: TU_BREAKPOINT(); return false; + } + } + + // Handle class requests + if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS) + { + uint8_t itf = TU_U16_LOW(p_request->wIndex); + uint8_t func_id; + + // Conduct checks which depend on the recipient + switch (p_request->bmRequestType_bit.recipient) + { + case TUSB_REQ_RCPT_INTERFACE: + { + uint8_t entityID = TU_U16_HIGH(p_request->wIndex); + + // Verify if entity is present + if (entityID != 0) + { + // Find index of audio driver structure and verify entity really exists + TU_VERIFY(audiod_verify_entity_exists(itf, entityID, &func_id)); + + // In case we got a get request invoke callback - callback needs to answer as defined in UAC2 specification page 89 - 5. Requests + if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN) + { + if (tud_audio_get_req_entity_cb) + { + return tud_audio_get_req_entity_cb(rhport, p_request); + } + else + { + TU_LOG2(" No entity get request callback available!\r\n"); + return false; // Stall + } + } + } + else + { + // Find index of audio driver structure and verify interface really exists + TU_VERIFY(audiod_verify_itf_exists(itf, &func_id)); + + // In case we got a get request invoke callback - callback needs to answer as defined in UAC2 specification page 89 - 5. Requests + if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN) + { + if (tud_audio_get_req_itf_cb) + { + return tud_audio_get_req_itf_cb(rhport, p_request); + } + else + { + TU_LOG2(" No interface get request callback available!\r\n"); + return false; // Stall + } + } + } + } + break; + + case TUSB_REQ_RCPT_ENDPOINT: + { + uint8_t ep = TU_U16_LOW(p_request->wIndex); + + // Find index of audio driver structure and verify EP really exists + TU_VERIFY(audiod_verify_ep_exists(ep, &func_id)); + + // In case we got a get request invoke callback - callback needs to answer as defined in UAC2 specification page 89 - 5. Requests + if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN) + { + if (tud_audio_get_req_ep_cb) + { + return tud_audio_get_req_ep_cb(rhport, p_request); + } + else + { + TU_LOG2(" No EP get request callback available!\r\n"); + return false; // Stall + } + } + } + break; + + // Unknown/Unsupported recipient + default: TU_LOG2(" Unsupported recipient: %d\r\n", p_request->bmRequestType_bit.recipient); TU_BREAKPOINT(); return false; + } + + // If we end here, the received request is a set request - we schedule a receive for the data stage and return true here. We handle the rest later in audiod_control_complete() once the data stage was finished + TU_VERIFY(tud_control_xfer(rhport, p_request, _audiod_fct[func_id].ctrl_buf, _audiod_fct[func_id].ctrl_buf_sz)); + return true; + } + + // There went something wrong - unsupported control request type + TU_BREAKPOINT(); + return false; +} + +bool audiod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + if ( stage == CONTROL_STAGE_SETUP ) + { + return audiod_control_request(rhport, request); + } + else if ( stage == CONTROL_STAGE_DATA ) + { + return audiod_control_complete(rhport, request); + } + + return true; +} + +bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) result; + (void) xferred_bytes; + + // Search for interface belonging to given end point address and proceed as required + uint8_t func_id; + for (func_id = 0; func_id < CFG_TUD_AUDIO; func_id++) + { + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN + + // Data transmission of control interrupt finished + if (_audiod_fct[func_id].ep_int_ctr == ep_addr) + { + // According to USB2 specification, maximum payload of interrupt EP is 8 bytes on low speed, 64 bytes on full speed, and 1024 bytes on high speed (but only if an alternate interface other than 0 is used - see specification p. 49) + // In case there is nothing to send we have to return a NAK - this is taken care of by PHY ??? + // In case of an erroneous transmission a retransmission is conducted - this is taken care of by PHY ??? + + // I assume here, that things above are handled by PHY + // All transmission is done - what remains to do is to inform job was completed + + if (tud_audio_int_ctr_done_cb) TU_VERIFY(tud_audio_int_ctr_done_cb(rhport, (uint16_t) xferred_bytes)); + } + +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN + + // Data transmission of audio packet finished + if (_audiod_fct[func_id].ep_in == ep_addr && _audiod_fct[func_id].alt_setting != 0) + { + // USB 2.0, section 5.6.4, third paragraph, states "An isochronous endpoint must specify its required bus access period. However, an isochronous endpoint must be prepared to handle poll rates faster than the one specified." + // That paragraph goes on to say "An isochronous IN endpoint must return a zero-length packet whenever data is requested at a faster interval than the specified interval and data is not available." + // This can only be solved reliably if we load a ZLP after every IN transmission since we can not say if the host requests samples earlier than we declared! Once all samples are collected we overwrite the loaded ZLP. + + // Check if there is data to load into EPs buffer - if not load it with ZLP + // Be aware - we as a device are not able to know if the host polls for data with a faster rate as we stated this in the descriptors. Therefore we always have to put something into the EPs buffer. However, once we did that, there is no way of aborting this or replacing what we put into the buffer before! + // This is the only place where we can fill something into the EPs buffer! + + // Load new data + TU_VERIFY(audiod_tx_done_cb(rhport, &_audiod_fct[func_id])); + + // Transmission of ZLP is done by audiod_tx_done_cb() + return true; + } +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + + // New audio packet received + if (_audiod_fct[func_id].ep_out == ep_addr) + { + TU_VERIFY(audiod_rx_done_cb(rhport, &_audiod_fct[func_id], (uint16_t) xferred_bytes)); + return true; + } + + +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + // Transmission of feedback EP finished + if (_audiod_fct[func_id].ep_fb == ep_addr) + { + if (tud_audio_fb_done_cb) TU_VERIFY(tud_audio_fb_done_cb(rhport)); + + // Schedule a transmit with the new value if EP is not busy + if (!usbd_edpt_busy(rhport, _audiod_fct[func_id].ep_fb)) + { + // Schedule next transmission - value is changed bytud_audio_n_fb_set() in the meantime or the old value gets sent + return audiod_fb_send(rhport, &_audiod_fct[func_id]); + } + } +#endif +#endif + } + + return false; +} + +bool tud_audio_buffer_and_schedule_control_xfer(uint8_t rhport, tusb_control_request_t const * p_request, void* data, uint16_t len) +{ + // Handles only sending of data not receiving + if (p_request->bmRequestType_bit.direction == TUSB_DIR_OUT) return false; + + // Get corresponding driver index + uint8_t func_id; + uint8_t itf = TU_U16_LOW(p_request->wIndex); + + // Conduct checks which depend on the recipient + switch (p_request->bmRequestType_bit.recipient) + { + case TUSB_REQ_RCPT_INTERFACE: + { + uint8_t entityID = TU_U16_HIGH(p_request->wIndex); + + // Verify if entity is present + if (entityID != 0) + { + // Find index of audio driver structure and verify entity really exists + TU_VERIFY(audiod_verify_entity_exists(itf, entityID, &func_id)); + } + else + { + // Find index of audio driver structure and verify interface really exists + TU_VERIFY(audiod_verify_itf_exists(itf, &func_id)); + } + } + break; + + case TUSB_REQ_RCPT_ENDPOINT: + { + uint8_t ep = TU_U16_LOW(p_request->wIndex); + + // Find index of audio driver structure and verify EP really exists + TU_VERIFY(audiod_verify_ep_exists(ep, &func_id)); + } + break; + + // Unknown/Unsupported recipient + default: TU_LOG2(" Unsupported recipient: %d\r\n", p_request->bmRequestType_bit.recipient); TU_BREAKPOINT(); return false; + } + + // Crop length + if (len > _audiod_fct[func_id].ctrl_buf_sz) len = _audiod_fct[func_id].ctrl_buf_sz; + + // Copy into buffer + memcpy((void *)_audiod_fct[func_id].ctrl_buf, data, (size_t)len); + + // Schedule transmit + return tud_control_xfer(rhport, p_request, (void*)_audiod_fct[func_id].ctrl_buf, len); +} + +// This helper function finds for a given audio function and AS interface number the index of the attached driver structure, the index of the interface in the audio function +// (e.g. the std. AS interface with interface number 15 is the first AS interface for the given audio function and thus gets index zero), and +// finally a pointer to the std. AS interface, where the pointer always points to the first alternate setting i.e. alternate interface zero. +static bool audiod_get_AS_interface_index(uint8_t itf, audiod_function_t * audio, uint8_t *idxItf, uint8_t const **pp_desc_int) +{ + if (audio->p_desc) + { + // Get pointer at end + uint8_t const *p_desc_end = audio->p_desc + audio->desc_length - TUD_AUDIO_DESC_IAD_LEN; + + // Advance past AC descriptors + uint8_t const *p_desc = tu_desc_next(audio->p_desc); + p_desc += ((audio_desc_cs_ac_interface_t const *)p_desc)->wTotalLength; + + uint8_t tmp = 0; + while (p_desc < p_desc_end) + { + // We assume the number of alternate settings is increasing thus we return the index of alternate setting zero! + if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const * )p_desc)->bAlternateSetting == 0) + { + if (((tusb_desc_interface_t const * )p_desc)->bInterfaceNumber == itf) + { + *idxItf = tmp; + *pp_desc_int = p_desc; + return true; + } + // Increase index, bytes read, and pointer + tmp++; + } + p_desc = tu_desc_next(p_desc); + } + } + return false; +} + +// This helper function finds for a given AS interface number the index of the attached driver structure, the index of the interface in the audio function +// (e.g. the std. AS interface with interface number 15 is the first AS interface for the given audio function and thus gets index zero), and +// finally a pointer to the std. AS interface, where the pointer always points to the first alternate setting i.e. alternate interface zero. +static bool audiod_get_AS_interface_index_global(uint8_t itf, uint8_t *func_id, uint8_t *idxItf, uint8_t const **pp_desc_int) +{ + // Loop over audio driver interfaces + uint8_t i; + for (i = 0; i < CFG_TUD_AUDIO; i++) + { + if (audiod_get_AS_interface_index(itf, &_audiod_fct[i], idxItf, pp_desc_int)) + { + *func_id = i; + return true; + } + } + + return false; +} + +// Verify an entity with the given ID exists and returns also the corresponding driver index +static bool audiod_verify_entity_exists(uint8_t itf, uint8_t entityID, uint8_t *func_id) +{ + uint8_t i; + for (i = 0; i < CFG_TUD_AUDIO; i++) + { + // Look for the correct driver by checking if the unique standard AC interface number fits + if (_audiod_fct[i].p_desc && ((tusb_desc_interface_t const *)_audiod_fct[i].p_desc)->bInterfaceNumber == itf) + { + // Get pointers after class specific AC descriptors and end of AC descriptors - entities are defined in between + uint8_t const *p_desc = tu_desc_next(_audiod_fct[i].p_desc); // Points to CS AC descriptor + uint8_t const *p_desc_end = ((audio_desc_cs_ac_interface_t const *)p_desc)->wTotalLength + p_desc; + p_desc = tu_desc_next(p_desc); // Get past CS AC descriptor + + while (p_desc < p_desc_end) + { + if (p_desc[3] == entityID) // Entity IDs are always at offset 3 + { + *func_id = i; + return true; + } + p_desc = tu_desc_next(p_desc); + } + } + } + return false; +} + +static bool audiod_verify_itf_exists(uint8_t itf, uint8_t *func_id) +{ + uint8_t i; + for (i = 0; i < CFG_TUD_AUDIO; i++) + { + if (_audiod_fct[i].p_desc) + { + // Get pointer at beginning and end + uint8_t const *p_desc = _audiod_fct[i].p_desc; + uint8_t const *p_desc_end = _audiod_fct[i].p_desc + _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN; + + while (p_desc < p_desc_end) + { + if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const *)_audiod_fct[i].p_desc)->bInterfaceNumber == itf) + { + *func_id = i; + return true; + } + p_desc = tu_desc_next(p_desc); + } + } + } + return false; +} + +static bool audiod_verify_ep_exists(uint8_t ep, uint8_t *func_id) +{ + uint8_t i; + for (i = 0; i < CFG_TUD_AUDIO; i++) + { + if (_audiod_fct[i].p_desc) + { + // Get pointer at end + uint8_t const *p_desc_end = _audiod_fct[i].p_desc + _audiod_fct[i].desc_length; + + // Advance past AC descriptors - EP we look for are streaming EPs + uint8_t const *p_desc = tu_desc_next(_audiod_fct[i].p_desc); + p_desc += ((audio_desc_cs_ac_interface_t const *)p_desc)->wTotalLength; + + while (p_desc < p_desc_end) + { + if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT && ((tusb_desc_endpoint_t const * )p_desc)->bEndpointAddress == ep) + { + *func_id = i; + return true; + } + p_desc = tu_desc_next(p_desc); + } + } + } + return false; +} + +#if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_ENABLE_DECODING +// p_desc points to the AS interface of alternate setting zero +// itf is the interface number of the corresponding interface - we check if the interface belongs to EP in or EP out to see if it is a TX or RX parameter +// Currently, only AS interfaces with an EP (in or out) are supposed to be parsed for! +static void audiod_parse_for_AS_params(audiod_function_t* audio, uint8_t const * p_desc, uint8_t const * p_desc_end, uint8_t const as_itf) +{ + p_desc = tu_desc_next(p_desc); // Exclude standard AS interface descriptor of current alternate interface descriptor + + while (p_desc < p_desc_end) + { + // Abort if follow up descriptor is a new standard interface descriptor - indicates the last AS descriptor was already finished + if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE) break; + + // Look for a Class-Specific AS Interface Descriptor(4.9.2) to verify format type and format and also to get number of physical channels + if (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && tu_desc_subtype(p_desc) == AUDIO_CS_AS_INTERFACE_AS_GENERAL) + { +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf != audio->ep_in_as_intf_num && as_itf != audio->ep_out_as_intf_num) break; // Abort loop, this interface has no EP, this driver does not support this currently +#endif +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf != audio->ep_in_as_intf_num) break; +#endif +#if !CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf != audio->ep_out_as_intf_num) break; +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN + if (as_itf == audio->ep_in_as_intf_num) + { + audio->n_channels_tx = ((audio_desc_cs_as_interface_t const * )p_desc)->bNrChannels; + audio->format_type_tx = (audio_format_type_t)(((audio_desc_cs_as_interface_t const * )p_desc)->bFormatType); + +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING + audio->format_type_I_tx = (audio_data_format_type_I_t)(((audio_desc_cs_as_interface_t const * )p_desc)->bmFormats); +#endif + } +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf == audio->ep_out_as_intf_num) + { + audio->n_channels_rx = ((audio_desc_cs_as_interface_t const * )p_desc)->bNrChannels; + audio->format_type_rx = ((audio_desc_cs_as_interface_t const * )p_desc)->bFormatType; +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING + audio->format_type_I_rx = ((audio_desc_cs_as_interface_t const * )p_desc)->bmFormats; +#endif + } +#endif + } + + // Look for a Type I Format Type Descriptor(2.3.1.6 - Audio Formats) +#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING || CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING + if (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && tu_desc_subtype(p_desc) == AUDIO_CS_AS_INTERFACE_FORMAT_TYPE && ((audio_desc_type_I_format_t const * )p_desc)->bFormatType == AUDIO_FORMAT_TYPE_I) + { +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf != audio->ep_in_as_intf_num && as_itf != audio->ep_out_as_intf_num) break; // Abort loop, this interface has no EP, this driver does not support this currently +#endif +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf != audio->ep_in_as_intf_num) break; +#endif +#if !CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf != audio->ep_out_as_intf_num) break; +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN + if (as_itf == audio->ep_in_as_intf_num) + { + audio->n_bytes_per_sampe_tx = ((audio_desc_type_I_format_t const * )p_desc)->bSubslotSize; + } +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT + if (as_itf == audio->ep_out_as_intf_num) + { + audio->n_bytes_per_sampe_rx = ((audio_desc_type_I_format_t const * )p_desc)->bSubslotSize; + } +#endif + } +#endif + + // Other format types are not supported yet + + p_desc = tu_desc_next(p_desc); + } +} +#endif + +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP + +bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback) +{ + TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL); + + // Format the feedback value +#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION + if ( TUSB_SPEED_FULL == tud_speed_get() ) + { + uint8_t * fb = (uint8_t *) &_audiod_fct[func_id].fb_val; + + // For FS format is 10.14 + *(fb++) = (feedback >> 2) & 0xFF; + *(fb++) = (feedback >> 10) & 0xFF; + *(fb++) = (feedback >> 18) & 0xFF; + // 4th byte is needed to work correctly with MS Windows + *fb = 0; + }else +#else + { + // Send value as-is, caller will choose the appropriate format + _audiod_fct[func_id].fb_val = feedback; + } +#endif + + // Schedule a transmit with the new value if EP is not busy - this triggers repetitive scheduling of the feedback value + if (!usbd_edpt_busy(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_fb)) + { + return audiod_fb_send(_audiod_fct[func_id].rhport, &_audiod_fct[func_id]); + } + + return true; +} +#endif + +// No security checks here - internal function only which should always succeed +uint8_t audiod_get_audio_fct_idx(audiod_function_t * audio) +{ + for (uint8_t cnt=0; cnt < CFG_TUD_AUDIO; cnt++) + { + if (&_audiod_fct[cnt] == audio) return cnt; + } + return 0; +} + +#endif //TUSB_OPT_DEVICE_ENABLED && CFG_TUD_AUDIO diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio_device.h new file mode 100644 index 000000000..f406cf281 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/audio/audio_device.h @@ -0,0 +1,637 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Ha Thach (tinyusb.org) + * Copyright (c) 2020 Reinhard Panhuber + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_AUDIO_DEVICE_H_ +#define _TUSB_AUDIO_DEVICE_H_ + +#include "audio.h" + +//--------------------------------------------------------------------+ +// Class Driver Configuration +//--------------------------------------------------------------------+ + +// All sizes are in bytes! + +#ifndef CFG_TUD_AUDIO_FUNC_1_DESC_LEN +#error You must tell the driver the length of the audio function descriptor including IAD descriptor +#endif +#if CFG_TUD_AUDIO > 1 +#ifndef CFG_TUD_AUDIO_FUNC_2_DESC_LEN +#error You must tell the driver the length of the audio function descriptor including IAD descriptor +#endif +#endif +#if CFG_TUD_AUDIO > 2 +#ifndef CFG_TUD_AUDIO_FUNC_3_DESC_LEN +#error You must tell the driver the length of the audio function descriptor including IAD descriptor +#endif +#endif + +// Number of Standard AS Interface Descriptors (4.9.1) defined per audio function - this is required to be able to remember the current alternate settings of these interfaces +#ifndef CFG_TUD_AUDIO_FUNC_1_N_AS_INT +#error You must tell the driver the number of Standard AS Interface Descriptors you have defined in the audio function descriptor! +#endif +#if CFG_TUD_AUDIO > 1 +#ifndef CFG_TUD_AUDIO_FUNC_2_N_AS_INT +#error You must tell the driver the number of Standard AS Interface Descriptors you have defined in the audio function descriptor! +#endif +#endif +#if CFG_TUD_AUDIO > 2 +#ifndef CFG_TUD_AUDIO_FUNC_3_N_AS_INT +#error You must tell the driver the number of Standard AS Interface Descriptors you have defined in the audio function descriptor! +#endif +#endif + +// Size of control buffer used to receive and send control messages via EP0 - has to be big enough to hold your biggest request structure e.g. range requests with multiple intervals defined or cluster descriptors +#ifndef CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ +#error You must define an audio class control request buffer size! +#endif + +#if CFG_TUD_AUDIO > 1 +#ifndef CFG_TUD_AUDIO_FUNC_2_CTRL_BUF_SZ +#error You must define an audio class control request buffer size! +#endif +#endif + +#if CFG_TUD_AUDIO > 2 +#ifndef CFG_TUD_AUDIO_FUNC_3_CTRL_BUF_SZ +#error You must define an audio class control request buffer size! +#endif +#endif + +// End point sizes IN BYTES - Limits: Full Speed <= 1023, High Speed <= 1024 +#ifndef CFG_TUD_AUDIO_ENABLE_EP_IN +#define CFG_TUD_AUDIO_ENABLE_EP_IN 0 // TX +#endif + +#ifndef CFG_TUD_AUDIO_ENABLE_EP_OUT +#define CFG_TUD_AUDIO_ENABLE_EP_OUT 0 // RX +#endif + +// Maximum EP sizes for all alternate AS interface settings - used for checks and buffer allocation +#if CFG_TUD_AUDIO_ENABLE_EP_IN +#ifndef CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX +#error You must tell the driver the biggest EP IN size! +#endif +#if CFG_TUD_AUDIO > 1 +#ifndef CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX +#error You must tell the driver the biggest EP IN size! +#endif +#endif +#if CFG_TUD_AUDIO > 2 +#ifndef CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX +#error You must tell the driver the biggest EP IN size! +#endif +#endif +#endif // CFG_TUD_AUDIO_ENABLE_EP_IN + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT +#ifndef CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX +#error You must tell the driver the biggest EP OUT size! +#endif +#if CFG_TUD_AUDIO > 1 +#ifndef CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX +#error You must tell the driver the biggest EP OUT size! +#endif +#endif +#if CFG_TUD_AUDIO > 2 +#ifndef CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX +#error You must tell the driver the biggest EP OUT size! +#endif +#endif +#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT + +// Software EP FIFO buffer sizes - must be >= max EP SIZEs! +#ifndef CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ +#define CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ +#define CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ +#define CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ 0 +#endif + +#ifndef CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ +#define CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ +#define CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ +#define CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ 0 +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN +#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ < CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX +#error EP software buffer size MUST BE at least as big as maximum EP size +#endif + +#if CFG_TUD_AUDIO > 1 +#if CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ < CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX +#error EP software buffer size MUST BE at least as big as maximum EP size +#endif +#endif + +#if CFG_TUD_AUDIO > 2 +#if CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ < CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX +#error EP software buffer size MUST BE at least as big as maximum EP size +#endif +#endif +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT +#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ < CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX +#error EP software buffer size MUST BE at least as big as maximum EP size +#endif + +#if CFG_TUD_AUDIO > 1 +#if CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ < CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX +#error EP software buffer size MUST BE at least as big as maximum EP size +#endif +#endif + +#if CFG_TUD_AUDIO > 2 +#if CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ < CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX +#error EP software buffer size MUST BE at least as big as maximum EP size +#endif +#endif +#endif + +// Enable/disable feedback EP (required for asynchronous RX applications) +#ifndef CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP +#define CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP 0 // Feedback - 0 or 1 +#endif + +// Enable/disable conversion from 16.16 to 10.14 format on full-speed devices. See tud_audio_n_fb_set(). +#ifndef CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION +#define CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION 0 // 0 or 1 +#endif + +// Audio interrupt control EP size - disabled if 0 +#ifndef CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN +#define CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN 0 // Audio interrupt control - if required - 6 Bytes according to UAC 2 specification (p. 74) +#endif + +#ifndef CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE +#define CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE 6 // Buffer size of audio control interrupt EP - 6 Bytes according to UAC 2 specification (p. 74) +#endif + +// Use software encoding/decoding + +// The software coding feature of the driver is not mandatory. It is useful if, for instance, you have two I2S streams which need to be interleaved +// into a single PCM stream as SAMPLE_1 | SAMPLE_2 | SAMPLE_3 | SAMPLE_4. +// +// Currently, only PCM type I encoding/decoding is supported! +// +// If the coding feature is to be used, support FIFOs need to be configured. Their sizes and numbers are defined below. + +// Encoding/decoding is done in software and thus time consuming. If you can encode/decode your stream more efficiently do not use the +// support FIFOs but write/read directly into/from the EP_X_SW_BUFFER_FIFOs using +// - tud_audio_n_write() or +// - tud_audio_n_read(). +// To write/read to/from the support FIFOs use +// - tud_audio_n_write_support_ff() or +// - tud_audio_n_read_support_ff(). +// +// The encoding/decoding format type done is defined below. +// +// The encoding/decoding starts when the private callback functions +// - audio_tx_done_cb() +// - audio_rx_done_cb() +// are invoked. If support FIFOs are used, the corresponding encoding/decoding functions are called from there. +// Once encoding/decoding is done the result is put directly into the EP_X_SW_BUFFER_FIFOs. You can use the public callback functions +// - tud_audio_tx_done_pre_load_cb() or tud_audio_tx_done_post_load_cb() +// - tud_audio_rx_done_pre_read_cb() or tud_audio_rx_done_post_read_cb() +// if you want to get informed what happened. +// +// If you don't use the support FIFOs you may use the public callback functions +// - tud_audio_tx_done_pre_load_cb() or tud_audio_tx_done_post_load_cb() +// - tud_audio_rx_done_pre_read_cb() or tud_audio_rx_done_post_read_cb() +// to write/read from/into the EP_X_SW_BUFFER_FIFOs at the right time. +// +// If you need a different encoding which is not support so far implement it in the +// - audio_tx_done_cb() +// - audio_rx_done_cb() +// functions. + +// Enable encoding/decodings - for these to work, support FIFOs need to be setup in appropriate numbers and size +// The actual coding parameters of active AS alternate interface is parsed from the descriptors + +// The item size of the FIFO is always fixed to one i.e. bytes! Furthermore, the actively used FIFO depth is reconfigured such that the depth is a multiple of the current sample size in order to avoid samples to get split up in case of a wrap in the FIFO ring buffer (depth = (max_depth / sampe_sz) * sampe_sz)! +// This is important to remind in case you use DMAs! If the sample sizes changes, the DMA MUST BE RECONFIGURED just like the FIFOs for a different depth!!! + +// For PCM encoding/decoding + +#ifndef CFG_TUD_AUDIO_ENABLE_ENCODING +#define CFG_TUD_AUDIO_ENABLE_ENCODING 0 +#endif + +#ifndef CFG_TUD_AUDIO_ENABLE_DECODING +#define CFG_TUD_AUDIO_ENABLE_DECODING 0 +#endif + +// This enabling allows to save the current coding parameters e.g. # of bytes per sample etc. - TYPE_I includes common PCM encoding +#ifndef CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING +#define CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING 0 +#endif + +#ifndef CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING +#define CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING 0 +#endif + +// Type I Coding parameters not given within UAC2 descriptors +// It would be possible to allow for a more flexible setting and not fix this parameter as done below. However, this is most often not needed and kept for later if really necessary. The more flexible setting could be implemented within set_interface(), however, how the values are saved per alternate setting is to be determined! +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING +#ifndef CFG_TUD_AUDIO_FUNC_1_CHANNEL_PER_FIFO_TX +#error You must tell the driver the number of channels per FIFO for the interleaved encoding! E.g. for an I2S interface having two channels, CHANNEL_PER_FIFO = 2 as the I2S stream having two channels is usually saved within one FIFO +#endif +#if CFG_TUD_AUDIO > 1 +#ifndef CFG_TUD_AUDIO_FUNC_2_CHANNEL_PER_FIFO_TX +#error You must tell the driver the number of channels per FIFO for the interleaved encoding! E.g. for an I2S interface having two channels, CHANNEL_PER_FIFO = 2 as the I2S stream having two channels is usually saved within one FIFO +#endif +#endif +#if CFG_TUD_AUDIO > 2 +#ifndef CFG_TUD_AUDIO_FUNC_3_CHANNEL_PER_FIFO_TX +#error You must tell the driver the number of channels per FIFO for the interleaved encoding! E.g. for an I2S interface having two channels, CHANNEL_PER_FIFO = 2 as the I2S stream having two channels is usually saved within one FIFO +#endif +#endif +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING +#ifndef CFG_TUD_AUDIO_FUNC_1_CHANNEL_PER_FIFO_RX +#error You must tell the driver the number of channels per FIFO for the interleaved encoding! E.g. for an I2S interface having two channels, CHANNEL_PER_FIFO = 2 as the I2S stream having two channels is usually saved within one FIFO +#endif +#if CFG_TUD_AUDIO > 1 +#ifndef CFG_TUD_AUDIO_FUNC_2_CHANNEL_PER_FIFO_RX +#error You must tell the driver the number of channels per FIFO for the interleaved encoding! E.g. for an I2S interface having two channels, CHANNEL_PER_FIFO = 2 as the I2S stream having two channels is usually saved within one FIFO +#endif +#endif +#if CFG_TUD_AUDIO > 2 +#ifndef CFG_TUD_AUDIO_FUNC_3_CHANNEL_PER_FIFO_RX +#error You must tell the driver the number of channels per FIFO for the interleaved encoding! E.g. for an I2S interface having two channels, CHANNEL_PER_FIFO = 2 as the I2S stream having two channels is usually saved within one FIFO +#endif +#endif +#endif + +// Remaining types not support so far + +// Number of support FIFOs to set up - multiple channels can be handled by one FIFO - very common is two channels per FIFO stemming from one I2S interface +#ifndef CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO +#define CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO +#define CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO +#define CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO 0 +#endif + +#ifndef CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO +#define CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO +#define CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO +#define CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO 0 +#endif + +// Size of support FIFOs IN BYTES - if size > 0 there are as many FIFOs set up as CFG_TUD_AUDIO_FUNC_X_N_TX_SUPP_SW_FIFO and CFG_TUD_AUDIO_FUNC_X_N_RX_SUPP_SW_FIFO +#ifndef CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ +#define CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ 0 // FIFO size - minimum size: ceil(f_s/1000) * max(# of TX channels) / (# of TX support FIFOs) * max(# of bytes per sample) +#endif +#ifndef CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ +#define CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ +#define CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ 0 +#endif + +#ifndef CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ +#define CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ 0 // FIFO size - minimum size: ceil(f_s/1000) * max(# of RX channels) / (# of RX support FIFOs) * max(# of bytes per sample) +#endif +#ifndef CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ +#define CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ 0 +#endif +#ifndef CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ +#define CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ 0 +#endif + +//static_assert(sizeof(tud_audio_desc_lengths) != CFG_TUD_AUDIO, "Supply audio function descriptor pack length!"); + +// Supported types of this driver: +// AUDIO_DATA_FORMAT_TYPE_I_PCM - Required definitions: CFG_TUD_AUDIO_N_CHANNELS and CFG_TUD_AUDIO_BYTES_PER_CHANNEL + +#ifdef __cplusplus +extern "C" { +#endif + +/** \addtogroup AUDIO_Serial Serial + * @{ + * \defgroup AUDIO_Serial_Device Device + * @{ */ + +//--------------------------------------------------------------------+ +// Application API (Multiple Interfaces) +// CFG_TUD_AUDIO > 1 +//--------------------------------------------------------------------+ +bool tud_audio_n_mounted (uint8_t func_id); + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING +uint16_t tud_audio_n_available (uint8_t func_id); +uint16_t tud_audio_n_read (uint8_t func_id, void* buffer, uint16_t bufsize); +bool tud_audio_n_clear_ep_out_ff (uint8_t func_id); // Delete all content in the EP OUT FIFO +tu_fifo_t* tud_audio_n_get_ep_out_ff (uint8_t func_id); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING +bool tud_audio_n_clear_rx_support_ff (uint8_t func_id, uint8_t ff_idx); // Delete all content in the support RX FIFOs +uint16_t tud_audio_n_available_support_ff (uint8_t func_id, uint8_t ff_idx); +uint16_t tud_audio_n_read_support_ff (uint8_t func_id, uint8_t ff_idx, void* buffer, uint16_t bufsize); +tu_fifo_t* tud_audio_n_get_rx_support_ff (uint8_t func_id, uint8_t ff_idx); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING +uint16_t tud_audio_n_write (uint8_t func_id, const void * data, uint16_t len); +bool tud_audio_n_clear_ep_in_ff (uint8_t func_id); // Delete all content in the EP IN FIFO +tu_fifo_t* tud_audio_n_get_ep_in_ff (uint8_t func_id); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING +uint16_t tud_audio_n_flush_tx_support_ff (uint8_t func_id); // Force all content in the support TX FIFOs to be written into EP SW FIFO +bool tud_audio_n_clear_tx_support_ff (uint8_t func_id, uint8_t ff_idx); +uint16_t tud_audio_n_write_support_ff (uint8_t func_id, uint8_t ff_idx, const void * data, uint16_t len); +tu_fifo_t* tud_audio_n_get_tx_support_ff (uint8_t func_id, uint8_t ff_idx); +#endif + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN +uint16_t tud_audio_int_ctr_n_write (uint8_t func_id, uint8_t const* buffer, uint16_t len); +#endif + +//--------------------------------------------------------------------+ +// Application API (Interface0) +//--------------------------------------------------------------------+ + +static inline bool tud_audio_mounted (void); + +// RX API + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING +static inline uint16_t tud_audio_available (void); +static inline bool tud_audio_clear_ep_out_ff (void); // Delete all content in the EP OUT FIFO +static inline uint16_t tud_audio_read (void* buffer, uint16_t bufsize); +static inline tu_fifo_t* tud_audio_get_ep_out_ff (void); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING +static inline bool tud_audio_clear_rx_support_ff (uint8_t ff_idx); +static inline uint16_t tud_audio_available_support_ff (uint8_t ff_idx); +static inline uint16_t tud_audio_read_support_ff (uint8_t ff_idx, void* buffer, uint16_t bufsize); +static inline tu_fifo_t* tud_audio_get_rx_support_ff (uint8_t ff_idx); +#endif + +// TX API + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING +static inline uint16_t tud_audio_write (const void * data, uint16_t len); +static inline bool tud_audio_clear_ep_in_ff (void); +static inline tu_fifo_t* tud_audio_get_ep_in_ff (void); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING +static inline uint16_t tud_audio_flush_tx_support_ff (void); +static inline uint16_t tud_audio_clear_tx_support_ff (uint8_t ff_idx); +static inline uint16_t tud_audio_write_support_ff (uint8_t ff_idx, const void * data, uint16_t len); +static inline tu_fifo_t* tud_audio_get_tx_support_ff (uint8_t ff_idx); +#endif + +// INT CTR API + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN +static inline uint16_t tud_audio_int_ctr_write (uint8_t const* buffer, uint16_t len); +#endif + +// Buffer control EP data and schedule a transmit +// This function is intended to be used if you do not have a persistent buffer or memory location available (e.g. non-local variables) and need to answer onto a +// get request. This function buffers your answer request frame into the control buffer of the corresponding audio driver and schedules a transmit for sending it. +// Since transmission is triggered via interrupts, a persistent memory location is required onto which the buffer pointer in pointing. If you already have such +// available you may directly use 'tud_control_xfer(...)'. In this case data does not need to be copied into an additional buffer and you save some time. +// If the request's wLength is zero, a status packet is sent instead. +bool tud_audio_buffer_and_schedule_control_xfer(uint8_t rhport, tusb_control_request_t const * p_request, void* data, uint16_t len); + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ + +#if CFG_TUD_AUDIO_ENABLE_EP_IN +TU_ATTR_WEAK bool tud_audio_tx_done_pre_load_cb(uint8_t rhport, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting); +TU_ATTR_WEAK bool tud_audio_tx_done_post_load_cb(uint8_t rhport, uint16_t n_bytes_copied, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT +TU_ATTR_WEAK bool tud_audio_rx_done_pre_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting); +TU_ATTR_WEAK bool tud_audio_rx_done_post_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting); +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP +TU_ATTR_WEAK bool tud_audio_fb_done_cb(uint8_t rhport); + +// This function is used to provide data rate feedback from an asynchronous sink. Feedback value will be sent at FB endpoint interval till it's changed. +// +// The feedback format is specified to be 16.16 for HS and 10.14 for FS devices (see Universal Serial Bus Specification Revision 2.0 5.12.4.2). By default, +// the choice of format is left to the caller and feedback argument is sent as-is. If CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION is set, then tinyusb +// expects 16.16 format and handles the conversion to 10.14 on FS. +// +// Note that due to a bug in its USB Audio 2.0 driver, Windows currently requires 16.16 format for _all_ USB 2.0 devices. On Linux and macOS it seems the +// driver can work with either format. So a good compromise is to keep format correction disabled and stick to 16.16 format. +bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback); +static inline bool tud_audio_fb_set(uint32_t feedback); +#endif + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN +TU_ATTR_WEAK bool tud_audio_int_ctr_done_cb(uint8_t rhport, uint16_t n_bytes_copied); +#endif + +// Invoked when audio set interface request received +TU_ATTR_WEAK bool tud_audio_set_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request); + +// Invoked when audio set interface request received which closes an EP +TU_ATTR_WEAK bool tud_audio_set_itf_close_EP_cb(uint8_t rhport, tusb_control_request_t const * p_request); + +// Invoked when audio class specific set request received for an EP +TU_ATTR_WEAK bool tud_audio_set_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff); + +// Invoked when audio class specific set request received for an interface +TU_ATTR_WEAK bool tud_audio_set_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff); + +// Invoked when audio class specific set request received for an entity +TU_ATTR_WEAK bool tud_audio_set_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff); + +// Invoked when audio class specific get request received for an EP +TU_ATTR_WEAK bool tud_audio_get_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request); + +// Invoked when audio class specific get request received for an interface +TU_ATTR_WEAK bool tud_audio_get_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request); + +// Invoked when audio class specific get request received for an entity +TU_ATTR_WEAK bool tud_audio_get_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request); + +//--------------------------------------------------------------------+ +// Inline Functions +//--------------------------------------------------------------------+ + +static inline bool tud_audio_mounted(void) +{ + return tud_audio_n_mounted(0); +} + +// RX API + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING + +static inline uint16_t tud_audio_available(void) +{ + return tud_audio_n_available(0); +} + +static inline uint16_t tud_audio_read(void* buffer, uint16_t bufsize) +{ + return tud_audio_n_read(0, buffer, bufsize); +} + +static inline bool tud_audio_clear_ep_out_ff(void) +{ + return tud_audio_n_clear_ep_out_ff(0); +} + +static inline tu_fifo_t* tud_audio_get_ep_out_ff(void) +{ + return tud_audio_n_get_ep_out_ff(0); +} + +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING + +static inline bool tud_audio_clear_rx_support_ff(uint8_t ff_idx) +{ + return tud_audio_n_clear_rx_support_ff(0, ff_idx); +} + +static inline uint16_t tud_audio_available_support_ff(uint8_t ff_idx) +{ + return tud_audio_n_available_support_ff(0, ff_idx); +} + +static inline uint16_t tud_audio_read_support_ff(uint8_t ff_idx, void* buffer, uint16_t bufsize) +{ + return tud_audio_n_read_support_ff(0, ff_idx, buffer, bufsize); +} + +static inline tu_fifo_t* tud_audio_get_rx_support_ff(uint8_t ff_idx) +{ + return tud_audio_n_get_rx_support_ff(0, ff_idx); +} + +#endif + +// TX API + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING + +static inline uint16_t tud_audio_write(const void * data, uint16_t len) +{ + return tud_audio_n_write(0, data, len); +} + +static inline bool tud_audio_clear_ep_in_ff(void) +{ + return tud_audio_n_clear_ep_in_ff(0); +} + +static inline tu_fifo_t* tud_audio_get_ep_in_ff(void) +{ + return tud_audio_n_get_ep_in_ff(0); +} + +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING + +static inline uint16_t tud_audio_flush_tx_support_ff(void) +{ + return tud_audio_n_flush_tx_support_ff(0); +} + +static inline uint16_t tud_audio_clear_tx_support_ff(uint8_t ff_idx) +{ + return tud_audio_n_clear_tx_support_ff(0, ff_idx); +} + +static inline uint16_t tud_audio_write_support_ff(uint8_t ff_idx, const void * data, uint16_t len) +{ + return tud_audio_n_write_support_ff(0, ff_idx, data, len); +} + +static inline tu_fifo_t* tud_audio_get_tx_support_ff(uint8_t ff_idx) +{ + return tud_audio_n_get_tx_support_ff(0, ff_idx); +} + +#endif + +#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN +static inline uint16_t tud_audio_int_ctr_write(uint8_t const* buffer, uint16_t len) +{ + return tud_audio_int_ctr_n_write(0, buffer, len); +} +#endif + +#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP +static inline bool tud_audio_fb_set(uint32_t feedback) +{ + return tud_audio_n_fb_set(0, feedback); +} +#endif + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void audiod_init (void); +void audiod_reset (uint8_t rhport); +uint16_t audiod_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool audiod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +bool audiod_xfer_cb (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes); + +#ifdef __cplusplus +} +#endif + +#endif /* _TUSB_AUDIO_DEVICE_H_ */ + +/** @} */ +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/bth/bth_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/bth/bth_device.c new file mode 100755 index 000000000..8ef609622 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/bth/bth_device.c @@ -0,0 +1,258 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Jerzy Kasenberg + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_BTH) + +//--------------------------------------------------------------------+ +// INCLUDE +//--------------------------------------------------------------------+ +#include "bth_device.h" +#include + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef struct +{ + uint8_t itf_num; + uint8_t ep_ev; + uint8_t ep_acl_in; + uint8_t ep_acl_out; + uint8_t ep_voice[2]; // Not used yet + uint8_t ep_voice_size[2][CFG_TUD_BTH_ISO_ALT_COUNT]; + + // Endpoint Transfer buffer + CFG_TUSB_MEM_ALIGN bt_hci_cmd_t hci_cmd; + CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_BTH_DATA_EPSIZE]; + +} btd_interface_t; + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +CFG_TUSB_MEM_SECTION btd_interface_t _btd_itf; + +static bool bt_tx_data(uint8_t ep, void *data, uint16_t len) +{ + // skip if previous transfer not complete + TU_VERIFY(!usbd_edpt_busy(TUD_OPT_RHPORT, ep)); + + TU_ASSERT(usbd_edpt_xfer(TUD_OPT_RHPORT, ep, data, len)); + + return true; +} + +//--------------------------------------------------------------------+ +// READ API +//--------------------------------------------------------------------+ + + +//--------------------------------------------------------------------+ +// WRITE API +//--------------------------------------------------------------------+ + +bool tud_bt_event_send(void *event, uint16_t event_len) +{ + return bt_tx_data(_btd_itf.ep_ev, event, event_len); +} + +bool tud_bt_acl_data_send(void *event, uint16_t event_len) +{ + return bt_tx_data(_btd_itf.ep_acl_in, event, event_len); +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void btd_init(void) +{ + tu_memclr(&_btd_itf, sizeof(_btd_itf)); +} + +void btd_reset(uint8_t rhport) +{ + (void)rhport; +} + +uint16_t btd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len) +{ + tusb_desc_endpoint_t const *desc_ep; + uint16_t drv_len = 0; + // Size of single alternative of ISO interface + const uint16_t iso_alt_itf_size = sizeof(tusb_desc_interface_t) + 2 * sizeof(tusb_desc_endpoint_t); + // Size of hci interface + const uint16_t hci_itf_size = sizeof(tusb_desc_interface_t) + 3 * sizeof(tusb_desc_endpoint_t); + // Ensure this is BT Primary Controller + TU_VERIFY(TUSB_CLASS_WIRELESS_CONTROLLER == itf_desc->bInterfaceClass && + TUD_BT_APP_SUBCLASS == itf_desc->bInterfaceSubClass && + TUD_BT_PROTOCOL_PRIMARY_CONTROLLER == itf_desc->bInterfaceProtocol, 0); + + TU_ASSERT(itf_desc->bNumEndpoints == 3 && max_len >= hci_itf_size); + + _btd_itf.itf_num = itf_desc->bInterfaceNumber; + + desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc); + + TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType && TUSB_XFER_INTERRUPT == desc_ep->bmAttributes.xfer, 0); + TU_ASSERT(usbd_edpt_open(rhport, desc_ep), 0); + _btd_itf.ep_ev = desc_ep->bEndpointAddress; + + // Open endpoint pair + TU_ASSERT(usbd_open_edpt_pair(rhport, tu_desc_next(desc_ep), 2, TUSB_XFER_BULK, &_btd_itf.ep_acl_out, + &_btd_itf.ep_acl_in), 0); + + itf_desc = (tusb_desc_interface_t const *)tu_desc_next(tu_desc_next(tu_desc_next(desc_ep))); + + // Prepare for incoming data from host + TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_itf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE), 0); + + drv_len = hci_itf_size; + + // Ensure this is still BT Primary Controller + TU_ASSERT(TUSB_CLASS_WIRELESS_CONTROLLER == itf_desc->bInterfaceClass && + TUD_BT_APP_SUBCLASS == itf_desc->bInterfaceSubClass && + TUD_BT_PROTOCOL_PRIMARY_CONTROLLER == itf_desc->bInterfaceProtocol, 0); + TU_ASSERT(itf_desc->bNumEndpoints == 2 && max_len >= iso_alt_itf_size + drv_len); + + uint8_t dir; + + desc_ep = (tusb_desc_endpoint_t const *)tu_desc_next(itf_desc); + TU_ASSERT(itf_desc->bAlternateSetting < CFG_TUD_BTH_ISO_ALT_COUNT, 0); + TU_ASSERT(desc_ep->bDescriptorType == TUSB_DESC_ENDPOINT, 0); + dir = tu_edpt_dir(desc_ep->bEndpointAddress); + _btd_itf.ep_voice[dir] = desc_ep->bEndpointAddress; + // Store endpoint size for alternative + _btd_itf.ep_voice_size[dir][itf_desc->bAlternateSetting] = (uint8_t) tu_edpt_packet_size(desc_ep); + + desc_ep = (tusb_desc_endpoint_t const *)tu_desc_next(desc_ep); + TU_ASSERT(desc_ep->bDescriptorType == TUSB_DESC_ENDPOINT, 0); + dir = tu_edpt_dir(desc_ep->bEndpointAddress); + _btd_itf.ep_voice[dir] = desc_ep->bEndpointAddress; + // Store endpoint size for alternative + _btd_itf.ep_voice_size[dir][itf_desc->bAlternateSetting] = (uint8_t) tu_edpt_packet_size(desc_ep); + drv_len += iso_alt_itf_size; + + for (int i = 1; i < CFG_TUD_BTH_ISO_ALT_COUNT && drv_len + iso_alt_itf_size <= max_len; ++i) { + // Make sure rest of alternatives matches + itf_desc = (tusb_desc_interface_t const *)tu_desc_next(desc_ep); + if (itf_desc->bDescriptorType != TUSB_DESC_INTERFACE || + TUSB_CLASS_WIRELESS_CONTROLLER != itf_desc->bInterfaceClass || + TUD_BT_APP_SUBCLASS != itf_desc->bInterfaceSubClass || + TUD_BT_PROTOCOL_PRIMARY_CONTROLLER != itf_desc->bInterfaceProtocol) + { + // Not an Iso interface instance + break; + } + TU_ASSERT(itf_desc->bAlternateSetting < CFG_TUD_BTH_ISO_ALT_COUNT, 0); + + desc_ep = (tusb_desc_endpoint_t const *)tu_desc_next(itf_desc); + dir = tu_edpt_dir(desc_ep->bEndpointAddress); + // Verify that alternative endpoint are same as first ones + TU_ASSERT(desc_ep->bDescriptorType == TUSB_DESC_ENDPOINT && + _btd_itf.ep_voice[dir] == desc_ep->bEndpointAddress, 0); + _btd_itf.ep_voice_size[dir][itf_desc->bAlternateSetting] = (uint8_t) tu_edpt_packet_size(desc_ep); + + desc_ep = (tusb_desc_endpoint_t const *)tu_desc_next(desc_ep); + dir = tu_edpt_dir(desc_ep->bEndpointAddress); + // Verify that alternative endpoint are same as first ones + TU_ASSERT(desc_ep->bDescriptorType == TUSB_DESC_ENDPOINT && + _btd_itf.ep_voice[dir] == desc_ep->bEndpointAddress, 0); + _btd_itf.ep_voice_size[dir][itf_desc->bAlternateSetting] = (uint8_t) tu_edpt_packet_size(desc_ep); + drv_len += iso_alt_itf_size; + } + + return drv_len; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool btd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request) +{ + (void)rhport; + + if ( stage == CONTROL_STAGE_SETUP ) + { + if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS && + request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE) + { + // HCI command packet addressing for single function Primary Controllers + TU_VERIFY(request->bRequest == 0 && request->wValue == 0 && request->wIndex == 0); + } + else if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE) + { + if (request->bRequest == TUSB_REQ_SET_INTERFACE && _btd_itf.itf_num + 1 == request->wIndex) + { + // TODO: Set interface it would involve changing size of endpoint size + } + else + { + // HCI command packet for Primary Controller function in a composite device + TU_VERIFY(request->bRequest == 0 && request->wValue == 0 && request->wIndex == _btd_itf.itf_num); + } + } + else return false; + + return tud_control_xfer(rhport, request, &_btd_itf.hci_cmd, sizeof(_btd_itf.hci_cmd)); + } + else if ( stage == CONTROL_STAGE_DATA ) + { + // Handle class request only + TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS); + + if (tud_bt_hci_cmd_cb) tud_bt_hci_cmd_cb(&_btd_itf.hci_cmd, tu_min16(request->wLength, sizeof(_btd_itf.hci_cmd))); + } + + return true; +} + +bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void)result; + + // received new data from host + if (ep_addr == _btd_itf.ep_acl_out) + { + if (tud_bt_acl_data_received_cb) tud_bt_acl_data_received_cb(_btd_itf.epout_buf, xferred_bytes); + + // prepare for next data + TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_itf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE)); + } + else if (ep_addr == _btd_itf.ep_ev) + { + if (tud_bt_event_sent_cb) tud_bt_event_sent_cb((uint16_t)xferred_bytes); + } + else if (ep_addr == _btd_itf.ep_acl_in) + { + if (tud_bt_acl_data_sent_cb) tud_bt_acl_data_sent_cb((uint16_t)xferred_bytes); + } + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/bth/bth_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/bth/bth_device.h new file mode 100755 index 000000000..1b90d0915 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/bth/bth_device.h @@ -0,0 +1,109 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Jerzy Kasenberg + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_BTH_DEVICE_H_ +#define _TUSB_BTH_DEVICE_H_ + +#include +#include + +//--------------------------------------------------------------------+ +// Class Driver Configuration +//--------------------------------------------------------------------+ +#ifndef CFG_TUD_BTH_EVENT_EPSIZE +#define CFG_TUD_BTH_EVENT_EPSIZE 16 +#endif +#ifndef CFG_TUD_BTH_DATA_EPSIZE +#define CFG_TUD_BTH_DATA_EPSIZE 64 +#endif + +typedef struct TU_ATTR_PACKED +{ + uint16_t op_code; + uint8_t param_length; + uint8_t param[255]; +} bt_hci_cmd_t; + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ + +// Invoked when HCI command was received over USB from Bluetooth host. +// Detailed format is described in Bluetooth core specification Vol 2, +// Part E, 5.4.1. +// Length of the command is from 3 bytes (2 bytes for OpCode, +// 1 byte for parameter total length) to 258. +TU_ATTR_WEAK void tud_bt_hci_cmd_cb(void *hci_cmd, size_t cmd_len); + +// Invoked when ACL data was received over USB from Bluetooth host. +// Detailed format is described in Bluetooth core specification Vol 2, +// Part E, 5.4.2. +// Length is from 4 bytes, (12 bits for Handle, 4 bits for flags +// and 16 bits for data total length) to endpoint size. +TU_ATTR_WEAK void tud_bt_acl_data_received_cb(void *acl_data, uint16_t data_len); + +// Called when event sent with tud_bt_event_send() was delivered to BT stack. +// Controller can release/reuse buffer with Event packet at this point. +TU_ATTR_WEAK void tud_bt_event_sent_cb(uint16_t sent_bytes); + +// Called when ACL data that was sent with tud_bt_acl_data_send() +// was delivered to BT stack. +// Controller can release/reuse buffer with ACL packet at this point. +TU_ATTR_WEAK void tud_bt_acl_data_sent_cb(uint16_t sent_bytes); + +// Bluetooth controller calls this function when it wants to send even packet +// as described in Bluetooth core specification Vol 2, Part E, 5.4.4. +// Event has at least 2 bytes, first is Event code second contains parameter +// total length. Controller can release/reuse event memory after +// tud_bt_event_sent_cb() is called. +bool tud_bt_event_send(void *event, uint16_t event_len); + +// Bluetooth controller calls this to send ACL data packet +// as described in Bluetooth core specification Vol 2, Part E, 5.4.2 +// Minimum length is 4 bytes, (12 bits for Handle, 4 bits for flags +// and 16 bits for data total length). Upper limit is not limited +// to endpoint size since buffer is allocate by controller +// and must not be reused till tud_bt_acl_data_sent_cb() is called. +bool tud_bt_acl_data_send(void *acl_data, uint16_t data_len); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void btd_init (void); +void btd_reset (uint8_t rhport); +uint16_t btd_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool btd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const *request); +bool btd_xfer_cb (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_BTH_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc.h new file mode 100644 index 000000000..e345139ea --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc.h @@ -0,0 +1,409 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup group_class + * \defgroup ClassDriver_CDC Communication Device Class (CDC) + * Currently only Abstract Control Model subclass is supported + * @{ */ + +#ifndef _TUSB_CDC_H__ +#define _TUSB_CDC_H__ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +/** \defgroup ClassDriver_CDC_Common Common Definitions + * @{ */ + +// TODO remove +/// CDC Pipe ID, used to indicate which pipe the API is addressing to (Notification, Out, In) +typedef enum +{ + CDC_PIPE_NOTIFICATION , ///< Notification pipe + CDC_PIPE_DATA_IN , ///< Data in pipe + CDC_PIPE_DATA_OUT , ///< Data out pipe + CDC_PIPE_ERROR , ///< Invalid Pipe ID +}cdc_pipeid_t; + +//--------------------------------------------------------------------+ +// CDC Communication Interface Class +//--------------------------------------------------------------------+ + +/// Communication Interface Subclass Codes +typedef enum +{ + CDC_COMM_SUBCLASS_DIRECT_LINE_CONTROL_MODEL = 0x01 , ///< Direct Line Control Model [USBPSTN1.2] + CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL = 0x02 , ///< Abstract Control Model [USBPSTN1.2] + CDC_COMM_SUBCLASS_TELEPHONE_CONTROL_MODEL = 0x03 , ///< Telephone Control Model [USBPSTN1.2] + CDC_COMM_SUBCLASS_MULTICHANNEL_CONTROL_MODEL = 0x04 , ///< Multi-Channel Control Model [USBISDN1.2] + CDC_COMM_SUBCLASS_CAPI_CONTROL_MODEL = 0x05 , ///< CAPI Control Model [USBISDN1.2] + CDC_COMM_SUBCLASS_ETHERNET_CONTROL_MODEL = 0x06 , ///< Ethernet Networking Control Model [USBECM1.2] + CDC_COMM_SUBCLASS_ATM_NETWORKING_CONTROL_MODEL = 0x07 , ///< ATM Networking Control Model [USBATM1.2] + CDC_COMM_SUBCLASS_WIRELESS_HANDSET_CONTROL_MODEL = 0x08 , ///< Wireless Handset Control Model [USBWMC1.1] + CDC_COMM_SUBCLASS_DEVICE_MANAGEMENT = 0x09 , ///< Device Management [USBWMC1.1] + CDC_COMM_SUBCLASS_MOBILE_DIRECT_LINE_MODEL = 0x0A , ///< Mobile Direct Line Model [USBWMC1.1] + CDC_COMM_SUBCLASS_OBEX = 0x0B , ///< OBEX [USBWMC1.1] + CDC_COMM_SUBCLASS_ETHERNET_EMULATION_MODEL = 0x0C , ///< Ethernet Emulation Model [USBEEM1.0] + CDC_COMM_SUBCLASS_NETWORK_CONTROL_MODEL = 0x0D ///< Network Control Model [USBNCM1.0] +} cdc_comm_sublcass_type_t; + +/// Communication Interface Protocol Codes +typedef enum +{ + CDC_COMM_PROTOCOL_NONE = 0x00 , ///< No specific protocol + CDC_COMM_PROTOCOL_ATCOMMAND = 0x01 , ///< AT Commands: V.250 etc + CDC_COMM_PROTOCOL_ATCOMMAND_PCCA_101 = 0x02 , ///< AT Commands defined by PCCA-101 + CDC_COMM_PROTOCOL_ATCOMMAND_PCCA_101_AND_ANNEXO = 0x03 , ///< AT Commands defined by PCCA-101 & Annex O + CDC_COMM_PROTOCOL_ATCOMMAND_GSM_707 = 0x04 , ///< AT Commands defined by GSM 07.07 + CDC_COMM_PROTOCOL_ATCOMMAND_3GPP_27007 = 0x05 , ///< AT Commands defined by 3GPP 27.007 + CDC_COMM_PROTOCOL_ATCOMMAND_CDMA = 0x06 , ///< AT Commands defined by TIA for CDMA + CDC_COMM_PROTOCOL_ETHERNET_EMULATION_MODEL = 0x07 ///< Ethernet Emulation Model +} cdc_comm_protocol_type_t; + +//------------- SubType Descriptor in COMM Functional Descriptor -------------// +/// Communication Interface SubType Descriptor +typedef enum +{ + CDC_FUNC_DESC_HEADER = 0x00 , ///< Header Functional Descriptor, which marks the beginning of the concatenated set of functional descriptors for the interface. + CDC_FUNC_DESC_CALL_MANAGEMENT = 0x01 , ///< Call Management Functional Descriptor. + CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT = 0x02 , ///< Abstract Control Management Functional Descriptor. + CDC_FUNC_DESC_DIRECT_LINE_MANAGEMENT = 0x03 , ///< Direct Line Management Functional Descriptor. + CDC_FUNC_DESC_TELEPHONE_RINGER = 0x04 , ///< Telephone Ringer Functional Descriptor. + CDC_FUNC_DESC_TELEPHONE_CALL_AND_LINE_STATE_REPORTING_CAPACITY = 0x05 , ///< Telephone Call and Line State Reporting Capabilities Functional Descriptor. + CDC_FUNC_DESC_UNION = 0x06 , ///< Union Functional Descriptor + CDC_FUNC_DESC_COUNTRY_SELECTION = 0x07 , ///< Country Selection Functional Descriptor + CDC_FUNC_DESC_TELEPHONE_OPERATIONAL_MODES = 0x08 , ///< Telephone Operational ModesFunctional Descriptor + CDC_FUNC_DESC_USB_TERMINAL = 0x09 , ///< USB Terminal Functional Descriptor + CDC_FUNC_DESC_NETWORK_CHANNEL_TERMINAL = 0x0A , ///< Network Channel Terminal Descriptor + CDC_FUNC_DESC_PROTOCOL_UNIT = 0x0B , ///< Protocol Unit Functional Descriptor + CDC_FUNC_DESC_EXTENSION_UNIT = 0x0C , ///< Extension Unit Functional Descriptor + CDC_FUNC_DESC_MULTICHANEL_MANAGEMENT = 0x0D , ///< Multi-Channel Management Functional Descriptor + CDC_FUNC_DESC_CAPI_CONTROL_MANAGEMENT = 0x0E , ///< CAPI Control Management Functional Descriptor + CDC_FUNC_DESC_ETHERNET_NETWORKING = 0x0F , ///< Ethernet Networking Functional Descriptor + CDC_FUNC_DESC_ATM_NETWORKING = 0x10 , ///< ATM Networking Functional Descriptor + CDC_FUNC_DESC_WIRELESS_HANDSET_CONTROL_MODEL = 0x11 , ///< Wireless Handset Control Model Functional Descriptor + CDC_FUNC_DESC_MOBILE_DIRECT_LINE_MODEL = 0x12 , ///< Mobile Direct Line Model Functional Descriptor + CDC_FUNC_DESC_MOBILE_DIRECT_LINE_MODEL_DETAIL = 0x13 , ///< MDLM Detail Functional Descriptor + CDC_FUNC_DESC_DEVICE_MANAGEMENT_MODEL = 0x14 , ///< Device Management Model Functional Descriptor + CDC_FUNC_DESC_OBEX = 0x15 , ///< OBEX Functional Descriptor + CDC_FUNC_DESC_COMMAND_SET = 0x16 , ///< Command Set Functional Descriptor + CDC_FUNC_DESC_COMMAND_SET_DETAIL = 0x17 , ///< Command Set Detail Functional Descriptor + CDC_FUNC_DESC_TELEPHONE_CONTROL_MODEL = 0x18 , ///< Telephone Control Model Functional Descriptor + CDC_FUNC_DESC_OBEX_SERVICE_IDENTIFIER = 0x19 , ///< OBEX Service Identifier Functional Descriptor + CDC_FUNC_DESC_NCM = 0x1A , ///< NCM Functional Descriptor +}cdc_func_desc_type_t; + +//--------------------------------------------------------------------+ +// CDC Data Interface Class +//--------------------------------------------------------------------+ + +// SUBCLASS code of Data Interface is not used and should/must be zero + +// Data Interface Protocol Codes +typedef enum{ + CDC_DATA_PROTOCOL_ISDN_BRI = 0x30, ///< Physical interface protocol for ISDN BRI + CDC_DATA_PROTOCOL_HDLC = 0x31, ///< HDLC + CDC_DATA_PROTOCOL_TRANSPARENT = 0x32, ///< Transparent + CDC_DATA_PROTOCOL_Q921_MANAGEMENT = 0x50, ///< Management protocol for Q.921 data link protocol + CDC_DATA_PROTOCOL_Q921_DATA_LINK = 0x51, ///< Data link protocol for Q.931 + CDC_DATA_PROTOCOL_Q921_TEI_MULTIPLEXOR = 0x52, ///< TEI-multiplexor for Q.921 data link protocol + CDC_DATA_PROTOCOL_V42BIS_DATA_COMPRESSION = 0x90, ///< Data compression procedures + CDC_DATA_PROTOCOL_EURO_ISDN = 0x91, ///< Euro-ISDN protocol control + CDC_DATA_PROTOCOL_V24_RATE_ADAPTION_TO_ISDN = 0x92, ///< V.24 rate adaptation to ISDN + CDC_DATA_PROTOCOL_CAPI_COMMAND = 0x93, ///< CAPI Commands + CDC_DATA_PROTOCOL_HOST_BASED_DRIVER = 0xFD, ///< Host based driver. Note: This protocol code should only be used in messages between host and device to identify the host driver portion of a protocol stack. + CDC_DATA_PROTOCOL_IN_PROTOCOL_UNIT_FUNCTIONAL_DESCRIPTOR = 0xFE ///< The protocol(s) are described using a ProtocolUnit Functional Descriptors on Communications Class Interface +}cdc_data_protocol_type_t; + +//--------------------------------------------------------------------+ +// Management Element Request (Control Endpoint) +//--------------------------------------------------------------------+ + +/// Communication Interface Management Element Request Codes +typedef enum +{ + CDC_REQUEST_SEND_ENCAPSULATED_COMMAND = 0x00, ///< is used to issue a command in the format of the supported control protocol of the Communications Class interface + CDC_REQUEST_GET_ENCAPSULATED_RESPONSE = 0x01, ///< is used to request a response in the format of the supported control protocol of the Communications Class interface. + CDC_REQUEST_SET_COMM_FEATURE = 0x02, + CDC_REQUEST_GET_COMM_FEATURE = 0x03, + CDC_REQUEST_CLEAR_COMM_FEATURE = 0x04, + + CDC_REQUEST_SET_AUX_LINE_STATE = 0x10, + CDC_REQUEST_SET_HOOK_STATE = 0x11, + CDC_REQUEST_PULSE_SETUP = 0x12, + CDC_REQUEST_SEND_PULSE = 0x13, + CDC_REQUEST_SET_PULSE_TIME = 0x14, + CDC_REQUEST_RING_AUX_JACK = 0x15, + + CDC_REQUEST_SET_LINE_CODING = 0x20, + CDC_REQUEST_GET_LINE_CODING = 0x21, + CDC_REQUEST_SET_CONTROL_LINE_STATE = 0x22, + CDC_REQUEST_SEND_BREAK = 0x23, + + CDC_REQUEST_SET_RINGER_PARMS = 0x30, + CDC_REQUEST_GET_RINGER_PARMS = 0x31, + CDC_REQUEST_SET_OPERATION_PARMS = 0x32, + CDC_REQUEST_GET_OPERATION_PARMS = 0x33, + CDC_REQUEST_SET_LINE_PARMS = 0x34, + CDC_REQUEST_GET_LINE_PARMS = 0x35, + CDC_REQUEST_DIAL_DIGITS = 0x36, + CDC_REQUEST_SET_UNIT_PARAMETER = 0x37, + CDC_REQUEST_GET_UNIT_PARAMETER = 0x38, + CDC_REQUEST_CLEAR_UNIT_PARAMETER = 0x39, + CDC_REQUEST_GET_PROFILE = 0x3A, + + CDC_REQUEST_SET_ETHERNET_MULTICAST_FILTERS = 0x40, + CDC_REQUEST_SET_ETHERNET_POWER_MANAGEMENT_PATTERN_FILTER = 0x41, + CDC_REQUEST_GET_ETHERNET_POWER_MANAGEMENT_PATTERN_FILTER = 0x42, + CDC_REQUEST_SET_ETHERNET_PACKET_FILTER = 0x43, + CDC_REQUEST_GET_ETHERNET_STATISTIC = 0x44, + + CDC_REQUEST_SET_ATM_DATA_FORMAT = 0x50, + CDC_REQUEST_GET_ATM_DEVICE_STATISTICS = 0x51, + CDC_REQUEST_SET_ATM_DEFAULT_VC = 0x52, + CDC_REQUEST_GET_ATM_VC_STATISTICS = 0x53, + + CDC_REQUEST_MDLM_SEMANTIC_MODEL = 0x60, +}cdc_management_request_t; + +//--------------------------------------------------------------------+ +// Management Elemenent Notification (Notification Endpoint) +//--------------------------------------------------------------------+ + +/// 6.3 Notification Codes +typedef enum +{ + CDC_NOTIF_NETWORK_CONNECTION = 0x00, ///< This notification allows the device to notify the host about network connection status. + CDC_NOTIF_RESPONSE_AVAILABLE = 0x01, ///< This notification allows the device to notify the hostthat a response is available. This response can be retrieved with a subsequent \ref CDC_REQUEST_GET_ENCAPSULATED_RESPONSE request. + CDC_NOTIF_AUX_JACK_HOOK_STATE = 0x08, + CDC_NOTIF_RING_DETECT = 0x09, + CDC_NOTIF_SERIAL_STATE = 0x20, + CDC_NOTIF_CALL_STATE_CHANGE = 0x28, + CDC_NOTIF_LINE_STATE_CHANGE = 0x29, + CDC_NOTIF_CONNECTION_SPEED_CHANGE = 0x2A, ///< This notification allows the device to inform the host-networking driver that a change in either the upstream or the downstream bit rate of the connection has occurred + CDC_NOTIF_MDLM_SEMANTIC_MODEL_NOTIFICATION = 0x40, +}cdc_notification_request_t; + +//--------------------------------------------------------------------+ +// Class Specific Functional Descriptor (Communication Interface) +//--------------------------------------------------------------------+ + +// Start of all packed definitions for compiler without per-type packed +TU_ATTR_PACKED_BEGIN +TU_ATTR_BIT_FIELD_ORDER_BEGIN + +/// Header Functional Descriptor (Communication Interface) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUNC_DESC_ + uint16_t bcdCDC ; ///< CDC release number in Binary-Coded Decimal +}cdc_desc_func_header_t; + +/// Union Functional Descriptor (Communication Interface) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + uint8_t bControlInterface ; ///< Interface number of Communication Interface + uint8_t bSubordinateInterface ; ///< Array of Interface number of Data Interface +}cdc_desc_func_union_t; + +#define cdc_desc_func_union_n_t(no_slave)\ + struct TU_ATTR_PACKED { \ + uint8_t bLength ;\ + uint8_t bDescriptorType ;\ + uint8_t bDescriptorSubType ;\ + uint8_t bControlInterface ;\ + uint8_t bSubordinateInterface[no_slave] ;\ +} + +/// Country Selection Functional Descriptor (Communication Interface) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + uint8_t iCountryCodeRelDate ; ///< Index of a string giving the release date for the implemented ISO 3166 Country Codes. + uint16_t wCountryCode ; ///< Country code in the format as defined in [ISO3166], release date as specified inoffset 3 for the first supported country. +}cdc_desc_func_country_selection_t; + +#define cdc_desc_func_country_selection_n_t(no_country) \ + struct TU_ATTR_PACKED { \ + uint8_t bLength ;\ + uint8_t bDescriptorType ;\ + uint8_t bDescriptorSubType ;\ + uint8_t iCountryCodeRelDate ;\ + uint16_t wCountryCode[no_country] ;\ +} + +//--------------------------------------------------------------------+ +// PUBLIC SWITCHED TELEPHONE NETWORK (PSTN) SUBCLASS +//--------------------------------------------------------------------+ + +/// \brief Call Management Functional Descriptor +/// \details This functional descriptor describes the processing of calls for the Communications Class interface. +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + + struct { + uint8_t handle_call : 1; ///< 0 - Device sends/receives call management information only over the Communications Class interface. 1 - Device can send/receive call management information over a Data Class interface. + uint8_t send_recv_call : 1; ///< 0 - Device does not handle call management itself. 1 - Device handles call management itself. + uint8_t TU_RESERVED : 6; + } bmCapabilities; + + uint8_t bDataInterface; +}cdc_desc_func_call_management_t; + +typedef struct TU_ATTR_PACKED +{ + uint8_t support_comm_request : 1; ///< Device supports the request combination of Set_Comm_Feature, Clear_Comm_Feature, and Get_Comm_Feature. + uint8_t support_line_request : 1; ///< Device supports the request combination of Set_Line_Coding, Set_Control_Line_State, Get_Line_Coding, and the notification Serial_State. + uint8_t support_send_break : 1; ///< Device supports the request Send_Break + uint8_t support_notification_network_connection : 1; ///< Device supports the notification Network_Connection. + uint8_t TU_RESERVED : 4; +}cdc_acm_capability_t; + +TU_VERIFY_STATIC(sizeof(cdc_acm_capability_t) == 1, "mostly problem with compiler"); + +/// Abstract Control Management Functional Descriptor +/// This functional descriptor describes the commands supported by by the Communications Class interface with SubClass code of \ref CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + cdc_acm_capability_t bmCapabilities ; +}cdc_desc_func_acm_t; + +/// \brief Direct Line Management Functional Descriptor +/// \details This functional descriptor describes the commands supported by the Communications Class interface with SubClass code of \ref CDC_FUNC_DESC_DIRECT_LINE_MANAGEMENT +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + struct { + uint8_t require_pulse_setup : 1; ///< Device requires extra Pulse_Setup request during pulse dialing sequence to disengage holding circuit. + uint8_t support_aux_request : 1; ///< Device supports the request combination of Set_Aux_Line_State, Ring_Aux_Jack, and notification Aux_Jack_Hook_State. + uint8_t support_pulse_request : 1; ///< Device supports the request combination of Pulse_Setup, Send_Pulse, and Set_Pulse_Time. + uint8_t TU_RESERVED : 5; + } bmCapabilities; +}cdc_desc_func_direct_line_management_t; + +/// \brief Telephone Ringer Functional Descriptor +/// \details The Telephone Ringer functional descriptor describes the ringer capabilities supported by the Communications Class interface, +/// with the SubClass code of \ref CDC_COMM_SUBCLASS_TELEPHONE_CONTROL_MODEL +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + uint8_t bRingerVolSteps ; + uint8_t bNumRingerPatterns ; +}cdc_desc_func_telephone_ringer_t; + +/// \brief Telephone Operational Modes Functional Descriptor +/// \details The Telephone Operational Modes functional descriptor describes the operational modes supported by +/// the Communications Class interface, with the SubClass code of \ref CDC_COMM_SUBCLASS_TELEPHONE_CONTROL_MODEL +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + struct { + uint8_t simple_mode : 1; + uint8_t standalone_mode : 1; + uint8_t computer_centric_mode : 1; + uint8_t TU_RESERVED : 5; + } bmCapabilities; +}cdc_desc_func_telephone_operational_modes_t; + +/// \brief Telephone Call and Line State Reporting Capabilities Descriptor +/// \details The Telephone Call and Line State Reporting Capabilities functional descriptor describes the abilities of a +/// telephone device to report optional call and line states. +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType one of above CDC_FUCN_DESC_ + struct { + uint32_t interrupted_dialtone : 1; ///< 0 : Reports only dialtone (does not differentiate between normal and interrupted dialtone). 1 : Reports interrupted dialtone in addition to normal dialtone + uint32_t ringback_busy_fastbusy : 1; ///< 0 : Reports only dialing state. 1 : Reports ringback, busy, and fast busy states. + uint32_t caller_id : 1; ///< 0 : Does not report caller ID. 1 : Reports caller ID information. + uint32_t incoming_distinctive : 1; ///< 0 : Reports only incoming ringing. 1 : Reports incoming distinctive ringing patterns. + uint32_t dual_tone_multi_freq : 1; ///< 0 : Cannot report dual tone multi-frequency (DTMF) digits input remotely over the telephone line. 1 : Can report DTMF digits input remotely over the telephone line. + uint32_t line_state_change : 1; ///< 0 : Does not support line state change notification. 1 : Does support line state change notification + uint32_t TU_RESERVED : 26; + } bmCapabilities; +}cdc_desc_func_telephone_call_state_reporting_capabilities_t; + +// TODO remove +static inline uint8_t cdc_functional_desc_typeof(uint8_t const * p_desc) +{ + return p_desc[2]; +} + +//--------------------------------------------------------------------+ +// Requests +//--------------------------------------------------------------------+ +typedef struct TU_ATTR_PACKED +{ + uint32_t bit_rate; + uint8_t stop_bits; ///< 0: 1 stop bit - 1: 1.5 stop bits - 2: 2 stop bits + uint8_t parity; ///< 0: None - 1: Odd - 2: Even - 3: Mark - 4: Space + uint8_t data_bits; ///< can be 5, 6, 7, 8 or 16 +} cdc_line_coding_t; + +TU_VERIFY_STATIC(sizeof(cdc_line_coding_t) == 7, "size is not correct"); + +typedef struct TU_ATTR_PACKED +{ + uint16_t dte_is_present : 1; ///< Indicates to DCE if DTE is presentor not. This signal corresponds to V.24 signal 108/2 and RS-232 signal DTR. + uint16_t half_duplex_carrier_control : 1; + uint16_t : 14; +} cdc_line_control_state_t; + +TU_VERIFY_STATIC(sizeof(cdc_line_control_state_t) == 2, "size is not correct"); + +TU_ATTR_PACKED_END // End of all packed definitions +TU_ATTR_BIT_FIELD_ORDER_END + +#ifdef __cplusplus + } +#endif + +#endif + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_device.c new file mode 100644 index 000000000..08f2af253 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_device.c @@ -0,0 +1,486 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_CDC) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "cdc_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +enum +{ + BULK_PACKET_SIZE = (TUD_OPT_HIGH_SPEED ? 512 : 64) +}; + +typedef struct +{ + uint8_t itf_num; + uint8_t ep_notif; + uint8_t ep_in; + uint8_t ep_out; + + // Bit 0: DTR (Data Terminal Ready), Bit 1: RTS (Request to Send) + uint8_t line_state; + + /*------------- From this point, data is not cleared by bus reset -------------*/ + char wanted_char; + cdc_line_coding_t line_coding; + + // FIFO + tu_fifo_t rx_ff; + tu_fifo_t tx_ff; + + uint8_t rx_ff_buf[CFG_TUD_CDC_RX_BUFSIZE]; + uint8_t tx_ff_buf[CFG_TUD_CDC_TX_BUFSIZE]; + +#if CFG_FIFO_MUTEX + osal_mutex_def_t rx_ff_mutex; + osal_mutex_def_t tx_ff_mutex; +#endif + + // Endpoint Transfer buffer + CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_CDC_EP_BUFSIZE]; + CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_CDC_EP_BUFSIZE]; + +}cdcd_interface_t; + +#define ITF_MEM_RESET_SIZE offsetof(cdcd_interface_t, wanted_char) + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +CFG_TUSB_MEM_SECTION static cdcd_interface_t _cdcd_itf[CFG_TUD_CDC]; + +static bool _prep_out_transaction (cdcd_interface_t* p_cdc) +{ + uint8_t const rhport = TUD_OPT_RHPORT; + uint16_t available = tu_fifo_remaining(&p_cdc->rx_ff); + + // Prepare for incoming data but only allow what we can store in the ring buffer. + // TODO Actually we can still carry out the transfer, keeping count of received bytes + // and slowly move it to the FIFO when read(). + // This pre-check reduces endpoint claiming + TU_VERIFY(available >= sizeof(p_cdc->epout_buf)); + + // claim endpoint + TU_VERIFY(usbd_edpt_claim(rhport, p_cdc->ep_out)); + + // fifo can be changed before endpoint is claimed + available = tu_fifo_remaining(&p_cdc->rx_ff); + + if ( available >= sizeof(p_cdc->epout_buf) ) + { + return usbd_edpt_xfer(rhport, p_cdc->ep_out, p_cdc->epout_buf, sizeof(p_cdc->epout_buf)); + }else + { + // Release endpoint since we don't make any transfer + usbd_edpt_release(rhport, p_cdc->ep_out); + + return false; + } +} + +//--------------------------------------------------------------------+ +// APPLICATION API +//--------------------------------------------------------------------+ +bool tud_cdc_n_connected(uint8_t itf) +{ + // DTR (bit 0) active is considered as connected + return tud_ready() && tu_bit_test(_cdcd_itf[itf].line_state, 0); +} + +uint8_t tud_cdc_n_get_line_state (uint8_t itf) +{ + return _cdcd_itf[itf].line_state; +} + +void tud_cdc_n_get_line_coding (uint8_t itf, cdc_line_coding_t* coding) +{ + (*coding) = _cdcd_itf[itf].line_coding; +} + +void tud_cdc_n_set_wanted_char (uint8_t itf, char wanted) +{ + _cdcd_itf[itf].wanted_char = wanted; +} + + +//--------------------------------------------------------------------+ +// READ API +//--------------------------------------------------------------------+ +uint32_t tud_cdc_n_available(uint8_t itf) +{ + return tu_fifo_count(&_cdcd_itf[itf].rx_ff); +} + +uint32_t tud_cdc_n_read(uint8_t itf, void* buffer, uint32_t bufsize) +{ + cdcd_interface_t* p_cdc = &_cdcd_itf[itf]; + uint32_t num_read = tu_fifo_read_n(&p_cdc->rx_ff, buffer, bufsize); + _prep_out_transaction(p_cdc); + return num_read; +} + +bool tud_cdc_n_peek(uint8_t itf, uint8_t* chr) +{ + return tu_fifo_peek(&_cdcd_itf[itf].rx_ff, chr); +} + +void tud_cdc_n_read_flush (uint8_t itf) +{ + cdcd_interface_t* p_cdc = &_cdcd_itf[itf]; + tu_fifo_clear(&p_cdc->rx_ff); + _prep_out_transaction(p_cdc); +} + +//--------------------------------------------------------------------+ +// WRITE API +//--------------------------------------------------------------------+ +uint32_t tud_cdc_n_write(uint8_t itf, void const* buffer, uint32_t bufsize) +{ + cdcd_interface_t* p_cdc = &_cdcd_itf[itf]; + uint16_t ret = tu_fifo_write_n(&p_cdc->tx_ff, buffer, bufsize); + + // flush if queue more than packet size + if ( tu_fifo_count(&p_cdc->tx_ff) >= BULK_PACKET_SIZE ) + { + tud_cdc_n_write_flush(itf); + } + + return ret; +} + +uint32_t tud_cdc_n_write_flush (uint8_t itf) +{ + cdcd_interface_t* p_cdc = &_cdcd_itf[itf]; + + // Skip if usb is not ready yet + TU_VERIFY( tud_ready(), 0 ); + + // No data to send + if ( !tu_fifo_count(&p_cdc->tx_ff) ) return 0; + + uint8_t const rhport = TUD_OPT_RHPORT; + + // Claim the endpoint + TU_VERIFY( usbd_edpt_claim(rhport, p_cdc->ep_in), 0 ); + + // Pull data from FIFO + uint16_t const count = tu_fifo_read_n(&p_cdc->tx_ff, p_cdc->epin_buf, sizeof(p_cdc->epin_buf)); + + if ( count ) + { + TU_ASSERT( usbd_edpt_xfer(rhport, p_cdc->ep_in, p_cdc->epin_buf, count), 0 ); + return count; + }else + { + // Release endpoint since we don't make any transfer + // Note: data is dropped if terminal is not connected + usbd_edpt_release(rhport, p_cdc->ep_in); + return 0; + } +} + +uint32_t tud_cdc_n_write_available (uint8_t itf) +{ + return tu_fifo_remaining(&_cdcd_itf[itf].tx_ff); +} + +bool tud_cdc_n_write_clear (uint8_t itf) +{ + return tu_fifo_clear(&_cdcd_itf[itf].tx_ff); +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void cdcd_init(void) +{ + tu_memclr(_cdcd_itf, sizeof(_cdcd_itf)); + + for(uint8_t i=0; iwanted_char = (char) -1; + + // default line coding is : stop bit = 1, parity = none, data bits = 8 + p_cdc->line_coding.bit_rate = 115200; + p_cdc->line_coding.stop_bits = 0; + p_cdc->line_coding.parity = 0; + p_cdc->line_coding.data_bits = 8; + + // Config RX fifo + tu_fifo_config(&p_cdc->rx_ff, p_cdc->rx_ff_buf, TU_ARRAY_SIZE(p_cdc->rx_ff_buf), 1, false); + + // Config TX fifo as overwritable at initialization and will be changed to non-overwritable + // if terminal supports DTR bit. Without DTR we do not know if data is actually polled by terminal. + // In this way, the most current data is prioritized. + tu_fifo_config(&p_cdc->tx_ff, p_cdc->tx_ff_buf, TU_ARRAY_SIZE(p_cdc->tx_ff_buf), 1, true); + +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&p_cdc->rx_ff, NULL, osal_mutex_create(&p_cdc->rx_ff_mutex)); + tu_fifo_config_mutex(&p_cdc->tx_ff, osal_mutex_create(&p_cdc->tx_ff_mutex), NULL); +#endif + } +} + +void cdcd_reset(uint8_t rhport) +{ + (void) rhport; + + for(uint8_t i=0; irx_ff); + tu_fifo_clear(&p_cdc->tx_ff); + tu_fifo_set_overwritable(&p_cdc->tx_ff, true); + } +} + +uint16_t cdcd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + // Only support ACM subclass + TU_VERIFY( TUSB_CLASS_CDC == itf_desc->bInterfaceClass && + CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass, 0); + + // Find available interface + cdcd_interface_t * p_cdc = NULL; + for(uint8_t cdc_id=0; cdc_iditf_num = itf_desc->bInterfaceNumber; + + uint16_t drv_len = sizeof(tusb_desc_interface_t); + uint8_t const * p_desc = tu_desc_next( itf_desc ); + + // Communication Functional Descriptors + while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len ) + { + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) ) + { + // notification endpoint + tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc; + + TU_ASSERT( usbd_edpt_open(rhport, desc_ep), 0 ); + p_cdc->ep_notif = desc_ep->bEndpointAddress; + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + //------------- Data Interface (if any) -------------// + if ( (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && + (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const *) p_desc)->bInterfaceClass) ) + { + // next to endpoint descriptor + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + + // Open endpoint pair + TU_ASSERT( usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &p_cdc->ep_out, &p_cdc->ep_in), 0 ); + + drv_len += 2*sizeof(tusb_desc_endpoint_t); + } + + // Prepare for incoming data + _prep_out_transaction(p_cdc); + + return drv_len; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool cdcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + // Handle class request only + TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS); + + uint8_t itf = 0; + cdcd_interface_t* p_cdc = _cdcd_itf; + + // Identify which interface to use + for ( ; ; itf++, p_cdc++) + { + if (itf >= TU_ARRAY_SIZE(_cdcd_itf)) return false; + + if ( p_cdc->itf_num == request->wIndex ) break; + } + + switch ( request->bRequest ) + { + case CDC_REQUEST_SET_LINE_CODING: + if (stage == CONTROL_STAGE_SETUP) + { + TU_LOG2(" Set Line Coding\r\n"); + tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t)); + } + else if ( stage == CONTROL_STAGE_ACK) + { + if ( tud_cdc_line_coding_cb ) tud_cdc_line_coding_cb(itf, &p_cdc->line_coding); + } + break; + + case CDC_REQUEST_GET_LINE_CODING: + if (stage == CONTROL_STAGE_SETUP) + { + TU_LOG2(" Get Line Coding\r\n"); + tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t)); + } + break; + + case CDC_REQUEST_SET_CONTROL_LINE_STATE: + if (stage == CONTROL_STAGE_SETUP) + { + tud_control_status(rhport, request); + } + else if (stage == CONTROL_STAGE_ACK) + { + // CDC PSTN v1.2 section 6.3.12 + // Bit 0: Indicates if DTE is present or not. + // This signal corresponds to V.24 signal 108/2 and RS-232 signal DTR (Data Terminal Ready) + // Bit 1: Carrier control for half-duplex modems. + // This signal corresponds to V.24 signal 105 and RS-232 signal RTS (Request to Send) + bool const dtr = tu_bit_test(request->wValue, 0); + bool const rts = tu_bit_test(request->wValue, 1); + + p_cdc->line_state = (uint8_t) request->wValue; + + // Disable fifo overwriting if DTR bit is set + tu_fifo_set_overwritable(&p_cdc->tx_ff, !dtr); + + TU_LOG2(" Set Control Line State: DTR = %d, RTS = %d\r\n", dtr, rts); + + // Invoke callback + if ( tud_cdc_line_state_cb ) tud_cdc_line_state_cb(itf, dtr, rts); + } + break; + case CDC_REQUEST_SEND_BREAK: + if (stage == CONTROL_STAGE_SETUP) + { + tud_control_status(rhport, request); + } + else if (stage == CONTROL_STAGE_ACK) + { + TU_LOG2(" Send Break\r\n"); + if ( tud_cdc_send_break_cb ) tud_cdc_send_break_cb(itf, request->wValue); + } + break; + + default: return false; // stall unsupported request + } + + return true; +} + +bool cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) result; + + uint8_t itf; + cdcd_interface_t* p_cdc; + + // Identify which interface to use + for (itf = 0; itf < CFG_TUD_CDC; itf++) + { + p_cdc = &_cdcd_itf[itf]; + if ( ( ep_addr == p_cdc->ep_out ) || ( ep_addr == p_cdc->ep_in ) ) break; + } + TU_ASSERT(itf < CFG_TUD_CDC); + + // Received new data + if ( ep_addr == p_cdc->ep_out ) + { + tu_fifo_write_n(&p_cdc->rx_ff, &p_cdc->epout_buf, xferred_bytes); + + // Check for wanted char and invoke callback if needed + if ( tud_cdc_rx_wanted_cb && (((signed char) p_cdc->wanted_char) != -1) ) + { + for ( uint32_t i = 0; i < xferred_bytes; i++ ) + { + if ( (p_cdc->wanted_char == p_cdc->epout_buf[i]) && !tu_fifo_empty(&p_cdc->rx_ff) ) + { + tud_cdc_rx_wanted_cb(itf, p_cdc->wanted_char); + } + } + } + + // invoke receive callback (if there is still data) + if (tud_cdc_rx_cb && !tu_fifo_empty(&p_cdc->rx_ff) ) tud_cdc_rx_cb(itf); + + // prepare for OUT transaction + _prep_out_transaction(p_cdc); + } + + // Data sent to host, we continue to fetch from tx fifo to send. + // Note: This will cause incorrect baudrate set in line coding. + // Though maybe the baudrate is not really important !!! + if ( ep_addr == p_cdc->ep_in ) + { + // invoke transmit callback to possibly refill tx fifo + if ( tud_cdc_tx_complete_cb ) tud_cdc_tx_complete_cb(itf); + + if ( 0 == tud_cdc_n_write_flush(itf) ) + { + // If there is no data left, a ZLP should be sent if + // xferred_bytes is multiple of EP Packet size and not zero + if ( !tu_fifo_count(&p_cdc->tx_ff) && xferred_bytes && (0 == (xferred_bytes & (BULK_PACKET_SIZE-1))) ) + { + if ( usbd_edpt_claim(rhport, p_cdc->ep_in) ) + { + usbd_edpt_xfer(rhport, p_cdc->ep_in, NULL, 0); + } + } + } + } + + // nothing to do with notif endpoint for now + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_device.h new file mode 100644 index 000000000..fbc7162a3 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_device.h @@ -0,0 +1,260 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_CDC_DEVICE_H_ +#define _TUSB_CDC_DEVICE_H_ + +#include "common/tusb_common.h" +#include "cdc.h" + +//--------------------------------------------------------------------+ +// Class Driver Configuration +//--------------------------------------------------------------------+ +#if !defined(CFG_TUD_CDC_EP_BUFSIZE) && defined(CFG_TUD_CDC_EPSIZE) + #warning CFG_TUD_CDC_EPSIZE is renamed to CFG_TUD_CDC_EP_BUFSIZE, please update to use the new name + #define CFG_TUD_CDC_EP_BUFSIZE CFG_TUD_CDC_EPSIZE +#endif + +#ifndef CFG_TUD_CDC_EP_BUFSIZE + #define CFG_TUD_CDC_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +/** \addtogroup CDC_Serial Serial + * @{ + * \defgroup CDC_Serial_Device Device + * @{ */ + +//--------------------------------------------------------------------+ +// Application API (Multiple Ports) +// CFG_TUD_CDC > 1 +//--------------------------------------------------------------------+ + +// Check if terminal is connected to this port +bool tud_cdc_n_connected (uint8_t itf); + +// Get current line state. Bit 0: DTR (Data Terminal Ready), Bit 1: RTS (Request to Send) +uint8_t tud_cdc_n_get_line_state (uint8_t itf); + +// Get current line encoding: bit rate, stop bits parity etc .. +void tud_cdc_n_get_line_coding (uint8_t itf, cdc_line_coding_t* coding); + +// Set special character that will trigger tud_cdc_rx_wanted_cb() callback on receiving +void tud_cdc_n_set_wanted_char (uint8_t itf, char wanted); + +// Get the number of bytes available for reading +uint32_t tud_cdc_n_available (uint8_t itf); + +// Read received bytes +uint32_t tud_cdc_n_read (uint8_t itf, void* buffer, uint32_t bufsize); + +// Read a byte, return -1 if there is none +static inline +int32_t tud_cdc_n_read_char (uint8_t itf); + +// Clear the received FIFO +void tud_cdc_n_read_flush (uint8_t itf); + +// Get a byte from FIFO at the specified position without removing it +bool tud_cdc_n_peek (uint8_t itf, uint8_t* ui8); + +// Write bytes to TX FIFO, data may remain in the FIFO for a while +uint32_t tud_cdc_n_write (uint8_t itf, void const* buffer, uint32_t bufsize); + +// Write a byte +static inline +uint32_t tud_cdc_n_write_char (uint8_t itf, char ch); + +// Write a null-terminated string +static inline +uint32_t tud_cdc_n_write_str (uint8_t itf, char const* str); + +// Force sending data if possible, return number of forced bytes +uint32_t tud_cdc_n_write_flush (uint8_t itf); + +// Return the number of bytes (characters) available for writing to TX FIFO buffer in a single n_write operation. +uint32_t tud_cdc_n_write_available (uint8_t itf); + +// Clear the transmit FIFO +bool tud_cdc_n_write_clear (uint8_t itf); + +//--------------------------------------------------------------------+ +// Application API (Single Port) +//--------------------------------------------------------------------+ +static inline bool tud_cdc_connected (void); +static inline uint8_t tud_cdc_get_line_state (void); +static inline void tud_cdc_get_line_coding (cdc_line_coding_t* coding); +static inline void tud_cdc_set_wanted_char (char wanted); + +static inline uint32_t tud_cdc_available (void); +static inline int32_t tud_cdc_read_char (void); +static inline uint32_t tud_cdc_read (void* buffer, uint32_t bufsize); +static inline void tud_cdc_read_flush (void); +static inline bool tud_cdc_peek (uint8_t* ui8); + +static inline uint32_t tud_cdc_write_char (char ch); +static inline uint32_t tud_cdc_write (void const* buffer, uint32_t bufsize); +static inline uint32_t tud_cdc_write_str (char const* str); +static inline uint32_t tud_cdc_write_flush (void); +static inline uint32_t tud_cdc_write_available (void); +static inline bool tud_cdc_write_clear (void); + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ + +// Invoked when received new data +TU_ATTR_WEAK void tud_cdc_rx_cb(uint8_t itf); + +// Invoked when received `wanted_char` +TU_ATTR_WEAK void tud_cdc_rx_wanted_cb(uint8_t itf, char wanted_char); + +// Invoked when space becomes available in TX buffer +TU_ATTR_WEAK void tud_cdc_tx_complete_cb(uint8_t itf); + +// Invoked when line state DTR & RTS are changed via SET_CONTROL_LINE_STATE +TU_ATTR_WEAK void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts); + +// Invoked when line coding is change via SET_LINE_CODING +TU_ATTR_WEAK void tud_cdc_line_coding_cb(uint8_t itf, cdc_line_coding_t const* p_line_coding); + +// Invoked when received send break +TU_ATTR_WEAK void tud_cdc_send_break_cb(uint8_t itf, uint16_t duration_ms); + +//--------------------------------------------------------------------+ +// Inline Functions +//--------------------------------------------------------------------+ +static inline int32_t tud_cdc_n_read_char (uint8_t itf) +{ + uint8_t ch; + return tud_cdc_n_read(itf, &ch, 1) ? (int32_t) ch : -1; +} + +static inline uint32_t tud_cdc_n_write_char(uint8_t itf, char ch) +{ + return tud_cdc_n_write(itf, &ch, 1); +} + +static inline uint32_t tud_cdc_n_write_str (uint8_t itf, char const* str) +{ + return tud_cdc_n_write(itf, str, strlen(str)); +} + +static inline bool tud_cdc_connected (void) +{ + return tud_cdc_n_connected(0); +} + +static inline uint8_t tud_cdc_get_line_state (void) +{ + return tud_cdc_n_get_line_state(0); +} + +static inline void tud_cdc_get_line_coding (cdc_line_coding_t* coding) +{ + tud_cdc_n_get_line_coding(0, coding); +} + +static inline void tud_cdc_set_wanted_char (char wanted) +{ + tud_cdc_n_set_wanted_char(0, wanted); +} + +static inline uint32_t tud_cdc_available (void) +{ + return tud_cdc_n_available(0); +} + +static inline int32_t tud_cdc_read_char (void) +{ + return tud_cdc_n_read_char(0); +} + +static inline uint32_t tud_cdc_read (void* buffer, uint32_t bufsize) +{ + return tud_cdc_n_read(0, buffer, bufsize); +} + +static inline void tud_cdc_read_flush (void) +{ + tud_cdc_n_read_flush(0); +} + +static inline bool tud_cdc_peek (uint8_t* ui8) +{ + return tud_cdc_n_peek(0, ui8); +} + +static inline uint32_t tud_cdc_write_char (char ch) +{ + return tud_cdc_n_write_char(0, ch); +} + +static inline uint32_t tud_cdc_write (void const* buffer, uint32_t bufsize) +{ + return tud_cdc_n_write(0, buffer, bufsize); +} + +static inline uint32_t tud_cdc_write_str (char const* str) +{ + return tud_cdc_n_write_str(0, str); +} + +static inline uint32_t tud_cdc_write_flush (void) +{ + return tud_cdc_n_write_flush(0); +} + +static inline uint32_t tud_cdc_write_available(void) +{ + return tud_cdc_n_write_available(0); +} + +static inline bool tud_cdc_write_clear(void) +{ + return tud_cdc_n_write_clear(0); +} + +/** @} */ +/** @} */ + +//--------------------------------------------------------------------+ +// INTERNAL USBD-CLASS DRIVER API +//--------------------------------------------------------------------+ +void cdcd_init (void); +void cdcd_reset (uint8_t rhport); +uint16_t cdcd_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool cdcd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +bool cdcd_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_CDC_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_host.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_host.c new file mode 100644 index 000000000..f4fb6c1d6 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_host.c @@ -0,0 +1,249 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_HOST_ENABLED && CFG_TUH_CDC) + +#include "host/usbh.h" +#include "host/usbh_classdriver.h" + +#include "cdc_host.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef struct { + uint8_t itf_num; + uint8_t itf_protocol; + + uint8_t ep_notif; + uint8_t ep_in; + uint8_t ep_out; + + cdc_acm_capability_t acm_capability; + +} cdch_data_t; + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +static cdch_data_t cdch_data[CFG_TUH_DEVICE_MAX]; + +static inline cdch_data_t* get_itf(uint8_t dev_addr) +{ + return &cdch_data[dev_addr-1]; +} + +bool tuh_cdc_mounted(uint8_t dev_addr) +{ + cdch_data_t* cdc = get_itf(dev_addr); + return cdc->ep_in && cdc->ep_out; +} + +bool tuh_cdc_is_busy(uint8_t dev_addr, cdc_pipeid_t pipeid) +{ + if ( !tuh_cdc_mounted(dev_addr) ) return false; + + cdch_data_t const * p_cdc = get_itf(dev_addr); + + switch (pipeid) + { + case CDC_PIPE_NOTIFICATION: + return usbh_edpt_busy(dev_addr, p_cdc->ep_notif ); + + case CDC_PIPE_DATA_IN: + return usbh_edpt_busy(dev_addr, p_cdc->ep_in ); + + case CDC_PIPE_DATA_OUT: + return usbh_edpt_busy(dev_addr, p_cdc->ep_out ); + + default: + return false; + } +} + +//--------------------------------------------------------------------+ +// APPLICATION API (parameter validation needed) +//--------------------------------------------------------------------+ +bool tuh_cdc_serial_is_mounted(uint8_t dev_addr) +{ + // TODO consider all AT Command as serial candidate + return tuh_cdc_mounted(dev_addr) && + (cdch_data[dev_addr-1].itf_protocol <= CDC_COMM_PROTOCOL_ATCOMMAND_CDMA); +} + +bool tuh_cdc_send(uint8_t dev_addr, void const * p_data, uint32_t length, bool is_notify) +{ + (void) is_notify; + TU_VERIFY( tuh_cdc_mounted(dev_addr) ); + TU_VERIFY( p_data != NULL && length, TUSB_ERROR_INVALID_PARA); + + uint8_t const ep_out = cdch_data[dev_addr-1].ep_out; + if ( usbh_edpt_busy(dev_addr, ep_out) ) return false; + + return usbh_edpt_xfer(dev_addr, ep_out, (void*)(uintptr_t) p_data, length); +} + +bool tuh_cdc_receive(uint8_t dev_addr, void * p_buffer, uint32_t length, bool is_notify) +{ + (void) is_notify; + TU_VERIFY( tuh_cdc_mounted(dev_addr) ); + TU_VERIFY( p_buffer != NULL && length, TUSB_ERROR_INVALID_PARA); + + uint8_t const ep_in = cdch_data[dev_addr-1].ep_in; + if ( usbh_edpt_busy(dev_addr, ep_in) ) return false; + + return usbh_edpt_xfer(dev_addr, ep_in, p_buffer, length); +} + +bool tuh_cdc_set_control_line_state(uint8_t dev_addr, bool dtr, bool rts, tuh_control_complete_cb_t complete_cb) +{ + cdch_data_t const * p_cdc = get_itf(dev_addr); + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = CDC_REQUEST_SET_CONTROL_LINE_STATE, + .wValue = (rts ? 2 : 0) | (dtr ? 1 : 0), + .wIndex = p_cdc->itf_num, + .wLength = 0 + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &request, NULL, complete_cb) ); + return true; +} + +//--------------------------------------------------------------------+ +// USBH-CLASS DRIVER API +//--------------------------------------------------------------------+ +void cdch_init(void) +{ + tu_memclr(cdch_data, sizeof(cdch_data)); +} + +bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len) +{ + (void) max_len; + + // Only support ACM subclass + // Protocol 0xFF can be RNDIS device for windows XP + TU_VERIFY( TUSB_CLASS_CDC == itf_desc->bInterfaceClass && + CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass && + 0xFF != itf_desc->bInterfaceProtocol); + + cdch_data_t * p_cdc = get_itf(dev_addr); + + p_cdc->itf_num = itf_desc->bInterfaceNumber; + p_cdc->itf_protocol = itf_desc->bInterfaceProtocol; + + //------------- Communication Interface -------------// + uint16_t drv_len = tu_desc_len(itf_desc); + uint8_t const * p_desc = tu_desc_next(itf_desc); + + // Communication Functional Descriptors + while( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len ) + { + if ( CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT == cdc_functional_desc_typeof(p_desc) ) + { + // save ACM bmCapabilities + p_cdc->acm_capability = ((cdc_desc_func_acm_t const *) p_desc)->bmCapabilities; + } + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) ) + { + // notification endpoint + tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc; + + TU_ASSERT( usbh_edpt_open(rhport, dev_addr, desc_ep) ); + p_cdc->ep_notif = desc_ep->bEndpointAddress; + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + //------------- Data Interface (if any) -------------// + if ( (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && + (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const *) p_desc)->bInterfaceClass) ) + { + // next to endpoint descriptor + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + + // data endpoints expected to be in pairs + for(uint32_t i=0; i<2; i++) + { + tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc; + TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType && TUSB_XFER_BULK == desc_ep->bmAttributes.xfer); + + TU_ASSERT(usbh_edpt_open(rhport, dev_addr, desc_ep)); + + if ( tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN ) + { + p_cdc->ep_in = desc_ep->bEndpointAddress; + }else + { + p_cdc->ep_out = desc_ep->bEndpointAddress; + } + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next( p_desc ); + } + } + + return true; +} + +bool cdch_set_config(uint8_t dev_addr, uint8_t itf_num) +{ + (void) dev_addr; (void) itf_num; + return true; +} + +bool cdch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) +{ + (void) ep_addr; + tuh_cdc_xfer_isr( dev_addr, event, 0, xferred_bytes ); + return true; +} + +void cdch_close(uint8_t dev_addr) +{ + TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, ); + + cdch_data_t * p_cdc = get_itf(dev_addr); + tu_memclr(p_cdc, sizeof(cdch_data_t)); +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_host.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_host.h new file mode 100644 index 000000000..0d435138b --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_host.h @@ -0,0 +1,134 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_CDC_HOST_H_ +#define _TUSB_CDC_HOST_H_ + +#include "cdc.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// CDC APPLICATION PUBLIC API +//--------------------------------------------------------------------+ +/** \ingroup ClassDriver_CDC Communication Device Class (CDC) + * \addtogroup CDC_Serial Serial + * @{ + * \defgroup CDC_Serial_Host Host + * @{ */ + +bool tuh_cdc_set_control_line_state(uint8_t dev_addr, bool dtr, bool rts, tuh_control_complete_cb_t complete_cb); + +static inline bool tuh_cdc_connect(uint8_t dev_addr, tuh_control_complete_cb_t complete_cb) +{ + return tuh_cdc_set_control_line_state(dev_addr, true, true, complete_cb); +} + +static inline bool tuh_cdc_disconnect(uint8_t dev_addr, tuh_control_complete_cb_t complete_cb) +{ + return tuh_cdc_set_control_line_state(dev_addr, false, false, complete_cb); +} + +/** \brief Check if device support CDC Serial interface or not + * \param[in] dev_addr device address + * \retval true if device supports + * \retval false if device does not support or is not mounted + */ +bool tuh_cdc_serial_is_mounted(uint8_t dev_addr); + +/** \brief Check if the interface is currently busy or not + * \param[in] dev_addr device address + * \param[in] pipeid value from \ref cdc_pipeid_t to indicate target pipe. + * \retval true if the interface is busy, meaning the stack is still transferring/waiting data from/to device + * \retval false if the interface is not busy, meaning the stack successfully transferred data from/to device + * \note This function is used to check if previous transfer is complete (success or error), so that the next transfer + * can be scheduled. User needs to make sure the corresponding interface is mounted + * (by \ref tuh_cdc_serial_is_mounted) before calling this function. + */ +bool tuh_cdc_is_busy(uint8_t dev_addr, cdc_pipeid_t pipeid); + +/** \brief Perform USB OUT transfer to device + * \param[in] dev_addr device address + * \param[in] p_data Buffer containing data. Must be accessible by USB controller (see \ref CFG_TUSB_MEM_SECTION) + * \param[in] length Number of bytes to be transferred via USB bus + * \retval TUSB_ERROR_NONE on success + * \retval TUSB_ERROR_INTERFACE_IS_BUSY if the interface is already transferring data with device + * \retval TUSB_ERROR_DEVICE_NOT_READY if device is not yet configured (by SET CONFIGURED request) + * \retval TUSB_ERROR_INVALID_PARA if input parameters are not correct + * \note This function is non-blocking and returns immediately. The result of USB transfer will be reported by the + * interface's callback function. \a p_data must be declared with \ref CFG_TUSB_MEM_SECTION. + */ +bool tuh_cdc_send(uint8_t dev_addr, void const * p_data, uint32_t length, bool is_notify); + +/** \brief Perform USB IN transfer to get data from device + * \param[in] dev_addr device address + * \param[in] p_buffer Buffer containing received data. Must be accessible by USB controller (see \ref CFG_TUSB_MEM_SECTION) + * \param[in] length Number of bytes to be transferred via USB bus + * \retval TUSB_ERROR_NONE on success + * \retval TUSB_ERROR_INTERFACE_IS_BUSY if the interface is already transferring data with device + * \retval TUSB_ERROR_DEVICE_NOT_READY if device is not yet configured (by SET CONFIGURED request) + * \retval TUSB_ERROR_INVALID_PARA if input parameters are not correct + * \note This function is non-blocking and returns immediately. The result of USB transfer will be reported by the + * interface's callback function. \a p_data must be declared with \ref CFG_TUSB_MEM_SECTION. + */ +bool tuh_cdc_receive(uint8_t dev_addr, void * p_buffer, uint32_t length, bool is_notify); + +//--------------------------------------------------------------------+ +// CDC APPLICATION CALLBACKS +//--------------------------------------------------------------------+ + +/** \brief Callback function that is invoked when an transferring event occurred + * \param[in] dev_addr Address of device + * \param[in] event an value from \ref xfer_result_t + * \param[in] pipe_id value from \ref cdc_pipeid_t indicate the pipe + * \param[in] xferred_bytes Number of bytes transferred via USB bus + * \note event can be one of following + * - XFER_RESULT_SUCCESS : previously scheduled transfer completes successfully. + * - XFER_RESULT_FAILED : previously scheduled transfer encountered a transaction error. + * - XFER_RESULT_STALLED : previously scheduled transfer is stalled by device. + * \note + */ +void tuh_cdc_xfer_isr(uint8_t dev_addr, xfer_result_t event, cdc_pipeid_t pipe_id, uint32_t xferred_bytes); + +/// @} // group CDC_Serial_Host +/// @} + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void cdch_init (void); +bool cdch_open (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len); +bool cdch_set_config (uint8_t dev_addr, uint8_t itf_num); +bool cdch_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); +void cdch_close (uint8_t dev_addr); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_CDC_HOST_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis.h new file mode 100644 index 000000000..e0f129fe3 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis.h @@ -0,0 +1,301 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup ClassDriver_CDC Communication Device Class (CDC) + * \defgroup CDC_RNDIS Remote Network Driver Interface Specification (RNDIS) + * @{ + * \defgroup CDC_RNDIS_Common Common Definitions + * @{ */ + +#ifndef _TUSB_CDC_RNDIS_H_ +#define _TUSB_CDC_RNDIS_H_ + +#include "cdc.h" + +#ifdef __cplusplus + extern "C" { +#endif + +#ifdef __CC_ARM +#pragma diag_suppress 66 // Suppress Keil warnings #66-D: enumeration value is out of "int" range +#endif + +/// RNDIS Message Types +typedef enum +{ + RNDIS_MSG_PACKET = 0x00000001UL, ///< The host and device use this to send network data to one another. + + RNDIS_MSG_INITIALIZE = 0x00000002UL, ///< Sent by the host to initialize the device. + RNDIS_MSG_INITIALIZE_CMPLT = 0x80000002UL, ///< Device response to an initialize message. + + RNDIS_MSG_HALT = 0x00000003UL, ///< Sent by the host to halt the device. This does not have a response. It is optional for the device to send this message to the host. + + RNDIS_MSG_QUERY = 0x00000004UL, ///< Sent by the host to send a query OID. + RNDIS_MSG_QUERY_CMPLT = 0x80000004UL, ///< Device response to a query OID. + + RNDIS_MSG_SET = 0x00000005UL, ///< Sent by the host to send a set OID. + RNDIS_MSG_SET_CMPLT = 0x80000005UL, ///< Device response to a set OID. + + RNDIS_MSG_RESET = 0x00000006UL, ///< Sent by the host to perform a soft reset on the device. + RNDIS_MSG_RESET_CMPLT = 0x80000006UL, ///< Device response to reset message. + + RNDIS_MSG_INDICATE_STATUS = 0x00000007UL, ///< Sent by the device to indicate its status or an error when an unrecognized message is received. + + RNDIS_MSG_KEEP_ALIVE = 0x00000008UL, ///< During idle periods, sent every few seconds by the host to check that the device is still responsive. It is optional for the device to send this message to check if the host is active. + RNDIS_MSG_KEEP_ALIVE_CMPLT = 0x80000008UL ///< The device response to a keepalivemessage. The host can respond with this message to a keepalive message from the device when the device implements the optional KeepAliveTimer. +}rndis_msg_type_t; + +/// RNDIS Message Status Values +typedef enum +{ + RNDIS_STATUS_SUCCESS = 0x00000000UL, ///< Success + RNDIS_STATUS_FAILURE = 0xC0000001UL, ///< Unspecified error + RNDIS_STATUS_INVALID_DATA = 0xC0010015UL, ///< Invalid data error + RNDIS_STATUS_NOT_SUPPORTED = 0xC00000BBUL, ///< Unsupported request error + RNDIS_STATUS_MEDIA_CONNECT = 0x4001000BUL, ///< Device is connected to a network medium. + RNDIS_STATUS_MEDIA_DISCONNECT = 0x4001000CUL ///< Device is disconnected from the medium. +}rndis_msg_status_t; + +#ifdef __CC_ARM +#pragma diag_default 66 // return Keil 66 to normal severity +#endif + +//--------------------------------------------------------------------+ +// MESSAGE STRUCTURE +//--------------------------------------------------------------------+ + +//------------- Initialize -------------// +/// \brief Initialize Message +/// \details This message MUST be sent by the host to initialize the device. +typedef struct { + uint32_t type ; ///< Message type, must be \ref RNDIS_MSG_INITIALIZE + uint32_t length ; ///< Message length in bytes, must be 0x18 + uint32_t request_id ; ///< A 32-bit integer value, generated by the host, used to match the host's sent request to the response from the device. + uint32_t major_version ; ///< The major version of the RNDIS Protocol implemented by the host. + uint32_t minor_version ; ///< The minor version of the RNDIS Protocol implemented by the host + uint32_t max_xfer_size ; ///< The maximum size, in bytes, of any single bus data transfer that the host expects to receive from the device. +}rndis_msg_initialize_t; + +/// \brief Initialize Complete Message +/// \details This message MUST be sent by the device in response to an initialize message. +typedef struct { + uint32_t type ; ///< Message Type, must be \ref RNDIS_MSG_INITIALIZE_CMPLT + uint32_t length ; ///< Message length in bytes, must be 0x30 + uint32_t request_id ; ///< A 32-bit integer value from \a request_id field of the \ref rndis_msg_initialize_t to which this message is a response. + uint32_t status ; ///< The initialization status of the device, has value from \ref rndis_msg_status_t + uint32_t major_version ; ///< the highest-numbered RNDIS Protocol version supported by the device. + uint32_t minor_version ; ///< the highest-numbered RNDIS Protocol version supported by the device. + uint32_t device_flags ; ///< MUST be set to 0x000000010. Other values are reserved for future use. + uint32_t medium ; ///< is 0x00 for RNDIS_MEDIUM_802_3 + uint32_t max_packet_per_xfer ; ///< The maximum number of concatenated \ref RNDIS_MSG_PACKET messages that the device can handle in a single bus transfer to it. This value MUST be at least 1. + uint32_t max_xfer_size ; ///< The maximum size, in bytes, of any single bus data transfer that the device expects to receive from the host. + uint32_t packet_alignment_factor ; ///< The byte alignment the device expects for each RNDIS message that is part of a multimessage transfer to it. The value is specified as an exponent of 2; for example, the host uses 2{PacketAlignmentFactor} as the alignment value. + uint32_t reserved[2] ; +} rndis_msg_initialize_cmplt_t; + +//------------- Query -------------// +/// \brief Query Message +/// \details This message MUST be sent by the host to query an OID. +typedef struct { + uint32_t type ; ///< Message Type, must be \ref RNDIS_MSG_QUERY + uint32_t length ; ///< Message length in bytes, including the header and the \a oid_buffer + uint32_t request_id ; ///< A 32-bit integer value, generated by the host, used to match the host's sent request to the response from the device. + uint32_t oid ; ///< The integer value of the host operating system-defined identifier, for the parameter of the device being queried for. + uint32_t buffer_length ; ///< The length, in bytes, of the input data required for the OID query. This MUST be set to 0 when there is no input data associated with the OID. + uint32_t buffer_offset ; ///< The offset, in bytes, from the beginning of \a request_id field where the input data for the query is located in the message. This value MUST be set to 0 when there is no input data associated with the OID. + uint32_t reserved ; + uint8_t oid_buffer[] ; ///< Flexible array contains the input data supplied by the host, required for the OID query request processing by the device, as per the host NDIS specification. +} rndis_msg_query_t, rndis_msg_set_t; + +TU_VERIFY_STATIC(sizeof(rndis_msg_query_t) == 28, "Make sure flexible array member does not affect layout"); + +/// \brief Query Complete Message +/// \details This message MUST be sent by the device in response to a query OID message. +typedef struct { + uint32_t type ; ///< Message Type, must be \ref RNDIS_MSG_QUERY_CMPLT + uint32_t length ; ///< Message length in bytes, including the header and the \a oid_buffer + uint32_t request_id ; ///< A 32-bit integer value from \a request_id field of the \ref rndis_msg_query_t to which this message is a response. + uint32_t status ; ///< The status of processing for the query request, has value from \ref rndis_msg_status_t. + uint32_t buffer_length ; ///< The length, in bytes, of the data in the response to the query. This MUST be set to 0 when there is no OIDInputBuffer. + uint32_t buffer_offset ; ///< The offset, in bytes, from the beginning of \a request_id field where the response data for the query is located in the message. This MUST be set to 0 when there is no \ref oid_buffer. + uint8_t oid_buffer[] ; ///< Flexible array member contains the response data to the OID query request as specified by the host. +} rndis_msg_query_cmplt_t; + +TU_VERIFY_STATIC(sizeof(rndis_msg_query_cmplt_t) == 24, "Make sure flexible array member does not affect layout"); + +//------------- Reset -------------// +/// \brief Reset Message +/// \details This message MUST be sent by the host to perform a soft reset on the device. +typedef struct { + uint32_t type ; ///< Message Type, must be \ref RNDIS_MSG_RESET + uint32_t length ; ///< Message length in bytes, MUST be 0x06 + uint32_t reserved ; +} rndis_msg_reset_t; + +/// \brief Reset Complete Message +/// \details This message MUST be sent by the device in response to a reset message. +typedef struct { + uint32_t type ; ///< Message Type, must be \ref RNDIS_MSG_RESET_CMPLT + uint32_t length ; ///< Message length in bytes, MUST be 0x10 + uint32_t status ; ///< The status of processing for the \ref rndis_msg_reset_t, has value from \ref rndis_msg_status_t. + uint32_t addressing_reset ; ///< This field indicates whether the addressing information, which is the multicast address list or packet filter, has been lost during the reset operation. This MUST be set to 0x00000001 if the device requires that the host to resend addressing information or MUST be set to zero otherwise. +} rndis_msg_reset_cmplt_t; + +//typedef struct { +// uint32_t type; +// uint32_t length; +// uint32_t status; +// uint32_t buffer_length; +// uint32_t buffer_offset; +// uint32_t diagnostic_status; // optional +// uint32_t diagnostic_error_offset; // optional +// uint32_t status_buffer[0]; // optional +//} rndis_msg_indicate_status_t; + +/// \brief Keep Alive Message +/// \details This message MUST be sent by the host to check that device is still responsive. It is optional for the device to send this message to check if the host is active +typedef struct { + uint32_t type ; ///< Message Type + uint32_t length ; ///< Message length in bytes, MUST be 0x10 + uint32_t request_id ; +} rndis_msg_keep_alive_t, rndis_msg_halt_t; + +/// \brief Set Complete Message +/// \brief This message MUST be sent in response to a the request message +typedef struct { + uint32_t type ; ///< Message Type + uint32_t length ; ///< Message length in bytes, MUST be 0x10 + uint32_t request_id ; ///< must be the same as requesting message + uint32_t status ; ///< The status of processing for the request message request by the device to which this message is the response. +} rndis_msg_set_cmplt_t, rndis_msg_keep_alive_cmplt_t; + +/// \brief Packet Data Message +/// \brief This message MUST be used by the host and the device to send network data to one another. +typedef struct { + uint32_t type ; ///< Message Type, must be \ref RNDIS_MSG_PACKET + uint32_t length ; ///< Message length in bytes, The total length of this RNDIS message including the header, payload, and padding. + uint32_t data_offset ; ///< Specifies the offset, in bytes, from the start of this \a data_offset field of this message to the start of the data. This MUST be an integer multiple of 4. + uint32_t data_length ; ///< Specifies the number of bytes in the payload of this message. + uint32_t out_of_band_data_offet ; ///< Specifies the offset, in bytes, of the first out-of-band data record from the start of the DataOffset field in this message. MUST be an integer multiple of 4 when out-of-band data is present or set to 0 otherwise. When there are multiple out-ofband data records, each subsequent record MUST immediately follow the previous out-of-band data record. + uint32_t out_of_band_data_length ; ///< Specifies, in bytes, the total length of the out-of-band data. + uint32_t num_out_of_band_data_elements ; ///< Specifies the number of out-of-band records in this message. + uint32_t per_packet_info_offset ; ///< Specifies the offset, in bytes, of the start of per-packet-info data record from the start of the \a data_offset field in this message. MUST be an integer multiple of 4 when per-packet-info data record is present or MUST be set to 0 otherwise. When there are multiple per-packet-info data records, each subsequent record MUST immediately follow the previous record. + uint32_t per_packet_info_length ; ///< Specifies, in bytes, the total length of per-packetinformation contained in this message. + uint32_t reserved[2] ; + uint32_t payload[0] ; ///< Network data contained in this message. + + // uint8_t padding[0] + // Additional bytes of zeros added at the end of the message to comply with + // the internal and external padding requirements. Internal padding SHOULD be as per the + // specification of the out-of-band data record and per-packet-info data record. The external + //padding size SHOULD be determined based on the PacketAlignmentFactor field specification + //in REMOTE_NDIS_INITIALIZE_CMPLT message by the device, when multiple + //REMOTE_NDIS_PACKET_MSG messages are bundled together in a single bus-native message. + //In this case, all but the very last REMOTE_NDIS_PACKET_MSG MUST respect the + //PacketAlignmentFactor field. + + // rndis_msg_packet_t [0] : (optional) more packet if multiple packet per bus transaction is supported +} rndis_msg_packet_t; + + +typedef struct { + uint32_t size ; ///< Length, in bytes, of this header and appended data and padding. This value MUST be an integer multiple of 4. + uint32_t type ; ///< MUST be as per host operating system specification. + uint32_t offset ; ///< The byte offset from the beginning of this record to the beginning of data. + uint32_t data[0] ; ///< Flexible array contains data +} rndis_msg_out_of_band_data_t, rndis_msg_per_packet_info_t; + +//--------------------------------------------------------------------+ +// NDIS Object ID +//--------------------------------------------------------------------+ + +/// NDIS Object ID +typedef enum +{ + //------------- General Required OIDs -------------// + RNDIS_OID_GEN_SUPPORTED_LIST = 0x00010101, ///< List of supported OIDs + RNDIS_OID_GEN_HARDWARE_STATUS = 0x00010102, ///< Hardware status + RNDIS_OID_GEN_MEDIA_SUPPORTED = 0x00010103, ///< Media types supported (encoded) + RNDIS_OID_GEN_MEDIA_IN_USE = 0x00010104, ///< Media types in use (encoded) + RNDIS_OID_GEN_MAXIMUM_LOOKAHEAD = 0x00010105, ///< + RNDIS_OID_GEN_MAXIMUM_FRAME_SIZE = 0x00010106, ///< Maximum frame size in bytes + RNDIS_OID_GEN_LINK_SPEED = 0x00010107, ///< Link speed in units of 100 bps + RNDIS_OID_GEN_TRANSMIT_BUFFER_SPACE = 0x00010108, ///< Transmit buffer space + RNDIS_OID_GEN_RECEIVE_BUFFER_SPACE = 0x00010109, ///< Receive buffer space + RNDIS_OID_GEN_TRANSMIT_BLOCK_SIZE = 0x0001010A, ///< Minimum amount of storage, in bytes, that a single packet occupies in the transmit buffer space of the NIC + RNDIS_OID_GEN_RECEIVE_BLOCK_SIZE = 0x0001010B, ///< Amount of storage, in bytes, that a single packet occupies in the receive buffer space of the NIC + RNDIS_OID_GEN_VENDOR_ID = 0x0001010C, ///< Vendor NIC code + RNDIS_OID_GEN_VENDOR_DESCRIPTION = 0x0001010D, ///< Vendor network card description + RNDIS_OID_GEN_CURRENT_PACKET_FILTER = 0x0001010E, ///< Current packet filter (encoded) + RNDIS_OID_GEN_CURRENT_LOOKAHEAD = 0x0001010F, ///< Current lookahead size in bytes + RNDIS_OID_GEN_DRIVER_VERSION = 0x00010110, ///< NDIS version number used by the driver + RNDIS_OID_GEN_MAXIMUM_TOTAL_SIZE = 0x00010111, ///< Maximum total packet length in bytes + RNDIS_OID_GEN_PROTOCOL_OPTIONS = 0x00010112, ///< Optional protocol flags (encoded) + RNDIS_OID_GEN_MAC_OPTIONS = 0x00010113, ///< Optional NIC flags (encoded) + RNDIS_OID_GEN_MEDIA_CONNECT_STATUS = 0x00010114, ///< Whether the NIC is connected to the network + RNDIS_OID_GEN_MAXIMUM_SEND_PACKETS = 0x00010115, ///< The maximum number of send packets the driver can accept per call to its MiniportSendPacketsfunction + + //------------- General Optional OIDs -------------// + RNDIS_OID_GEN_VENDOR_DRIVER_VERSION = 0x00010116, ///< Vendor-assigned version number of the driver + RNDIS_OID_GEN_SUPPORTED_GUIDS = 0x00010117, ///< The custom GUIDs (Globally Unique Identifier) supported by the miniport driver + RNDIS_OID_GEN_NETWORK_LAYER_ADDRESSES = 0x00010118, ///< List of network-layer addresses associated with the binding between a transport and the driver + RNDIS_OID_GEN_TRANSPORT_HEADER_OFFSET = 0x00010119, ///< Size of packets' additional headers + RNDIS_OID_GEN_MEDIA_CAPABILITIES = 0x00010201, ///< + RNDIS_OID_GEN_PHYSICAL_MEDIUM = 0x00010202, ///< Physical media supported by the miniport driver (encoded) + + //------------- 802.3 Objects (Ethernet) -------------// + RNDIS_OID_802_3_PERMANENT_ADDRESS = 0x01010101, ///< Permanent station address + RNDIS_OID_802_3_CURRENT_ADDRESS = 0x01010102, ///< Current station address + RNDIS_OID_802_3_MULTICAST_LIST = 0x01010103, ///< Current multicast address list + RNDIS_OID_802_3_MAXIMUM_LIST_SIZE = 0x01010104, ///< Maximum size of multicast address list +} rndis_oid_type_t; + +/// RNDIS Packet Filter Bits \ref RNDIS_OID_GEN_CURRENT_PACKET_FILTER. +typedef enum +{ + RNDIS_PACKET_TYPE_DIRECTED = 0x00000001, ///< Directed packets. Directed packets contain a destination address equal to the station address of the NIC. + RNDIS_PACKET_TYPE_MULTICAST = 0x00000002, ///< Multicast address packets sent to addresses in the multicast address list. + RNDIS_PACKET_TYPE_ALL_MULTICAST = 0x00000004, ///< All multicast address packets, not just the ones enumerated in the multicast address list. + RNDIS_PACKET_TYPE_BROADCAST = 0x00000008, ///< Broadcast packets. + RNDIS_PACKET_TYPE_SOURCE_ROUTING = 0x00000010, ///< All source routing packets. If the protocol driver sets this bit, the NDIS library attempts to act as a source routing bridge. + RNDIS_PACKET_TYPE_PROMISCUOUS = 0x00000020, ///< Specifies all packets regardless of whether VLAN filtering is enabled or not and whether the VLAN identifier matches or not. + RNDIS_PACKET_TYPE_SMT = 0x00000040, ///< SMT packets that an FDDI NIC receives. + RNDIS_PACKET_TYPE_ALL_LOCAL = 0x00000080, ///< All packets sent by installed protocols and all packets indicated by the NIC that is identified by a given NdisBindingHandle. + RNDIS_PACKET_TYPE_GROUP = 0x00001000, ///< Packets sent to the current group address. + RNDIS_PACKET_TYPE_ALL_FUNCTIONAL = 0x00002000, ///< All functional address packets, not just the ones in the current functional address. + RNDIS_PACKET_TYPE_FUNCTIONAL = 0x00004000, ///< Functional address packets sent to addresses included in the current functional address. + RNDIS_PACKET_TYPE_MAC_FRAME = 0x00008000, ///< NIC driver frames that a Token Ring NIC receives. + RNDIS_PACKET_TYPE_NO_LOCAL = 0x00010000, +} rndis_packet_filter_type_t; + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_CDC_RNDIS_H_ */ + +/** @} */ +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis_host.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis_host.c new file mode 100644 index 000000000..cc4ffd1cf --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis_host.c @@ -0,0 +1,279 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_HOST_ENABLED && CFG_TUH_CDC && CFG_TUH_CDC_RNDIS) + +//--------------------------------------------------------------------+ +// INCLUDE +//--------------------------------------------------------------------+ +#include "common/tusb_common.h" +#include "cdc_host.h" +#include "cdc_rndis_host.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +#define RNDIS_MSG_PAYLOAD_MAX (1024*4) + +CFG_TUSB_MEM_SECTION static uint8_t msg_notification[CFG_TUH_DEVICE_MAX][8]; +CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4) static uint8_t msg_payload[RNDIS_MSG_PAYLOAD_MAX]; + +static rndish_data_t rndish_data[CFG_TUH_DEVICE_MAX]; + +// TODO Microsoft requires message length for any get command must be at least 4096 bytes + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +static tusb_error_t rndis_body_subtask(void); +static tusb_error_t send_message_get_response_subtask( uint8_t dev_addr, cdch_data_t *p_cdc, + uint8_t * p_mess, uint32_t mess_length, + uint8_t *p_response ); + +//--------------------------------------------------------------------+ +// APPLICATION API +//--------------------------------------------------------------------+ +tusb_error_t tusbh_cdc_rndis_get_mac_addr(uint8_t dev_addr, uint8_t mac_address[6]) +{ + TU_ASSERT( tusbh_cdc_rndis_is_mounted(dev_addr), TUSB_ERROR_CDCH_DEVICE_NOT_MOUNTED); + TU_VERIFY( mac_address, TUSB_ERROR_INVALID_PARA); + + memcpy(mac_address, rndish_data[dev_addr-1].mac_address, 6); + + return TUSB_ERROR_NONE; +} + +//--------------------------------------------------------------------+ +// IMPLEMENTATION +//--------------------------------------------------------------------+ + +// To enable the TASK_ASSERT style (quick return on false condition) in a real RTOS, a task must act as a wrapper +// and is used mainly to call subtasks. Within a subtask return statement can be called freely, the task with +// forever loop cannot have any return at all. +OSAL_TASK_FUNCTION(cdch_rndis_task) (void* param;) +{ + OSAL_TASK_BEGIN + rndis_body_subtask(); + OSAL_TASK_END +} + +static tusb_error_t rndis_body_subtask(void) +{ + static uint8_t relative_addr; + + OSAL_SUBTASK_BEGIN + + for (relative_addr = 0; relative_addr < CFG_TUH_DEVICE_MAX; relative_addr++) + { + + } + + osal_task_delay(100); + + OSAL_SUBTASK_END +} + +//--------------------------------------------------------------------+ +// RNDIS-CDC Driver API +//--------------------------------------------------------------------+ +void rndish_init(void) +{ + tu_memclr(rndish_data, sizeof(rndish_data_t)*CFG_TUH_DEVICE_MAX); + + //------------- Task creation -------------// + + //------------- semaphore creation for notificaiton pipe -------------// + for(uint8_t i=0; itype == RNDIS_MSG_INITIALIZE_CMPLT && p_init_cmpt->status == RNDIS_STATUS_SUCCESS && + p_init_cmpt->max_packet_per_xfer == 1 && p_init_cmpt->max_xfer_size <= RNDIS_MSG_PAYLOAD_MAX); + rndish_data[dev_addr-1].max_xfer_size = p_init_cmpt->max_xfer_size; + + //------------- Message Query 802.3 Permanent Address -------------// + memcpy(msg_payload, &msg_query_permanent_addr, sizeof(rndis_msg_query_t)); + tu_memclr(msg_payload + sizeof(rndis_msg_query_t), 6); // 6 bytes for MAC address + + STASK_INVOKE( + send_message_get_response_subtask( dev_addr, p_cdc, + msg_payload, sizeof(rndis_msg_query_t) + 6, + msg_payload), + error + ); + if ( TUSB_ERROR_NONE != error ) STASK_RETURN(error); + + rndis_msg_query_cmplt_t * const p_query_cmpt = (rndis_msg_query_cmplt_t *) msg_payload; + STASK_ASSERT(p_query_cmpt->type == RNDIS_MSG_QUERY_CMPLT && p_query_cmpt->status == RNDIS_STATUS_SUCCESS); + memcpy(rndish_data[dev_addr-1].mac_address, msg_payload + 8 + p_query_cmpt->buffer_offset, 6); + + //------------- Set OID_GEN_CURRENT_PACKET_FILTER to (DIRECTED | MULTICAST | BROADCAST) -------------// + memcpy(msg_payload, &msg_set_packet_filter, sizeof(rndis_msg_set_t)); + tu_memclr(msg_payload + sizeof(rndis_msg_set_t), 4); // 4 bytes for filter flags + ((rndis_msg_set_t*) msg_payload)->oid_buffer[0] = (RNDIS_PACKET_TYPE_DIRECTED | RNDIS_PACKET_TYPE_MULTICAST | RNDIS_PACKET_TYPE_BROADCAST); + + STASK_INVOKE( + send_message_get_response_subtask( dev_addr, p_cdc, + msg_payload, sizeof(rndis_msg_set_t) + 4, + msg_payload), + error + ); + if ( TUSB_ERROR_NONE != error ) STASK_RETURN(error); + + rndis_msg_set_cmplt_t * const p_set_cmpt = (rndis_msg_set_cmplt_t *) msg_payload; + STASK_ASSERT(p_set_cmpt->type == RNDIS_MSG_SET_CMPLT && p_set_cmpt->status == RNDIS_STATUS_SUCCESS); + + tusbh_cdc_rndis_mounted_cb(dev_addr); + + OSAL_SUBTASK_END +} + +void rndish_xfer_isr(cdch_data_t *p_cdc, pipe_handle_t pipe_hdl, xfer_result_t event, uint32_t xferred_bytes) +{ + if ( pipehandle_is_equal(pipe_hdl, p_cdc->pipe_notification) ) + { + osal_semaphore_post( rndish_data[pipe_hdl.dev_addr-1].sem_notification_hdl ); + } +} + +//--------------------------------------------------------------------+ +// INTERNAL & HELPER +//--------------------------------------------------------------------+ +static tusb_error_t send_message_get_response_subtask( uint8_t dev_addr, cdch_data_t *p_cdc, + uint8_t * p_mess, uint32_t mess_length, + uint8_t *p_response) +{ + tusb_error_t error; + + OSAL_SUBTASK_BEGIN + + //------------- Send RNDIS Control Message -------------// + STASK_INVOKE( + usbh_control_xfer_subtask( dev_addr, bm_request_type(TUSB_DIR_OUT, TUSB_REQ_TYPE_CLASS, TUSB_REQ_RCPT_INTERFACE), + CDC_REQUEST_SEND_ENCAPSULATED_COMMAND, 0, p_cdc->interface_number, + mess_length, p_mess), + error + ); + if ( TUSB_ERROR_NONE != error ) STASK_RETURN(error); + + //------------- waiting for Response Available notification -------------// + (void) usbh_edpt_xfer(p_cdc->pipe_notification, msg_notification[dev_addr-1], 8); + osal_semaphore_wait(rndish_data[dev_addr-1].sem_notification_hdl, OSAL_TIMEOUT_NORMAL, &error); + if ( TUSB_ERROR_NONE != error ) STASK_RETURN(error); + STASK_ASSERT(msg_notification[dev_addr-1][0] == 1); + + //------------- Get RNDIS Message Initialize Complete -------------// + STASK_INVOKE( + usbh_control_xfer_subtask( dev_addr, bm_request_type(TUSB_DIR_IN, TUSB_REQ_TYPE_CLASS, TUSB_REQ_RCPT_INTERFACE), + CDC_REQUEST_GET_ENCAPSULATED_RESPONSE, 0, p_cdc->interface_number, + RNDIS_MSG_PAYLOAD_MAX, p_response), + error + ); + if ( TUSB_ERROR_NONE != error ) STASK_RETURN(error); + + OSAL_SUBTASK_END +} + +//static tusb_error_t send_process_msg_initialize_subtask(uint8_t dev_addr, cdch_data_t *p_cdc) +//{ +// tusb_error_t error; +// +// OSAL_SUBTASK_BEGIN +// +// *((rndis_msg_initialize_t*) msg_payload) = (rndis_msg_initialize_t) +// { +// .type = RNDIS_MSG_INITIALIZE, +// .length = sizeof(rndis_msg_initialize_t), +// .request_id = 1, // TODO should use some magic number +// .major_version = 1, +// .minor_version = 0, +// .max_xfer_size = 0x4000 // TODO mimic windows +// }; +// +// +// +// OSAL_SUBTASK_END +//} +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis_host.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis_host.h new file mode 100644 index 000000000..170cb3b0e --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/cdc/cdc_rndis_host.h @@ -0,0 +1,63 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup CDC_RNDIS + * \defgroup CDC_RNSID_Host Host + * @{ */ + +#ifndef _TUSB_CDC_RNDIS_HOST_H_ +#define _TUSB_CDC_RNDIS_HOST_H_ + +#include "common/tusb_common.h" +#include "host/usbh.h" +#include "cdc_rndis.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// INTERNAL RNDIS-CDC Driver API +//--------------------------------------------------------------------+ +typedef struct { + OSAL_SEM_DEF(semaphore_notification); + osal_semaphore_handle_t sem_notification_hdl; // used to wait on notification pipe + uint32_t max_xfer_size; // got from device's msg initialize complete + uint8_t mac_address[6]; +}rndish_data_t; + +void rndish_init(void); +tusb_error_t rndish_open_subtask(uint8_t dev_addr, cdch_data_t *p_cdc); +void rndish_xfer_isr(cdch_data_t *p_cdc, pipe_handle_t pipe_hdl, xfer_result_t event, uint32_t xferred_bytes); +void rndish_close(uint8_t dev_addr); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_CDC_RNDIS_HOST_H_ */ + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu.h new file mode 100644 index 000000000..114c827b8 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu.h @@ -0,0 +1,119 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021 XMOS LIMITED + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_DFU_H_ +#define _TUSB_DFU_H_ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Common Definitions +//--------------------------------------------------------------------+ + +// DFU Protocol +typedef enum +{ + DFU_PROTOCOL_RT = 0x01, + DFU_PROTOCOL_DFU = 0x02, +} dfu_protocol_type_t; + +// DFU Descriptor Type +typedef enum +{ + DFU_DESC_FUNCTIONAL = 0x21, +} dfu_descriptor_type_t; + +// DFU Requests +typedef enum { + DFU_REQUEST_DETACH = 0, + DFU_REQUEST_DNLOAD = 1, + DFU_REQUEST_UPLOAD = 2, + DFU_REQUEST_GETSTATUS = 3, + DFU_REQUEST_CLRSTATUS = 4, + DFU_REQUEST_GETSTATE = 5, + DFU_REQUEST_ABORT = 6, +} dfu_requests_t; + +// DFU States +typedef enum { + APP_IDLE = 0, + APP_DETACH = 1, + DFU_IDLE = 2, + DFU_DNLOAD_SYNC = 3, + DFU_DNBUSY = 4, + DFU_DNLOAD_IDLE = 5, + DFU_MANIFEST_SYNC = 6, + DFU_MANIFEST = 7, + DFU_MANIFEST_WAIT_RESET = 8, + DFU_UPLOAD_IDLE = 9, + DFU_ERROR = 10, +} dfu_state_t; + +// DFU Status +typedef enum { + DFU_STATUS_OK = 0x00, + DFU_STATUS_ERR_TARGET = 0x01, + DFU_STATUS_ERR_FILE = 0x02, + DFU_STATUS_ERR_WRITE = 0x03, + DFU_STATUS_ERR_ERASE = 0x04, + DFU_STATUS_ERR_CHECK_ERASED = 0x05, + DFU_STATUS_ERR_PROG = 0x06, + DFU_STATUS_ERR_VERIFY = 0x07, + DFU_STATUS_ERR_ADDRESS = 0x08, + DFU_STATUS_ERR_NOTDONE = 0x09, + DFU_STATUS_ERR_FIRMWARE = 0x0A, + DFU_STATUS_ERR_VENDOR = 0x0B, + DFU_STATUS_ERR_USBR = 0x0C, + DFU_STATUS_ERR_POR = 0x0D, + DFU_STATUS_ERR_UNKNOWN = 0x0E, + DFU_STATUS_ERR_STALLEDPKT = 0x0F, +} dfu_status_t; + +#define DFU_ATTR_CAN_DOWNLOAD (1u << 0) +#define DFU_ATTR_CAN_UPLOAD (1u << 1) +#define DFU_ATTR_MANIFESTATION_TOLERANT (1u << 2) +#define DFU_ATTR_WILL_DETACH (1u << 3) + +// DFU Status Request Payload +typedef struct TU_ATTR_PACKED +{ + uint8_t bStatus; + uint8_t bwPollTimeout[3]; + uint8_t bState; + uint8_t iString; +} dfu_status_response_t; + +TU_VERIFY_STATIC( sizeof(dfu_status_response_t) == 6, "size is not correct"); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_DFU_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_device.c new file mode 100644 index 000000000..ddfa608e4 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_device.c @@ -0,0 +1,458 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021 XMOS LIMITED + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_DFU) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "dfu_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +typedef struct +{ + uint8_t attrs; + uint8_t alt; + + dfu_state_t state; + dfu_status_t status; + + bool flashing_in_progress; + uint16_t block; + uint16_t length; + + CFG_TUSB_MEM_ALIGN uint8_t transfer_buf[CFG_TUD_DFU_XFER_BUFSIZE]; +} dfu_state_ctx_t; + +// Only a single dfu state is allowed +CFG_TUSB_MEM_SECTION static dfu_state_ctx_t _dfu_ctx; + +static void reset_state(void) +{ + _dfu_ctx.state = DFU_IDLE; + _dfu_ctx.status = DFU_STATUS_OK; + _dfu_ctx.flashing_in_progress = false; +} + +static bool reply_getstatus(uint8_t rhport, tusb_control_request_t const * request, dfu_state_t state, dfu_status_t status, uint32_t timeout); +static bool process_download_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +static bool process_manifest_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); + +//--------------------------------------------------------------------+ +// Debug +//--------------------------------------------------------------------+ +#if CFG_TUSB_DEBUG >= 2 + +static tu_lookup_entry_t const _dfu_request_lookup[] = +{ + { .key = DFU_REQUEST_DETACH , .data = "DETACH" }, + { .key = DFU_REQUEST_DNLOAD , .data = "DNLOAD" }, + { .key = DFU_REQUEST_UPLOAD , .data = "UPLOAD" }, + { .key = DFU_REQUEST_GETSTATUS , .data = "GETSTATUS" }, + { .key = DFU_REQUEST_CLRSTATUS , .data = "CLRSTATUS" }, + { .key = DFU_REQUEST_GETSTATE , .data = "GETSTATE" }, + { .key = DFU_REQUEST_ABORT , .data = "ABORT" }, +}; + +static tu_lookup_table_t const _dfu_request_table = +{ + .count = TU_ARRAY_SIZE(_dfu_request_lookup), + .items = _dfu_request_lookup +}; + +static tu_lookup_entry_t const _dfu_state_lookup[] = +{ + { .key = APP_IDLE , .data = "APP_IDLE" }, + { .key = APP_DETACH , .data = "APP_DETACH" }, + { .key = DFU_IDLE , .data = "IDLE" }, + { .key = DFU_DNLOAD_SYNC , .data = "DNLOAD_SYNC" }, + { .key = DFU_DNBUSY , .data = "DNBUSY" }, + { .key = DFU_DNLOAD_IDLE , .data = "DNLOAD_IDLE" }, + { .key = DFU_MANIFEST_SYNC , .data = "MANIFEST_SYNC" }, + { .key = DFU_MANIFEST , .data = "MANIFEST" }, + { .key = DFU_MANIFEST_WAIT_RESET , .data = "MANIFEST_WAIT_RESET" }, + { .key = DFU_UPLOAD_IDLE , .data = "UPLOAD_IDLE" }, + { .key = DFU_ERROR , .data = "ERROR" }, +}; + +static tu_lookup_table_t const _dfu_state_table = +{ + .count = TU_ARRAY_SIZE(_dfu_state_lookup), + .items = _dfu_state_lookup +}; + +static tu_lookup_entry_t const _dfu_status_lookup[] = +{ + { .key = DFU_STATUS_OK , .data = "OK" }, + { .key = DFU_STATUS_ERR_TARGET , .data = "errTARGET" }, + { .key = DFU_STATUS_ERR_FILE , .data = "errFILE" }, + { .key = DFU_STATUS_ERR_WRITE , .data = "errWRITE" }, + { .key = DFU_STATUS_ERR_ERASE , .data = "errERASE" }, + { .key = DFU_STATUS_ERR_CHECK_ERASED , .data = "errCHECK_ERASED" }, + { .key = DFU_STATUS_ERR_PROG , .data = "errPROG" }, + { .key = DFU_STATUS_ERR_VERIFY , .data = "errVERIFY" }, + { .key = DFU_STATUS_ERR_ADDRESS , .data = "errADDRESS" }, + { .key = DFU_STATUS_ERR_NOTDONE , .data = "errNOTDONE" }, + { .key = DFU_STATUS_ERR_FIRMWARE , .data = "errFIRMWARE" }, + { .key = DFU_STATUS_ERR_VENDOR , .data = "errVENDOR" }, + { .key = DFU_STATUS_ERR_USBR , .data = "errUSBR" }, + { .key = DFU_STATUS_ERR_POR , .data = "errPOR" }, + { .key = DFU_STATUS_ERR_UNKNOWN , .data = "errUNKNOWN" }, + { .key = DFU_STATUS_ERR_STALLEDPKT , .data = "errSTALLEDPKT" }, +}; + +static tu_lookup_table_t const _dfu_status_table = +{ + .count = TU_ARRAY_SIZE(_dfu_status_lookup), + .items = _dfu_status_lookup +}; + +#endif + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void dfu_moded_reset(uint8_t rhport) +{ + (void) rhport; + + _dfu_ctx.attrs = 0; + _dfu_ctx.alt = 0; + + reset_state(); +} + +void dfu_moded_init(void) +{ + dfu_moded_reset(0); +} + +uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + (void) rhport; + + //------------- Interface (with Alt) descriptor -------------// + uint8_t const itf_num = itf_desc->bInterfaceNumber; + uint8_t alt_count = 0; + + uint16_t drv_len = 0; + while(itf_desc->bInterfaceSubClass == TUD_DFU_APP_SUBCLASS && itf_desc->bInterfaceProtocol == DFU_PROTOCOL_DFU) + { + TU_ASSERT(max_len > drv_len, 0); + + // Alternate must have the same interface number + TU_ASSERT(itf_desc->bInterfaceNumber == itf_num, 0); + + // Alt should increase by one every time + TU_ASSERT(itf_desc->bAlternateSetting == alt_count, 0); + alt_count++; + + drv_len += tu_desc_len(itf_desc); + itf_desc = (tusb_desc_interface_t const *) tu_desc_next(itf_desc); + } + + //------------- DFU Functional descriptor -------------// + tusb_desc_dfu_functional_t const *func_desc = (tusb_desc_dfu_functional_t const *) itf_desc; + TU_ASSERT(tu_desc_type(func_desc) == TUSB_DESC_FUNCTIONAL, 0); + drv_len += sizeof(tusb_desc_dfu_functional_t); + + _dfu_ctx.attrs = func_desc->bAttributes; + + // CFG_TUD_DFU_XFER_BUFSIZE has to be set to the buffer size used in TUD_DFU_DESCRIPTOR + uint16_t const transfer_size = tu_le16toh( tu_unaligned_read16((uint8_t const*) func_desc + offsetof(tusb_desc_dfu_functional_t, wTransferSize)) ); + TU_ASSERT(transfer_size <= CFG_TUD_DFU_XFER_BUFSIZE, drv_len); + + return drv_len; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool dfu_moded_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE); + + TU_LOG2(" DFU State : %s, Status: %s\r\n", tu_lookup_find(&_dfu_state_table, _dfu_ctx.state), tu_lookup_find(&_dfu_status_table, _dfu_ctx.status)); + + if ( request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD ) + { + // Standard request include GET/SET_INTERFACE + switch ( request->bRequest ) + { + case TUSB_REQ_SET_INTERFACE: + if ( stage == CONTROL_STAGE_SETUP ) + { + // Switch Alt interface and reset state machine + _dfu_ctx.alt = (uint8_t) request->wValue; + reset_state(); + return tud_control_status(rhport, request); + } + break; + + case TUSB_REQ_GET_INTERFACE: + if(stage == CONTROL_STAGE_SETUP) + { + return tud_control_xfer(rhport, request, &_dfu_ctx.alt, 1); + } + break; + + // unsupported request + default: return false; + } + } + else if ( request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS ) + { + TU_LOG2(" DFU Request: %s\r\n", tu_lookup_find(&_dfu_request_table, request->bRequest)); + + // Class request + switch ( request->bRequest ) + { + case DFU_REQUEST_DETACH: + if ( stage == CONTROL_STAGE_SETUP ) + { + tud_control_status(rhport, request); + } + else if ( stage == CONTROL_STAGE_ACK ) + { + if ( tud_dfu_detach_cb ) tud_dfu_detach_cb(); + } + break; + + case DFU_REQUEST_CLRSTATUS: + if ( stage == CONTROL_STAGE_SETUP ) + { + reset_state(); + tud_control_status(rhport, request); + } + break; + + case DFU_REQUEST_GETSTATE: + if ( stage == CONTROL_STAGE_SETUP ) + { + tud_control_xfer(rhport, request, &_dfu_ctx.state, 1); + } + break; + + case DFU_REQUEST_ABORT: + if ( stage == CONTROL_STAGE_SETUP ) + { + reset_state(); + tud_control_status(rhport, request); + } + else if ( stage == CONTROL_STAGE_ACK ) + { + if ( tud_dfu_abort_cb ) tud_dfu_abort_cb(_dfu_ctx.alt); + } + break; + + case DFU_REQUEST_UPLOAD: + if ( stage == CONTROL_STAGE_SETUP ) + { + TU_VERIFY(_dfu_ctx.attrs & DFU_ATTR_CAN_UPLOAD); + TU_VERIFY(tud_dfu_upload_cb); + TU_VERIFY(request->wLength <= CFG_TUD_DFU_XFER_BUFSIZE); + + uint16_t const xfer_len = tud_dfu_upload_cb(_dfu_ctx.alt, request->wValue, _dfu_ctx.transfer_buf, request->wLength); + + return tud_control_xfer(rhport, request, _dfu_ctx.transfer_buf, xfer_len); + } + break; + + case DFU_REQUEST_DNLOAD: + if ( stage == CONTROL_STAGE_SETUP ) + { + TU_VERIFY(_dfu_ctx.attrs & DFU_ATTR_CAN_DOWNLOAD); + TU_VERIFY(_dfu_ctx.state == DFU_IDLE || _dfu_ctx.state == DFU_DNLOAD_IDLE); + TU_VERIFY(request->wLength <= CFG_TUD_DFU_XFER_BUFSIZE); + + // set to true for both download and manifest + _dfu_ctx.flashing_in_progress = true; + + // save block and length for flashing + _dfu_ctx.block = request->wValue; + _dfu_ctx.length = request->wLength; + + if ( request->wLength ) + { + // Download with payload -> transition to DOWNLOAD SYNC + _dfu_ctx.state = DFU_DNLOAD_SYNC; + return tud_control_xfer(rhport, request, _dfu_ctx.transfer_buf, request->wLength); + } + else + { + // Download is complete -> transition to MANIFEST SYNC + _dfu_ctx.state = DFU_MANIFEST_SYNC; + return tud_control_status(rhport, request); + } + } + break; + + case DFU_REQUEST_GETSTATUS: + switch ( _dfu_ctx.state ) + { + case DFU_DNLOAD_SYNC: + return process_download_get_status(rhport, stage, request); + break; + + case DFU_MANIFEST_SYNC: + return process_manifest_get_status(rhport, stage, request); + break; + + default: + if ( stage == CONTROL_STAGE_SETUP ) return reply_getstatus(rhport, request, _dfu_ctx.state, _dfu_ctx.status, 0); + break; + } + break; + + default: return false; // stall unsupported request + } + }else + { + return false; // unsupported request + } + + return true; +} + +void tud_dfu_finish_flashing(uint8_t status) +{ + _dfu_ctx.flashing_in_progress = false; + + if ( status == DFU_STATUS_OK ) + { + if (_dfu_ctx.state == DFU_DNBUSY) + { + _dfu_ctx.state = DFU_DNLOAD_SYNC; + } + else if (_dfu_ctx.state == DFU_MANIFEST) + { + _dfu_ctx.state = (_dfu_ctx.attrs & DFU_ATTR_MANIFESTATION_TOLERANT) + ? DFU_MANIFEST_SYNC : DFU_MANIFEST_WAIT_RESET; + } + } + else + { + // failed while flashing, move to dfuError + _dfu_ctx.state = DFU_ERROR; + _dfu_ctx.status = (dfu_status_t)status; + } +} + +static bool process_download_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + if ( stage == CONTROL_STAGE_SETUP ) + { + // only transition to next state on CONTROL_STAGE_ACK + dfu_state_t next_state; + uint32_t timeout; + + if ( _dfu_ctx.flashing_in_progress ) + { + next_state = DFU_DNBUSY; + timeout = tud_dfu_get_timeout_cb(_dfu_ctx.alt, (uint8_t) next_state); + } + else + { + next_state = DFU_DNLOAD_IDLE; + timeout = 0; + } + + return reply_getstatus(rhport, request, next_state, _dfu_ctx.status, timeout); + } + else if ( stage == CONTROL_STAGE_ACK ) + { + if ( _dfu_ctx.flashing_in_progress ) + { + _dfu_ctx.state = DFU_DNBUSY; + tud_dfu_download_cb(_dfu_ctx.alt, _dfu_ctx.block, _dfu_ctx.transfer_buf, _dfu_ctx.length); + }else + { + _dfu_ctx.state = DFU_DNLOAD_IDLE; + } + } + + return true; +} + +static bool process_manifest_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + if ( stage == CONTROL_STAGE_SETUP ) + { + // only transition to next state on CONTROL_STAGE_ACK + dfu_state_t next_state; + uint32_t timeout; + + if ( _dfu_ctx.flashing_in_progress ) + { + next_state = DFU_MANIFEST; + timeout = tud_dfu_get_timeout_cb(_dfu_ctx.alt, next_state); + } + else + { + next_state = DFU_IDLE; + timeout = 0; + } + + return reply_getstatus(rhport, request, next_state, _dfu_ctx.status, timeout); + } + else if ( stage == CONTROL_STAGE_ACK ) + { + if ( _dfu_ctx.flashing_in_progress ) + { + _dfu_ctx.state = DFU_MANIFEST; + tud_dfu_manifest_cb(_dfu_ctx.alt); + } + else + { + _dfu_ctx.state = DFU_IDLE; + } + } + + return true; +} + +static bool reply_getstatus(uint8_t rhport, tusb_control_request_t const * request, dfu_state_t state, dfu_status_t status, uint32_t timeout) +{ + dfu_status_response_t resp; + resp.bStatus = (uint8_t) status; + resp.bwPollTimeout[0] = TU_U32_BYTE0(timeout); + resp.bwPollTimeout[1] = TU_U32_BYTE1(timeout); + resp.bwPollTimeout[2] = TU_U32_BYTE2(timeout); + resp.bState = (uint8_t) state; + resp.iString = 0; + + return tud_control_xfer(rhport, request, &resp, sizeof(dfu_status_response_t)); +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_device.h new file mode 100644 index 000000000..fecf8596f --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_device.h @@ -0,0 +1,98 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021 XMOS LIMITED + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_DFU_DEVICE_H_ +#define _TUSB_DFU_DEVICE_H_ + +#include "dfu.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Driver Default Configure & Validation +//--------------------------------------------------------------------+ + +#if !defined(CFG_TUD_DFU_XFER_BUFSIZE) + #error "CFG_TUD_DFU_XFER_BUFSIZE must be defined, it has to be set to the buffer size used in TUD_DFU_DESCRIPTOR" +#endif + +//--------------------------------------------------------------------+ +// Application API +//--------------------------------------------------------------------+ + +// Must be called when the application is done with flashing started by +// tud_dfu_download_cb() and tud_dfu_manifest_cb(). +// status is DFU_STATUS_OK if successful, any other error status will cause state to enter dfuError +void tud_dfu_finish_flashing(uint8_t status); + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ + +// Note: alt is used as the partition number, in order to support multiple partitions like FLASH, EEPROM, etc. + +// Invoked right before tud_dfu_download_cb() (state=DFU_DNBUSY) or tud_dfu_manifest_cb() (state=DFU_MANIFEST) +// Application return timeout in milliseconds (bwPollTimeout) for the next download/manifest operation. +// During this period, USB host won't try to communicate with us. +uint32_t tud_dfu_get_timeout_cb(uint8_t alt, uint8_t state); + +// Invoked when received DFU_DNLOAD (wLength>0) following by DFU_GETSTATUS (state=DFU_DNBUSY) requests +// This callback could be returned before flashing op is complete (async). +// Once finished flashing, application must call tud_dfu_finish_flashing() +void tud_dfu_download_cb (uint8_t alt, uint16_t block_num, uint8_t const *data, uint16_t length); + +// Invoked when download process is complete, received DFU_DNLOAD (wLength=0) following by DFU_GETSTATUS (state=Manifest) +// Application can do checksum, or actual flashing if buffered entire image previously. +// Once finished flashing, application must call tud_dfu_finish_flashing() +void tud_dfu_manifest_cb(uint8_t alt); + +// Invoked when received DFU_UPLOAD request +// Application must populate data with up to length bytes and +// Return the number of written bytes +TU_ATTR_WEAK uint16_t tud_dfu_upload_cb(uint8_t alt, uint16_t block_num, uint8_t* data, uint16_t length); + +// Invoked when a DFU_DETACH request is received +TU_ATTR_WEAK void tud_dfu_detach_cb(void); + +// Invoked when the Host has terminated a download or upload transfer +TU_ATTR_WEAK void tud_dfu_abort_cb(uint8_t alt); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void dfu_moded_init(void); +void dfu_moded_reset(uint8_t rhport); +uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool dfu_moded_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); + + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_DFU_MODE_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_rt_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_rt_device.c new file mode 100644 index 000000000..afee2aa1f --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_rt_device.c @@ -0,0 +1,128 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Sylvain Munaut + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_DFU_RUNTIME) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "dfu_rt_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void dfu_rtd_init(void) +{ +} + +void dfu_rtd_reset(uint8_t rhport) +{ + (void) rhport; +} + +uint16_t dfu_rtd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + (void) rhport; + (void) max_len; + + // Ensure this is DFU Runtime + TU_VERIFY((itf_desc->bInterfaceSubClass == TUD_DFU_APP_SUBCLASS) && + (itf_desc->bInterfaceProtocol == DFU_PROTOCOL_RT), 0); + + uint8_t const * p_desc = tu_desc_next( itf_desc ); + uint16_t drv_len = sizeof(tusb_desc_interface_t); + + if ( TUSB_DESC_FUNCTIONAL == tu_desc_type(p_desc) ) + { + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + return drv_len; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool dfu_rtd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + // nothing to do with DATA or ACK stage + if ( stage != CONTROL_STAGE_SETUP ) return true; + + TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE); + + // dfu-util will try to claim the interface with SET_INTERFACE request before sending DFU request + if ( TUSB_REQ_TYPE_STANDARD == request->bmRequestType_bit.type && + TUSB_REQ_SET_INTERFACE == request->bRequest ) + { + tud_control_status(rhport, request); + return true; + } + + // Handle class request only from here + TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS); + + switch (request->bRequest) + { + case DFU_REQUEST_DETACH: + { + TU_LOG2(" DFU RT Request: DETACH\r\n"); + tud_control_status(rhport, request); + tud_dfu_runtime_reboot_to_dfu_cb(); + } + break; + + case DFU_REQUEST_GETSTATUS: + { + TU_LOG2(" DFU RT Request: GETSTATUS\r\n"); + dfu_status_response_t resp; + // Status = OK, Poll timeout is ignored during RT, State = APP_IDLE, IString = 0 + memset(&resp, 0x00, sizeof(dfu_status_response_t)); + tud_control_xfer(rhport, request, &resp, sizeof(dfu_status_response_t)); + } + break; + + default: + { + TU_LOG2(" DFU RT Unexpected Request: %d\r\n", request->bRequest); + return false; // stall unsupported request + } + } + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_rt_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_rt_device.h new file mode 100644 index 000000000..babaa8214 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/dfu/dfu_rt_device.h @@ -0,0 +1,54 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Sylvain Munaut + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_DFU_RT_DEVICE_H_ +#define _TUSB_DFU_RT_DEVICE_H_ + +#include "dfu.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ +// Invoked when a DFU_DETACH request is received and bitWillDetach is set +void tud_dfu_runtime_reboot_to_dfu_cb(void); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void dfu_rtd_init(void); +void dfu_rtd_reset(uint8_t rhport); +uint16_t dfu_rtd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool dfu_rtd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_DFU_RT_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid.h new file mode 100644 index 000000000..20a5dd7f9 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid.h @@ -0,0 +1,1119 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup group_class + * \defgroup ClassDriver_HID Human Interface Device (HID) + * @{ */ + +#ifndef _TUSB_HID_H_ +#define _TUSB_HID_H_ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Common Definitions +//--------------------------------------------------------------------+ +/** \defgroup ClassDriver_HID_Common Common Definitions + * @{ */ + + /// USB HID Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength; /**< Numeric expression that is the total size of the HID descriptor */ + uint8_t bDescriptorType; /**< Constant name specifying type of HID descriptor. */ + + uint16_t bcdHID; /**< Numeric expression identifying the HID Class Specification release */ + uint8_t bCountryCode; /**< Numeric expression identifying country code of the localized hardware. */ + uint8_t bNumDescriptors; /**< Numeric expression specifying the number of class descriptors */ + + uint8_t bReportType; /**< Type of HID class report. */ + uint16_t wReportLength; /**< the total size of the Report descriptor. */ +} tusb_hid_descriptor_hid_t; + +/// HID Subclass +typedef enum +{ + HID_SUBCLASS_NONE = 0, ///< No Subclass + HID_SUBCLASS_BOOT = 1 ///< Boot Interface Subclass +}hid_subclass_enum_t; + +/// HID Interface Protocol +typedef enum +{ + HID_ITF_PROTOCOL_NONE = 0, ///< None + HID_ITF_PROTOCOL_KEYBOARD = 1, ///< Keyboard + HID_ITF_PROTOCOL_MOUSE = 2 ///< Mouse +}hid_interface_protocol_enum_t; + +/// HID Descriptor Type +typedef enum +{ + HID_DESC_TYPE_HID = 0x21, ///< HID Descriptor + HID_DESC_TYPE_REPORT = 0x22, ///< Report Descriptor + HID_DESC_TYPE_PHYSICAL = 0x23 ///< Physical Descriptor +}hid_descriptor_enum_t; + +/// HID Request Report Type +typedef enum +{ + HID_REPORT_TYPE_INVALID = 0, + HID_REPORT_TYPE_INPUT, ///< Input + HID_REPORT_TYPE_OUTPUT, ///< Output + HID_REPORT_TYPE_FEATURE ///< Feature +}hid_report_type_t; + +/// HID Class Specific Control Request +typedef enum +{ + HID_REQ_CONTROL_GET_REPORT = 0x01, ///< Get Report + HID_REQ_CONTROL_GET_IDLE = 0x02, ///< Get Idle + HID_REQ_CONTROL_GET_PROTOCOL = 0x03, ///< Get Protocol + HID_REQ_CONTROL_SET_REPORT = 0x09, ///< Set Report + HID_REQ_CONTROL_SET_IDLE = 0x0a, ///< Set Idle + HID_REQ_CONTROL_SET_PROTOCOL = 0x0b ///< Set Protocol +}hid_request_enum_t; + +/// HID Local Code +typedef enum +{ + HID_LOCAL_NotSupported = 0 , ///< NotSupported + HID_LOCAL_Arabic , ///< Arabic + HID_LOCAL_Belgian , ///< Belgian + HID_LOCAL_Canadian_Bilingual , ///< Canadian_Bilingual + HID_LOCAL_Canadian_French , ///< Canadian_French + HID_LOCAL_Czech_Republic , ///< Czech_Republic + HID_LOCAL_Danish , ///< Danish + HID_LOCAL_Finnish , ///< Finnish + HID_LOCAL_French , ///< French + HID_LOCAL_German , ///< German + HID_LOCAL_Greek , ///< Greek + HID_LOCAL_Hebrew , ///< Hebrew + HID_LOCAL_Hungary , ///< Hungary + HID_LOCAL_International , ///< International + HID_LOCAL_Italian , ///< Italian + HID_LOCAL_Japan_Katakana , ///< Japan_Katakana + HID_LOCAL_Korean , ///< Korean + HID_LOCAL_Latin_American , ///< Latin_American + HID_LOCAL_Netherlands_Dutch , ///< Netherlands/Dutch + HID_LOCAL_Norwegian , ///< Norwegian + HID_LOCAL_Persian_Farsi , ///< Persian (Farsi) + HID_LOCAL_Poland , ///< Poland + HID_LOCAL_Portuguese , ///< Portuguese + HID_LOCAL_Russia , ///< Russia + HID_LOCAL_Slovakia , ///< Slovakia + HID_LOCAL_Spanish , ///< Spanish + HID_LOCAL_Swedish , ///< Swedish + HID_LOCAL_Swiss_French , ///< Swiss/French + HID_LOCAL_Swiss_German , ///< Swiss/German + HID_LOCAL_Switzerland , ///< Switzerland + HID_LOCAL_Taiwan , ///< Taiwan + HID_LOCAL_Turkish_Q , ///< Turkish-Q + HID_LOCAL_UK , ///< UK + HID_LOCAL_US , ///< US + HID_LOCAL_Yugoslavia , ///< Yugoslavia + HID_LOCAL_Turkish_F ///< Turkish-F +} hid_local_enum_t; + +// HID protocol value used by GetProtocol / SetProtocol +typedef enum +{ + HID_PROTOCOL_BOOT = 0, + HID_PROTOCOL_REPORT = 1 +} hid_protocol_mode_enum_t; + +/** @} */ + +//--------------------------------------------------------------------+ +// GAMEPAD +//--------------------------------------------------------------------+ +/** \addtogroup ClassDriver_HID_Gamepad Gamepad + * @{ */ + +/* From https://www.kernel.org/doc/html/latest/input/gamepad.html + ____________________________ __ + / [__ZL__] [__ZR__] \ | + / [__ TL __] [__ TR __] \ | Front Triggers + __/________________________________\__ __| + / _ \ | + / /\ __ (N) \ | + / || __ |MO| __ _ _ \ | Main Pad + | <===DP===> |SE| |ST| (W) -|- (E) | | + \ || ___ ___ _ / | + /\ \/ / \ / \ (S) /\ __| + / \________ | LS | ____ | RS | ________/ \ | +| / \ \___/ / \ \___/ / \ | | Control Sticks +| / \_____/ \_____/ \ | __| +| / \ | + \_____/ \_____/ + + |________|______| |______|___________| + D-Pad Left Right Action Pad + Stick Stick + + |_____________| + Menu Pad + + Most gamepads have the following features: + - Action-Pad 4 buttons in diamonds-shape (on the right side) NORTH, SOUTH, WEST and EAST. + - D-Pad (Direction-pad) 4 buttons (on the left side) that point up, down, left and right. + - Menu-Pad Different constellations, but most-times 2 buttons: SELECT - START. + - Analog-Sticks provide freely moveable sticks to control directions, Analog-sticks may also + provide a digital button if you press them. + - Triggers are located on the upper-side of the pad in vertical direction. The upper buttons + are normally named Left- and Right-Triggers, the lower buttons Z-Left and Z-Right. + - Rumble Many devices provide force-feedback features. But are mostly just simple rumble motors. + */ + +/// HID Gamepad Protocol Report. +typedef struct TU_ATTR_PACKED +{ + int8_t x; ///< Delta x movement of left analog-stick + int8_t y; ///< Delta y movement of left analog-stick + int8_t z; ///< Delta z movement of right analog-joystick + int8_t rz; ///< Delta Rz movement of right analog-joystick + int8_t rx; ///< Delta Rx movement of analog left trigger + int8_t ry; ///< Delta Ry movement of analog right trigger + uint8_t hat; ///< Buttons mask for currently pressed buttons in the DPad/hat + uint32_t buttons; ///< Buttons mask for currently pressed buttons +}hid_gamepad_report_t; + +/// Standard Gamepad Buttons Bitmap +typedef enum +{ + GAMEPAD_BUTTON_0 = TU_BIT(0), + GAMEPAD_BUTTON_1 = TU_BIT(1), + GAMEPAD_BUTTON_2 = TU_BIT(2), + GAMEPAD_BUTTON_3 = TU_BIT(3), + GAMEPAD_BUTTON_4 = TU_BIT(4), + GAMEPAD_BUTTON_5 = TU_BIT(5), + GAMEPAD_BUTTON_6 = TU_BIT(6), + GAMEPAD_BUTTON_7 = TU_BIT(7), + GAMEPAD_BUTTON_8 = TU_BIT(8), + GAMEPAD_BUTTON_9 = TU_BIT(9), + GAMEPAD_BUTTON_10 = TU_BIT(10), + GAMEPAD_BUTTON_11 = TU_BIT(11), + GAMEPAD_BUTTON_12 = TU_BIT(12), + GAMEPAD_BUTTON_13 = TU_BIT(13), + GAMEPAD_BUTTON_14 = TU_BIT(14), + GAMEPAD_BUTTON_15 = TU_BIT(15), + GAMEPAD_BUTTON_16 = TU_BIT(16), + GAMEPAD_BUTTON_17 = TU_BIT(17), + GAMEPAD_BUTTON_18 = TU_BIT(18), + GAMEPAD_BUTTON_19 = TU_BIT(19), + GAMEPAD_BUTTON_20 = TU_BIT(20), + GAMEPAD_BUTTON_21 = TU_BIT(21), + GAMEPAD_BUTTON_22 = TU_BIT(22), + GAMEPAD_BUTTON_23 = TU_BIT(23), + GAMEPAD_BUTTON_24 = TU_BIT(24), + GAMEPAD_BUTTON_25 = TU_BIT(25), + GAMEPAD_BUTTON_26 = TU_BIT(26), + GAMEPAD_BUTTON_27 = TU_BIT(27), + GAMEPAD_BUTTON_28 = TU_BIT(28), + GAMEPAD_BUTTON_29 = TU_BIT(29), + GAMEPAD_BUTTON_30 = TU_BIT(30), + GAMEPAD_BUTTON_31 = TU_BIT(31), +}hid_gamepad_button_bm_t; + +/// Standard Gamepad Buttons Naming from Linux input event codes +/// https://github.com/torvalds/linux/blob/master/include/uapi/linux/input-event-codes.h +#define GAMEPAD_BUTTON_A GAMEPAD_BUTTON_0 +#define GAMEPAD_BUTTON_SOUTH GAMEPAD_BUTTON_0 + +#define GAMEPAD_BUTTON_B GAMEPAD_BUTTON_1 +#define GAMEPAD_BUTTON_EAST GAMEPAD_BUTTON_1 + +#define GAMEPAD_BUTTON_C GAMEPAD_BUTTON_2 + +#define GAMEPAD_BUTTON_X GAMEPAD_BUTTON_3 +#define GAMEPAD_BUTTON_NORTH GAMEPAD_BUTTON_3 + +#define GAMEPAD_BUTTON_Y GAMEPAD_BUTTON_4 +#define GAMEPAD_BUTTON_WEST GAMEPAD_BUTTON_4 + +#define GAMEPAD_BUTTON_Z GAMEPAD_BUTTON_5 +#define GAMEPAD_BUTTON_TL GAMEPAD_BUTTON_6 +#define GAMEPAD_BUTTON_TR GAMEPAD_BUTTON_7 +#define GAMEPAD_BUTTON_TL2 GAMEPAD_BUTTON_8 +#define GAMEPAD_BUTTON_TR2 GAMEPAD_BUTTON_9 +#define GAMEPAD_BUTTON_SELECT GAMEPAD_BUTTON_10 +#define GAMEPAD_BUTTON_START GAMEPAD_BUTTON_11 +#define GAMEPAD_BUTTON_MODE GAMEPAD_BUTTON_12 +#define GAMEPAD_BUTTON_THUMBL GAMEPAD_BUTTON_13 +#define GAMEPAD_BUTTON_THUMBR GAMEPAD_BUTTON_14 + +/// Standard Gamepad HAT/DPAD Buttons (from Linux input event codes) +typedef enum +{ + GAMEPAD_HAT_CENTERED = 0, ///< DPAD_CENTERED + GAMEPAD_HAT_UP = 1, ///< DPAD_UP + GAMEPAD_HAT_UP_RIGHT = 2, ///< DPAD_UP_RIGHT + GAMEPAD_HAT_RIGHT = 3, ///< DPAD_RIGHT + GAMEPAD_HAT_DOWN_RIGHT = 4, ///< DPAD_DOWN_RIGHT + GAMEPAD_HAT_DOWN = 5, ///< DPAD_DOWN + GAMEPAD_HAT_DOWN_LEFT = 6, ///< DPAD_DOWN_LEFT + GAMEPAD_HAT_LEFT = 7, ///< DPAD_LEFT + GAMEPAD_HAT_UP_LEFT = 8, ///< DPAD_UP_LEFT +}hid_gamepad_hat_t; + +/// @} + +//--------------------------------------------------------------------+ +// MOUSE +//--------------------------------------------------------------------+ +/** \addtogroup ClassDriver_HID_Mouse Mouse + * @{ */ + +/// Standard HID Boot Protocol Mouse Report. +typedef struct TU_ATTR_PACKED +{ + uint8_t buttons; /**< buttons mask for currently pressed buttons in the mouse. */ + int8_t x; /**< Current delta x movement of the mouse. */ + int8_t y; /**< Current delta y movement on the mouse. */ + int8_t wheel; /**< Current delta wheel movement on the mouse. */ + int8_t pan; // using AC Pan +} hid_mouse_report_t; + +/// Standard Mouse Buttons Bitmap +typedef enum +{ + MOUSE_BUTTON_LEFT = TU_BIT(0), ///< Left button + MOUSE_BUTTON_RIGHT = TU_BIT(1), ///< Right button + MOUSE_BUTTON_MIDDLE = TU_BIT(2), ///< Middle button + MOUSE_BUTTON_BACKWARD = TU_BIT(3), ///< Backward button, + MOUSE_BUTTON_FORWARD = TU_BIT(4), ///< Forward button, +}hid_mouse_button_bm_t; + +/// @} + +//--------------------------------------------------------------------+ +// Keyboard +//--------------------------------------------------------------------+ +/** \addtogroup ClassDriver_HID_Keyboard Keyboard + * @{ */ + +/// Standard HID Boot Protocol Keyboard Report. +typedef struct TU_ATTR_PACKED +{ + uint8_t modifier; /**< Keyboard modifier (KEYBOARD_MODIFIER_* masks). */ + uint8_t reserved; /**< Reserved for OEM use, always set to 0. */ + uint8_t keycode[6]; /**< Key codes of the currently pressed keys. */ +} hid_keyboard_report_t; + +/// Keyboard modifier codes bitmap +typedef enum +{ + KEYBOARD_MODIFIER_LEFTCTRL = TU_BIT(0), ///< Left Control + KEYBOARD_MODIFIER_LEFTSHIFT = TU_BIT(1), ///< Left Shift + KEYBOARD_MODIFIER_LEFTALT = TU_BIT(2), ///< Left Alt + KEYBOARD_MODIFIER_LEFTGUI = TU_BIT(3), ///< Left Window + KEYBOARD_MODIFIER_RIGHTCTRL = TU_BIT(4), ///< Right Control + KEYBOARD_MODIFIER_RIGHTSHIFT = TU_BIT(5), ///< Right Shift + KEYBOARD_MODIFIER_RIGHTALT = TU_BIT(6), ///< Right Alt + KEYBOARD_MODIFIER_RIGHTGUI = TU_BIT(7) ///< Right Window +}hid_keyboard_modifier_bm_t; + +typedef enum +{ + KEYBOARD_LED_NUMLOCK = TU_BIT(0), ///< Num Lock LED + KEYBOARD_LED_CAPSLOCK = TU_BIT(1), ///< Caps Lock LED + KEYBOARD_LED_SCROLLLOCK = TU_BIT(2), ///< Scroll Lock LED + KEYBOARD_LED_COMPOSE = TU_BIT(3), ///< Composition Mode + KEYBOARD_LED_KANA = TU_BIT(4) ///< Kana mode +}hid_keyboard_led_bm_t; + +/// @} + +//--------------------------------------------------------------------+ +// HID KEYCODE +//--------------------------------------------------------------------+ +#define HID_KEY_NONE 0x00 +#define HID_KEY_A 0x04 +#define HID_KEY_B 0x05 +#define HID_KEY_C 0x06 +#define HID_KEY_D 0x07 +#define HID_KEY_E 0x08 +#define HID_KEY_F 0x09 +#define HID_KEY_G 0x0A +#define HID_KEY_H 0x0B +#define HID_KEY_I 0x0C +#define HID_KEY_J 0x0D +#define HID_KEY_K 0x0E +#define HID_KEY_L 0x0F +#define HID_KEY_M 0x10 +#define HID_KEY_N 0x11 +#define HID_KEY_O 0x12 +#define HID_KEY_P 0x13 +#define HID_KEY_Q 0x14 +#define HID_KEY_R 0x15 +#define HID_KEY_S 0x16 +#define HID_KEY_T 0x17 +#define HID_KEY_U 0x18 +#define HID_KEY_V 0x19 +#define HID_KEY_W 0x1A +#define HID_KEY_X 0x1B +#define HID_KEY_Y 0x1C +#define HID_KEY_Z 0x1D +#define HID_KEY_1 0x1E +#define HID_KEY_2 0x1F +#define HID_KEY_3 0x20 +#define HID_KEY_4 0x21 +#define HID_KEY_5 0x22 +#define HID_KEY_6 0x23 +#define HID_KEY_7 0x24 +#define HID_KEY_8 0x25 +#define HID_KEY_9 0x26 +#define HID_KEY_0 0x27 +#define HID_KEY_ENTER 0x28 +#define HID_KEY_ESCAPE 0x29 +#define HID_KEY_BACKSPACE 0x2A +#define HID_KEY_TAB 0x2B +#define HID_KEY_SPACE 0x2C +#define HID_KEY_MINUS 0x2D +#define HID_KEY_EQUAL 0x2E +#define HID_KEY_BRACKET_LEFT 0x2F +#define HID_KEY_BRACKET_RIGHT 0x30 +#define HID_KEY_BACKSLASH 0x31 +#define HID_KEY_EUROPE_1 0x32 +#define HID_KEY_SEMICOLON 0x33 +#define HID_KEY_APOSTROPHE 0x34 +#define HID_KEY_GRAVE 0x35 +#define HID_KEY_COMMA 0x36 +#define HID_KEY_PERIOD 0x37 +#define HID_KEY_SLASH 0x38 +#define HID_KEY_CAPS_LOCK 0x39 +#define HID_KEY_F1 0x3A +#define HID_KEY_F2 0x3B +#define HID_KEY_F3 0x3C +#define HID_KEY_F4 0x3D +#define HID_KEY_F5 0x3E +#define HID_KEY_F6 0x3F +#define HID_KEY_F7 0x40 +#define HID_KEY_F8 0x41 +#define HID_KEY_F9 0x42 +#define HID_KEY_F10 0x43 +#define HID_KEY_F11 0x44 +#define HID_KEY_F12 0x45 +#define HID_KEY_PRINT_SCREEN 0x46 +#define HID_KEY_SCROLL_LOCK 0x47 +#define HID_KEY_PAUSE 0x48 +#define HID_KEY_INSERT 0x49 +#define HID_KEY_HOME 0x4A +#define HID_KEY_PAGE_UP 0x4B +#define HID_KEY_DELETE 0x4C +#define HID_KEY_END 0x4D +#define HID_KEY_PAGE_DOWN 0x4E +#define HID_KEY_ARROW_RIGHT 0x4F +#define HID_KEY_ARROW_LEFT 0x50 +#define HID_KEY_ARROW_DOWN 0x51 +#define HID_KEY_ARROW_UP 0x52 +#define HID_KEY_NUM_LOCK 0x53 +#define HID_KEY_KEYPAD_DIVIDE 0x54 +#define HID_KEY_KEYPAD_MULTIPLY 0x55 +#define HID_KEY_KEYPAD_SUBTRACT 0x56 +#define HID_KEY_KEYPAD_ADD 0x57 +#define HID_KEY_KEYPAD_ENTER 0x58 +#define HID_KEY_KEYPAD_1 0x59 +#define HID_KEY_KEYPAD_2 0x5A +#define HID_KEY_KEYPAD_3 0x5B +#define HID_KEY_KEYPAD_4 0x5C +#define HID_KEY_KEYPAD_5 0x5D +#define HID_KEY_KEYPAD_6 0x5E +#define HID_KEY_KEYPAD_7 0x5F +#define HID_KEY_KEYPAD_8 0x60 +#define HID_KEY_KEYPAD_9 0x61 +#define HID_KEY_KEYPAD_0 0x62 +#define HID_KEY_KEYPAD_DECIMAL 0x63 +#define HID_KEY_EUROPE_2 0x64 +#define HID_KEY_APPLICATION 0x65 +#define HID_KEY_POWER 0x66 +#define HID_KEY_KEYPAD_EQUAL 0x67 +#define HID_KEY_F13 0x68 +#define HID_KEY_F14 0x69 +#define HID_KEY_F15 0x6A +#define HID_KEY_F16 0x6B +#define HID_KEY_F17 0x6C +#define HID_KEY_F18 0x6D +#define HID_KEY_F19 0x6E +#define HID_KEY_F20 0x6F +#define HID_KEY_F21 0x70 +#define HID_KEY_F22 0x71 +#define HID_KEY_F23 0x72 +#define HID_KEY_F24 0x73 +#define HID_KEY_EXECUTE 0x74 +#define HID_KEY_HELP 0x75 +#define HID_KEY_MENU 0x76 +#define HID_KEY_SELECT 0x77 +#define HID_KEY_STOP 0x78 +#define HID_KEY_AGAIN 0x79 +#define HID_KEY_UNDO 0x7A +#define HID_KEY_CUT 0x7B +#define HID_KEY_COPY 0x7C +#define HID_KEY_PASTE 0x7D +#define HID_KEY_FIND 0x7E +#define HID_KEY_MUTE 0x7F +#define HID_KEY_VOLUME_UP 0x80 +#define HID_KEY_VOLUME_DOWN 0x81 +#define HID_KEY_LOCKING_CAPS_LOCK 0x82 +#define HID_KEY_LOCKING_NUM_LOCK 0x83 +#define HID_KEY_LOCKING_SCROLL_LOCK 0x84 +#define HID_KEY_KEYPAD_COMMA 0x85 +#define HID_KEY_KEYPAD_EQUAL_SIGN 0x86 +#define HID_KEY_KANJI1 0x87 +#define HID_KEY_KANJI2 0x88 +#define HID_KEY_KANJI3 0x89 +#define HID_KEY_KANJI4 0x8A +#define HID_KEY_KANJI5 0x8B +#define HID_KEY_KANJI6 0x8C +#define HID_KEY_KANJI7 0x8D +#define HID_KEY_KANJI8 0x8E +#define HID_KEY_KANJI9 0x8F +#define HID_KEY_LANG1 0x90 +#define HID_KEY_LANG2 0x91 +#define HID_KEY_LANG3 0x92 +#define HID_KEY_LANG4 0x93 +#define HID_KEY_LANG5 0x94 +#define HID_KEY_LANG6 0x95 +#define HID_KEY_LANG7 0x96 +#define HID_KEY_LANG8 0x97 +#define HID_KEY_LANG9 0x98 +#define HID_KEY_ALTERNATE_ERASE 0x99 +#define HID_KEY_SYSREQ_ATTENTION 0x9A +#define HID_KEY_CANCEL 0x9B +#define HID_KEY_CLEAR 0x9C +#define HID_KEY_PRIOR 0x9D +#define HID_KEY_RETURN 0x9E +#define HID_KEY_SEPARATOR 0x9F +#define HID_KEY_OUT 0xA0 +#define HID_KEY_OPER 0xA1 +#define HID_KEY_CLEAR_AGAIN 0xA2 +#define HID_KEY_CRSEL_PROPS 0xA3 +#define HID_KEY_EXSEL 0xA4 +// RESERVED 0xA5-DF +#define HID_KEY_CONTROL_LEFT 0xE0 +#define HID_KEY_SHIFT_LEFT 0xE1 +#define HID_KEY_ALT_LEFT 0xE2 +#define HID_KEY_GUI_LEFT 0xE3 +#define HID_KEY_CONTROL_RIGHT 0xE4 +#define HID_KEY_SHIFT_RIGHT 0xE5 +#define HID_KEY_ALT_RIGHT 0xE6 +#define HID_KEY_GUI_RIGHT 0xE7 + + +//--------------------------------------------------------------------+ +// REPORT DESCRIPTOR +//--------------------------------------------------------------------+ + +//------------- ITEM & TAG -------------// +#define HID_REPORT_DATA_0(data) +#define HID_REPORT_DATA_1(data) , data +#define HID_REPORT_DATA_2(data) , U16_TO_U8S_LE(data) +#define HID_REPORT_DATA_3(data) , U32_TO_U8S_LE(data) + +#define HID_REPORT_ITEM(data, tag, type, size) \ + (((tag) << 4) | ((type) << 2) | (size)) HID_REPORT_DATA_##size(data) + +// Report Item Types +enum { + RI_TYPE_MAIN = 0, + RI_TYPE_GLOBAL = 1, + RI_TYPE_LOCAL = 2 +}; + +//------------- Main Items - HID 1.11 section 6.2.2.4 -------------// + +// Report Item Main group +enum { + RI_MAIN_INPUT = 8, + RI_MAIN_OUTPUT = 9, + RI_MAIN_COLLECTION = 10, + RI_MAIN_FEATURE = 11, + RI_MAIN_COLLECTION_END = 12 +}; + +#define HID_INPUT(x) HID_REPORT_ITEM(x, RI_MAIN_INPUT , RI_TYPE_MAIN, 1) +#define HID_OUTPUT(x) HID_REPORT_ITEM(x, RI_MAIN_OUTPUT , RI_TYPE_MAIN, 1) +#define HID_COLLECTION(x) HID_REPORT_ITEM(x, RI_MAIN_COLLECTION , RI_TYPE_MAIN, 1) +#define HID_FEATURE(x) HID_REPORT_ITEM(x, RI_MAIN_FEATURE , RI_TYPE_MAIN, 1) +#define HID_COLLECTION_END HID_REPORT_ITEM(x, RI_MAIN_COLLECTION_END, RI_TYPE_MAIN, 0) + +//------------- Input, Output, Feature - HID 1.11 section 6.2.2.5 -------------// +#define HID_DATA (0<<0) +#define HID_CONSTANT (1<<0) + +#define HID_ARRAY (0<<1) +#define HID_VARIABLE (1<<1) + +#define HID_ABSOLUTE (0<<2) +#define HID_RELATIVE (1<<2) + +#define HID_WRAP_NO (0<<3) +#define HID_WRAP (1<<3) + +#define HID_LINEAR (0<<4) +#define HID_NONLINEAR (1<<4) + +#define HID_PREFERRED_STATE (0<<5) +#define HID_PREFERRED_NO (1<<5) + +#define HID_NO_NULL_POSITION (0<<6) +#define HID_NULL_STATE (1<<6) + +#define HID_NON_VOLATILE (0<<7) +#define HID_VOLATILE (1<<7) + +#define HID_BITFIELD (0<<8) +#define HID_BUFFERED_BYTES (1<<8) + +//------------- Collection Item - HID 1.11 section 6.2.2.6 -------------// +enum { + HID_COLLECTION_PHYSICAL = 0, + HID_COLLECTION_APPLICATION, + HID_COLLECTION_LOGICAL, + HID_COLLECTION_REPORT, + HID_COLLECTION_NAMED_ARRAY, + HID_COLLECTION_USAGE_SWITCH, + HID_COLLECTION_USAGE_MODIFIER +}; + +//------------- Global Items - HID 1.11 section 6.2.2.7 -------------// + +// Report Item Global group +enum { + RI_GLOBAL_USAGE_PAGE = 0, + RI_GLOBAL_LOGICAL_MIN = 1, + RI_GLOBAL_LOGICAL_MAX = 2, + RI_GLOBAL_PHYSICAL_MIN = 3, + RI_GLOBAL_PHYSICAL_MAX = 4, + RI_GLOBAL_UNIT_EXPONENT = 5, + RI_GLOBAL_UNIT = 6, + RI_GLOBAL_REPORT_SIZE = 7, + RI_GLOBAL_REPORT_ID = 8, + RI_GLOBAL_REPORT_COUNT = 9, + RI_GLOBAL_PUSH = 10, + RI_GLOBAL_POP = 11 +}; + +#define HID_USAGE_PAGE(x) HID_REPORT_ITEM(x, RI_GLOBAL_USAGE_PAGE, RI_TYPE_GLOBAL, 1) +#define HID_USAGE_PAGE_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_USAGE_PAGE, RI_TYPE_GLOBAL, n) + +#define HID_LOGICAL_MIN(x) HID_REPORT_ITEM(x, RI_GLOBAL_LOGICAL_MIN, RI_TYPE_GLOBAL, 1) +#define HID_LOGICAL_MIN_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_LOGICAL_MIN, RI_TYPE_GLOBAL, n) + +#define HID_LOGICAL_MAX(x) HID_REPORT_ITEM(x, RI_GLOBAL_LOGICAL_MAX, RI_TYPE_GLOBAL, 1) +#define HID_LOGICAL_MAX_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_LOGICAL_MAX, RI_TYPE_GLOBAL, n) + +#define HID_PHYSICAL_MIN(x) HID_REPORT_ITEM(x, RI_GLOBAL_PHYSICAL_MIN, RI_TYPE_GLOBAL, 1) +#define HID_PHYSICAL_MIN_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_PHYSICAL_MIN, RI_TYPE_GLOBAL, n) + +#define HID_PHYSICAL_MAX(x) HID_REPORT_ITEM(x, RI_GLOBAL_PHYSICAL_MAX, RI_TYPE_GLOBAL, 1) +#define HID_PHYSICAL_MAX_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_PHYSICAL_MAX, RI_TYPE_GLOBAL, n) + +#define HID_UNIT_EXPONENT(x) HID_REPORT_ITEM(x, RI_GLOBAL_UNIT_EXPONENT, RI_TYPE_GLOBAL, 1) +#define HID_UNIT_EXPONENT_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_UNIT_EXPONENT, RI_TYPE_GLOBAL, n) + +#define HID_UNIT(x) HID_REPORT_ITEM(x, RI_GLOBAL_UNIT, RI_TYPE_GLOBAL, 1) +#define HID_UNIT_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_UNIT, RI_TYPE_GLOBAL, n) + +#define HID_REPORT_SIZE(x) HID_REPORT_ITEM(x, RI_GLOBAL_REPORT_SIZE, RI_TYPE_GLOBAL, 1) +#define HID_REPORT_SIZE_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_REPORT_SIZE, RI_TYPE_GLOBAL, n) + +#define HID_REPORT_ID(x) HID_REPORT_ITEM(x, RI_GLOBAL_REPORT_ID, RI_TYPE_GLOBAL, 1), +#define HID_REPORT_ID_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_REPORT_ID, RI_TYPE_GLOBAL, n), + +#define HID_REPORT_COUNT(x) HID_REPORT_ITEM(x, RI_GLOBAL_REPORT_COUNT, RI_TYPE_GLOBAL, 1) +#define HID_REPORT_COUNT_N(x, n) HID_REPORT_ITEM(x, RI_GLOBAL_REPORT_COUNT, RI_TYPE_GLOBAL, n) + +#define HID_PUSH HID_REPORT_ITEM(x, RI_GLOBAL_PUSH, RI_TYPE_GLOBAL, 0) +#define HID_POP HID_REPORT_ITEM(x, RI_GLOBAL_POP, RI_TYPE_GLOBAL, 0) + +//------------- LOCAL ITEMS 6.2.2.8 -------------// + +enum { + RI_LOCAL_USAGE = 0, + RI_LOCAL_USAGE_MIN = 1, + RI_LOCAL_USAGE_MAX = 2, + RI_LOCAL_DESIGNATOR_INDEX = 3, + RI_LOCAL_DESIGNATOR_MIN = 4, + RI_LOCAL_DESIGNATOR_MAX = 5, + // 6 is reserved + RI_LOCAL_STRING_INDEX = 7, + RI_LOCAL_STRING_MIN = 8, + RI_LOCAL_STRING_MAX = 9, + RI_LOCAL_DELIMITER = 10, +}; + +#define HID_USAGE(x) HID_REPORT_ITEM(x, RI_LOCAL_USAGE, RI_TYPE_LOCAL, 1) +#define HID_USAGE_N(x, n) HID_REPORT_ITEM(x, RI_LOCAL_USAGE, RI_TYPE_LOCAL, n) + +#define HID_USAGE_MIN(x) HID_REPORT_ITEM(x, RI_LOCAL_USAGE_MIN, RI_TYPE_LOCAL, 1) +#define HID_USAGE_MIN_N(x, n) HID_REPORT_ITEM(x, RI_LOCAL_USAGE_MIN, RI_TYPE_LOCAL, n) + +#define HID_USAGE_MAX(x) HID_REPORT_ITEM(x, RI_LOCAL_USAGE_MAX, RI_TYPE_LOCAL, 1) +#define HID_USAGE_MAX_N(x, n) HID_REPORT_ITEM(x, RI_LOCAL_USAGE_MAX, RI_TYPE_LOCAL, n) + +//--------------------------------------------------------------------+ +// Usage Table +//--------------------------------------------------------------------+ + +/// HID Usage Table - Table 1: Usage Page Summary +enum { + HID_USAGE_PAGE_DESKTOP = 0x01, + HID_USAGE_PAGE_SIMULATE = 0x02, + HID_USAGE_PAGE_VIRTUAL_REALITY = 0x03, + HID_USAGE_PAGE_SPORT = 0x04, + HID_USAGE_PAGE_GAME = 0x05, + HID_USAGE_PAGE_GENERIC_DEVICE = 0x06, + HID_USAGE_PAGE_KEYBOARD = 0x07, + HID_USAGE_PAGE_LED = 0x08, + HID_USAGE_PAGE_BUTTON = 0x09, + HID_USAGE_PAGE_ORDINAL = 0x0a, + HID_USAGE_PAGE_TELEPHONY = 0x0b, + HID_USAGE_PAGE_CONSUMER = 0x0c, + HID_USAGE_PAGE_DIGITIZER = 0x0d, + HID_USAGE_PAGE_PID = 0x0f, + HID_USAGE_PAGE_UNICODE = 0x10, + HID_USAGE_PAGE_ALPHA_DISPLAY = 0x14, + HID_USAGE_PAGE_MEDICAL = 0x40, + HID_USAGE_PAGE_MONITOR = 0x80, //0x80 - 0x83 + HID_USAGE_PAGE_POWER = 0x84, // 0x084 - 0x87 + HID_USAGE_PAGE_BARCODE_SCANNER = 0x8c, + HID_USAGE_PAGE_SCALE = 0x8d, + HID_USAGE_PAGE_MSR = 0x8e, + HID_USAGE_PAGE_CAMERA = 0x90, + HID_USAGE_PAGE_ARCADE = 0x91, + HID_USAGE_PAGE_VENDOR = 0xFF00 // 0xFF00 - 0xFFFF +}; + +/// HID Usage Table - Table 6: Generic Desktop Page +enum { + HID_USAGE_DESKTOP_POINTER = 0x01, + HID_USAGE_DESKTOP_MOUSE = 0x02, + HID_USAGE_DESKTOP_JOYSTICK = 0x04, + HID_USAGE_DESKTOP_GAMEPAD = 0x05, + HID_USAGE_DESKTOP_KEYBOARD = 0x06, + HID_USAGE_DESKTOP_KEYPAD = 0x07, + HID_USAGE_DESKTOP_MULTI_AXIS_CONTROLLER = 0x08, + HID_USAGE_DESKTOP_TABLET_PC_SYSTEM = 0x09, + HID_USAGE_DESKTOP_X = 0x30, + HID_USAGE_DESKTOP_Y = 0x31, + HID_USAGE_DESKTOP_Z = 0x32, + HID_USAGE_DESKTOP_RX = 0x33, + HID_USAGE_DESKTOP_RY = 0x34, + HID_USAGE_DESKTOP_RZ = 0x35, + HID_USAGE_DESKTOP_SLIDER = 0x36, + HID_USAGE_DESKTOP_DIAL = 0x37, + HID_USAGE_DESKTOP_WHEEL = 0x38, + HID_USAGE_DESKTOP_HAT_SWITCH = 0x39, + HID_USAGE_DESKTOP_COUNTED_BUFFER = 0x3a, + HID_USAGE_DESKTOP_BYTE_COUNT = 0x3b, + HID_USAGE_DESKTOP_MOTION_WAKEUP = 0x3c, + HID_USAGE_DESKTOP_START = 0x3d, + HID_USAGE_DESKTOP_SELECT = 0x3e, + HID_USAGE_DESKTOP_VX = 0x40, + HID_USAGE_DESKTOP_VY = 0x41, + HID_USAGE_DESKTOP_VZ = 0x42, + HID_USAGE_DESKTOP_VBRX = 0x43, + HID_USAGE_DESKTOP_VBRY = 0x44, + HID_USAGE_DESKTOP_VBRZ = 0x45, + HID_USAGE_DESKTOP_VNO = 0x46, + HID_USAGE_DESKTOP_FEATURE_NOTIFICATION = 0x47, + HID_USAGE_DESKTOP_RESOLUTION_MULTIPLIER = 0x48, + HID_USAGE_DESKTOP_SYSTEM_CONTROL = 0x80, + HID_USAGE_DESKTOP_SYSTEM_POWER_DOWN = 0x81, + HID_USAGE_DESKTOP_SYSTEM_SLEEP = 0x82, + HID_USAGE_DESKTOP_SYSTEM_WAKE_UP = 0x83, + HID_USAGE_DESKTOP_SYSTEM_CONTEXT_MENU = 0x84, + HID_USAGE_DESKTOP_SYSTEM_MAIN_MENU = 0x85, + HID_USAGE_DESKTOP_SYSTEM_APP_MENU = 0x86, + HID_USAGE_DESKTOP_SYSTEM_MENU_HELP = 0x87, + HID_USAGE_DESKTOP_SYSTEM_MENU_EXIT = 0x88, + HID_USAGE_DESKTOP_SYSTEM_MENU_SELECT = 0x89, + HID_USAGE_DESKTOP_SYSTEM_MENU_RIGHT = 0x8A, + HID_USAGE_DESKTOP_SYSTEM_MENU_LEFT = 0x8B, + HID_USAGE_DESKTOP_SYSTEM_MENU_UP = 0x8C, + HID_USAGE_DESKTOP_SYSTEM_MENU_DOWN = 0x8D, + HID_USAGE_DESKTOP_SYSTEM_COLD_RESTART = 0x8E, + HID_USAGE_DESKTOP_SYSTEM_WARM_RESTART = 0x8F, + HID_USAGE_DESKTOP_DPAD_UP = 0x90, + HID_USAGE_DESKTOP_DPAD_DOWN = 0x91, + HID_USAGE_DESKTOP_DPAD_RIGHT = 0x92, + HID_USAGE_DESKTOP_DPAD_LEFT = 0x93, + HID_USAGE_DESKTOP_SYSTEM_DOCK = 0xA0, + HID_USAGE_DESKTOP_SYSTEM_UNDOCK = 0xA1, + HID_USAGE_DESKTOP_SYSTEM_SETUP = 0xA2, + HID_USAGE_DESKTOP_SYSTEM_BREAK = 0xA3, + HID_USAGE_DESKTOP_SYSTEM_DEBUGGER_BREAK = 0xA4, + HID_USAGE_DESKTOP_APPLICATION_BREAK = 0xA5, + HID_USAGE_DESKTOP_APPLICATION_DEBUGGER_BREAK = 0xA6, + HID_USAGE_DESKTOP_SYSTEM_SPEAKER_MUTE = 0xA7, + HID_USAGE_DESKTOP_SYSTEM_HIBERNATE = 0xA8, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_INVERT = 0xB0, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_INTERNAL = 0xB1, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_EXTERNAL = 0xB2, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_BOTH = 0xB3, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_DUAL = 0xB4, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_TOGGLE_INT_EXT = 0xB5, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_SWAP_PRIMARY_SECONDARY = 0xB6, + HID_USAGE_DESKTOP_SYSTEM_DISPLAY_LCD_AUTOSCALE = 0xB7 +}; + + +/// HID Usage Table: Consumer Page (0x0C) +/// Only contains controls that supported by Windows (whole list is too long) +enum +{ + // Generic Control + HID_USAGE_CONSUMER_CONTROL = 0x0001, + + // Power Control + HID_USAGE_CONSUMER_POWER = 0x0030, + HID_USAGE_CONSUMER_RESET = 0x0031, + HID_USAGE_CONSUMER_SLEEP = 0x0032, + + // Screen Brightness + HID_USAGE_CONSUMER_BRIGHTNESS_INCREMENT = 0x006F, + HID_USAGE_CONSUMER_BRIGHTNESS_DECREMENT = 0x0070, + + // These HID usages operate only on mobile systems (battery powered) and + // require Windows 8 (build 8302 or greater). + HID_USAGE_CONSUMER_WIRELESS_RADIO_CONTROLS = 0x000C, + HID_USAGE_CONSUMER_WIRELESS_RADIO_BUTTONS = 0x00C6, + HID_USAGE_CONSUMER_WIRELESS_RADIO_LED = 0x00C7, + HID_USAGE_CONSUMER_WIRELESS_RADIO_SLIDER_SWITCH = 0x00C8, + + // Media Control + HID_USAGE_CONSUMER_PLAY_PAUSE = 0x00CD, + HID_USAGE_CONSUMER_SCAN_NEXT = 0x00B5, + HID_USAGE_CONSUMER_SCAN_PREVIOUS = 0x00B6, + HID_USAGE_CONSUMER_STOP = 0x00B7, + HID_USAGE_CONSUMER_VOLUME = 0x00E0, + HID_USAGE_CONSUMER_MUTE = 0x00E2, + HID_USAGE_CONSUMER_BASS = 0x00E3, + HID_USAGE_CONSUMER_TREBLE = 0x00E4, + HID_USAGE_CONSUMER_BASS_BOOST = 0x00E5, + HID_USAGE_CONSUMER_VOLUME_INCREMENT = 0x00E9, + HID_USAGE_CONSUMER_VOLUME_DECREMENT = 0x00EA, + HID_USAGE_CONSUMER_BASS_INCREMENT = 0x0152, + HID_USAGE_CONSUMER_BASS_DECREMENT = 0x0153, + HID_USAGE_CONSUMER_TREBLE_INCREMENT = 0x0154, + HID_USAGE_CONSUMER_TREBLE_DECREMENT = 0x0155, + + // Application Launcher + HID_USAGE_CONSUMER_AL_CONSUMER_CONTROL_CONFIGURATION = 0x0183, + HID_USAGE_CONSUMER_AL_EMAIL_READER = 0x018A, + HID_USAGE_CONSUMER_AL_CALCULATOR = 0x0192, + HID_USAGE_CONSUMER_AL_LOCAL_BROWSER = 0x0194, + + // Browser/Explorer Specific + HID_USAGE_CONSUMER_AC_SEARCH = 0x0221, + HID_USAGE_CONSUMER_AC_HOME = 0x0223, + HID_USAGE_CONSUMER_AC_BACK = 0x0224, + HID_USAGE_CONSUMER_AC_FORWARD = 0x0225, + HID_USAGE_CONSUMER_AC_STOP = 0x0226, + HID_USAGE_CONSUMER_AC_REFRESH = 0x0227, + HID_USAGE_CONSUMER_AC_BOOKMARKS = 0x022A, + + // Mouse Horizontal scroll + HID_USAGE_CONSUMER_AC_PAN = 0x0238, +}; + +/*-------------------------------------------------------------------- + * ASCII to KEYCODE Conversion + * Expand to array of [128][2] (shift, keycode) + * + * Usage: example to convert input chr into keyboard report (modifier + keycode) + * + * uint8_t const conv_table[128][2] = { HID_ASCII_TO_KEYCODE }; + * + * uint8_t keycode[6] = { 0 }; + * uint8_t modifier = 0; + * + * if ( conv_table[chr][0] ) modifier = KEYBOARD_MODIFIER_LEFTSHIFT; + * keycode[0] = conv_table[chr][1]; + * tud_hid_keyboard_report(report_id, modifier, keycode); + * + *--------------------------------------------------------------------*/ +#define HID_ASCII_TO_KEYCODE \ + {0, 0 }, /* 0x00 Null */ \ + {0, 0 }, /* 0x01 */ \ + {0, 0 }, /* 0x02 */ \ + {0, 0 }, /* 0x03 */ \ + {0, 0 }, /* 0x04 */ \ + {0, 0 }, /* 0x05 */ \ + {0, 0 }, /* 0x06 */ \ + {0, 0 }, /* 0x07 */ \ + {0, HID_KEY_BACKSPACE }, /* 0x08 Backspace */ \ + {0, HID_KEY_TAB }, /* 0x09 Tab */ \ + {0, HID_KEY_ENTER }, /* 0x0A Line Feed */ \ + {0, 0 }, /* 0x0B */ \ + {0, 0 }, /* 0x0C */ \ + {0, HID_KEY_ENTER }, /* 0x0D CR */ \ + {0, 0 }, /* 0x0E */ \ + {0, 0 }, /* 0x0F */ \ + {0, 0 }, /* 0x10 */ \ + {0, 0 }, /* 0x11 */ \ + {0, 0 }, /* 0x12 */ \ + {0, 0 }, /* 0x13 */ \ + {0, 0 }, /* 0x14 */ \ + {0, 0 }, /* 0x15 */ \ + {0, 0 }, /* 0x16 */ \ + {0, 0 }, /* 0x17 */ \ + {0, 0 }, /* 0x18 */ \ + {0, 0 }, /* 0x19 */ \ + {0, 0 }, /* 0x1A */ \ + {0, HID_KEY_ESCAPE }, /* 0x1B Escape */ \ + {0, 0 }, /* 0x1C */ \ + {0, 0 }, /* 0x1D */ \ + {0, 0 }, /* 0x1E */ \ + {0, 0 }, /* 0x1F */ \ + \ + {0, HID_KEY_SPACE }, /* 0x20 */ \ + {1, HID_KEY_1 }, /* 0x21 ! */ \ + {1, HID_KEY_APOSTROPHE }, /* 0x22 " */ \ + {1, HID_KEY_3 }, /* 0x23 # */ \ + {1, HID_KEY_4 }, /* 0x24 $ */ \ + {1, HID_KEY_5 }, /* 0x25 % */ \ + {1, HID_KEY_7 }, /* 0x26 & */ \ + {0, HID_KEY_APOSTROPHE }, /* 0x27 ' */ \ + {1, HID_KEY_9 }, /* 0x28 ( */ \ + {1, HID_KEY_0 }, /* 0x29 ) */ \ + {1, HID_KEY_8 }, /* 0x2A * */ \ + {1, HID_KEY_EQUAL }, /* 0x2B + */ \ + {0, HID_KEY_COMMA }, /* 0x2C , */ \ + {0, HID_KEY_MINUS }, /* 0x2D - */ \ + {0, HID_KEY_PERIOD }, /* 0x2E . */ \ + {0, HID_KEY_SLASH }, /* 0x2F / */ \ + {0, HID_KEY_0 }, /* 0x30 0 */ \ + {0, HID_KEY_1 }, /* 0x31 1 */ \ + {0, HID_KEY_2 }, /* 0x32 2 */ \ + {0, HID_KEY_3 }, /* 0x33 3 */ \ + {0, HID_KEY_4 }, /* 0x34 4 */ \ + {0, HID_KEY_5 }, /* 0x35 5 */ \ + {0, HID_KEY_6 }, /* 0x36 6 */ \ + {0, HID_KEY_7 }, /* 0x37 7 */ \ + {0, HID_KEY_8 }, /* 0x38 8 */ \ + {0, HID_KEY_9 }, /* 0x39 9 */ \ + {1, HID_KEY_SEMICOLON }, /* 0x3A : */ \ + {0, HID_KEY_SEMICOLON }, /* 0x3B ; */ \ + {1, HID_KEY_COMMA }, /* 0x3C < */ \ + {0, HID_KEY_EQUAL }, /* 0x3D = */ \ + {1, HID_KEY_PERIOD }, /* 0x3E > */ \ + {1, HID_KEY_SLASH }, /* 0x3F ? */ \ + \ + {1, HID_KEY_2 }, /* 0x40 @ */ \ + {1, HID_KEY_A }, /* 0x41 A */ \ + {1, HID_KEY_B }, /* 0x42 B */ \ + {1, HID_KEY_C }, /* 0x43 C */ \ + {1, HID_KEY_D }, /* 0x44 D */ \ + {1, HID_KEY_E }, /* 0x45 E */ \ + {1, HID_KEY_F }, /* 0x46 F */ \ + {1, HID_KEY_G }, /* 0x47 G */ \ + {1, HID_KEY_H }, /* 0x48 H */ \ + {1, HID_KEY_I }, /* 0x49 I */ \ + {1, HID_KEY_J }, /* 0x4A J */ \ + {1, HID_KEY_K }, /* 0x4B K */ \ + {1, HID_KEY_L }, /* 0x4C L */ \ + {1, HID_KEY_M }, /* 0x4D M */ \ + {1, HID_KEY_N }, /* 0x4E N */ \ + {1, HID_KEY_O }, /* 0x4F O */ \ + {1, HID_KEY_P }, /* 0x50 P */ \ + {1, HID_KEY_Q }, /* 0x51 Q */ \ + {1, HID_KEY_R }, /* 0x52 R */ \ + {1, HID_KEY_S }, /* 0x53 S */ \ + {1, HID_KEY_T }, /* 0x55 T */ \ + {1, HID_KEY_U }, /* 0x55 U */ \ + {1, HID_KEY_V }, /* 0x56 V */ \ + {1, HID_KEY_W }, /* 0x57 W */ \ + {1, HID_KEY_X }, /* 0x58 X */ \ + {1, HID_KEY_Y }, /* 0x59 Y */ \ + {1, HID_KEY_Z }, /* 0x5A Z */ \ + {0, HID_KEY_BRACKET_LEFT }, /* 0x5B [ */ \ + {0, HID_KEY_BACKSLASH }, /* 0x5C '\' */ \ + {0, HID_KEY_BRACKET_RIGHT }, /* 0x5D ] */ \ + {1, HID_KEY_6 }, /* 0x5E ^ */ \ + {1, HID_KEY_MINUS }, /* 0x5F _ */ \ + \ + {0, HID_KEY_GRAVE }, /* 0x60 ` */ \ + {0, HID_KEY_A }, /* 0x61 a */ \ + {0, HID_KEY_B }, /* 0x62 b */ \ + {0, HID_KEY_C }, /* 0x63 c */ \ + {0, HID_KEY_D }, /* 0x66 d */ \ + {0, HID_KEY_E }, /* 0x65 e */ \ + {0, HID_KEY_F }, /* 0x66 f */ \ + {0, HID_KEY_G }, /* 0x67 g */ \ + {0, HID_KEY_H }, /* 0x68 h */ \ + {0, HID_KEY_I }, /* 0x69 i */ \ + {0, HID_KEY_J }, /* 0x6A j */ \ + {0, HID_KEY_K }, /* 0x6B k */ \ + {0, HID_KEY_L }, /* 0x6C l */ \ + {0, HID_KEY_M }, /* 0x6D m */ \ + {0, HID_KEY_N }, /* 0x6E n */ \ + {0, HID_KEY_O }, /* 0x6F o */ \ + {0, HID_KEY_P }, /* 0x70 p */ \ + {0, HID_KEY_Q }, /* 0x71 q */ \ + {0, HID_KEY_R }, /* 0x72 r */ \ + {0, HID_KEY_S }, /* 0x73 s */ \ + {0, HID_KEY_T }, /* 0x75 t */ \ + {0, HID_KEY_U }, /* 0x75 u */ \ + {0, HID_KEY_V }, /* 0x76 v */ \ + {0, HID_KEY_W }, /* 0x77 w */ \ + {0, HID_KEY_X }, /* 0x78 x */ \ + {0, HID_KEY_Y }, /* 0x79 y */ \ + {0, HID_KEY_Z }, /* 0x7A z */ \ + {1, HID_KEY_BRACKET_LEFT }, /* 0x7B { */ \ + {1, HID_KEY_BACKSLASH }, /* 0x7C | */ \ + {1, HID_KEY_BRACKET_RIGHT }, /* 0x7D } */ \ + {1, HID_KEY_GRAVE }, /* 0x7E ~ */ \ + {0, HID_KEY_DELETE } /* 0x7F Delete */ \ + +/*-------------------------------------------------------------------- + * KEYCODE to Ascii Conversion + * Expand to array of [128][2] (ascii without shift, ascii with shift) + * + * Usage: example to convert ascii from keycode (key) and shift modifier (shift). + * Here we assume key < 128 ( printable ) + * + * uint8_t const conv_table[128][2] = { HID_KEYCODE_TO_ASCII }; + * char ch = shift ? conv_table[chr][1] : conv_table[chr][0]; + * + *--------------------------------------------------------------------*/ +#define HID_KEYCODE_TO_ASCII \ + {0 , 0 }, /* 0x00 */ \ + {0 , 0 }, /* 0x01 */ \ + {0 , 0 }, /* 0x02 */ \ + {0 , 0 }, /* 0x03 */ \ + {'a' , 'A' }, /* 0x04 */ \ + {'b' , 'B' }, /* 0x05 */ \ + {'c' , 'C' }, /* 0x06 */ \ + {'d' , 'D' }, /* 0x07 */ \ + {'e' , 'E' }, /* 0x08 */ \ + {'f' , 'F' }, /* 0x09 */ \ + {'g' , 'G' }, /* 0x0a */ \ + {'h' , 'H' }, /* 0x0b */ \ + {'i' , 'I' }, /* 0x0c */ \ + {'j' , 'J' }, /* 0x0d */ \ + {'k' , 'K' }, /* 0x0e */ \ + {'l' , 'L' }, /* 0x0f */ \ + {'m' , 'M' }, /* 0x10 */ \ + {'n' , 'N' }, /* 0x11 */ \ + {'o' , 'O' }, /* 0x12 */ \ + {'p' , 'P' }, /* 0x13 */ \ + {'q' , 'Q' }, /* 0x14 */ \ + {'r' , 'R' }, /* 0x15 */ \ + {'s' , 'S' }, /* 0x16 */ \ + {'t' , 'T' }, /* 0x17 */ \ + {'u' , 'U' }, /* 0x18 */ \ + {'v' , 'V' }, /* 0x19 */ \ + {'w' , 'W' }, /* 0x1a */ \ + {'x' , 'X' }, /* 0x1b */ \ + {'y' , 'Y' }, /* 0x1c */ \ + {'z' , 'Z' }, /* 0x1d */ \ + {'1' , '!' }, /* 0x1e */ \ + {'2' , '@' }, /* 0x1f */ \ + {'3' , '#' }, /* 0x20 */ \ + {'4' , '$' }, /* 0x21 */ \ + {'5' , '%' }, /* 0x22 */ \ + {'6' , '^' }, /* 0x23 */ \ + {'7' , '&' }, /* 0x24 */ \ + {'8' , '*' }, /* 0x25 */ \ + {'9' , '(' }, /* 0x26 */ \ + {'0' , ')' }, /* 0x27 */ \ + {'\r' , '\r' }, /* 0x28 */ \ + {'\x1b', '\x1b' }, /* 0x29 */ \ + {'\b' , '\b' }, /* 0x2a */ \ + {'\t' , '\t' }, /* 0x2b */ \ + {' ' , ' ' }, /* 0x2c */ \ + {'-' , '_' }, /* 0x2d */ \ + {'=' , '+' }, /* 0x2e */ \ + {'[' , '{' }, /* 0x2f */ \ + {']' , '}' }, /* 0x30 */ \ + {'\\' , '|' }, /* 0x31 */ \ + {'#' , '~' }, /* 0x32 */ \ + {';' , ':' }, /* 0x33 */ \ + {'\'' , '\"' }, /* 0x34 */ \ + {'`' , '~' }, /* 0x35 */ \ + {',' , '<' }, /* 0x36 */ \ + {'.' , '>' }, /* 0x37 */ \ + {'/' , '?' }, /* 0x38 */ \ + \ + {0 , 0 }, /* 0x39 */ \ + {0 , 0 }, /* 0x3a */ \ + {0 , 0 }, /* 0x3b */ \ + {0 , 0 }, /* 0x3c */ \ + {0 , 0 }, /* 0x3d */ \ + {0 , 0 }, /* 0x3e */ \ + {0 , 0 }, /* 0x3f */ \ + {0 , 0 }, /* 0x40 */ \ + {0 , 0 }, /* 0x41 */ \ + {0 , 0 }, /* 0x42 */ \ + {0 , 0 }, /* 0x43 */ \ + {0 , 0 }, /* 0x44 */ \ + {0 , 0 }, /* 0x45 */ \ + {0 , 0 }, /* 0x46 */ \ + {0 , 0 }, /* 0x47 */ \ + {0 , 0 }, /* 0x48 */ \ + {0 , 0 }, /* 0x49 */ \ + {0 , 0 }, /* 0x4a */ \ + {0 , 0 }, /* 0x4b */ \ + {0 , 0 }, /* 0x4c */ \ + {0 , 0 }, /* 0x4d */ \ + {0 , 0 }, /* 0x4e */ \ + {0 , 0 }, /* 0x4f */ \ + {0 , 0 }, /* 0x50 */ \ + {0 , 0 }, /* 0x51 */ \ + {0 , 0 }, /* 0x52 */ \ + {0 , 0 }, /* 0x53 */ \ + \ + {'/' , '/' }, /* 0x54 */ \ + {'*' , '*' }, /* 0x55 */ \ + {'-' , '-' }, /* 0x56 */ \ + {'+' , '+' }, /* 0x57 */ \ + {'\r' , '\r' }, /* 0x58 */ \ + {'1' , 0 }, /* 0x59 */ \ + {'2' , 0 }, /* 0x5a */ \ + {'3' , 0 }, /* 0x5b */ \ + {'4' , 0 }, /* 0x5c */ \ + {'5' , '5' }, /* 0x5d */ \ + {'6' , 0 }, /* 0x5e */ \ + {'7' , 0 }, /* 0x5f */ \ + {'8' , 0 }, /* 0x60 */ \ + {'9' , 0 }, /* 0x61 */ \ + {'0' , 0 }, /* 0x62 */ \ + {'0' , 0 }, /* 0x63 */ \ + {'=' , '=' }, /* 0x67 */ \ + + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_HID_H__ */ + +/// @} diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_device.c new file mode 100644 index 000000000..588b61254 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_device.c @@ -0,0 +1,417 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_HID) + +//--------------------------------------------------------------------+ +// INCLUDE +//--------------------------------------------------------------------+ +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "hid_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef struct +{ + uint8_t itf_num; + uint8_t ep_in; + uint8_t ep_out; // optional Out endpoint + uint8_t itf_protocol; // Boot mouse or keyboard + + uint8_t protocol_mode; // Boot (0) or Report protocol (1) + uint8_t idle_rate; // up to application to handle idle rate + uint16_t report_desc_len; + + CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_HID_EP_BUFSIZE]; + CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_HID_EP_BUFSIZE]; + + // TODO save hid descriptor since host can specifically request this after enumeration + // Note: HID descriptor may be not available from application after enumeration + tusb_hid_descriptor_hid_t const * hid_descriptor; +} hidd_interface_t; + +CFG_TUSB_MEM_SECTION static hidd_interface_t _hidd_itf[CFG_TUD_HID]; + +/*------------- Helpers -------------*/ +static inline uint8_t get_index_by_itfnum(uint8_t itf_num) +{ + for (uint8_t i=0; i < CFG_TUD_HID; i++ ) + { + if ( itf_num == _hidd_itf[i].itf_num ) return i; + } + + return 0xFF; +} + +//--------------------------------------------------------------------+ +// APPLICATION API +//--------------------------------------------------------------------+ +bool tud_hid_n_ready(uint8_t instance) +{ + uint8_t const ep_in = _hidd_itf[instance].ep_in; + return tud_ready() && (ep_in != 0) && !usbd_edpt_busy(TUD_OPT_RHPORT, ep_in); +} + +bool tud_hid_n_report(uint8_t instance, uint8_t report_id, void const* report, uint8_t len) +{ + uint8_t const rhport = 0; + hidd_interface_t * p_hid = &_hidd_itf[instance]; + + // claim endpoint + TU_VERIFY( usbd_edpt_claim(rhport, p_hid->ep_in) ); + + // prepare data + if (report_id) + { + len = tu_min8(len, CFG_TUD_HID_EP_BUFSIZE-1); + + p_hid->epin_buf[0] = report_id; + memcpy(p_hid->epin_buf+1, report, len); + len++; + }else + { + // If report id = 0, skip ID field + len = tu_min8(len, CFG_TUD_HID_EP_BUFSIZE); + memcpy(p_hid->epin_buf, report, len); + } + + return usbd_edpt_xfer(TUD_OPT_RHPORT, p_hid->ep_in, p_hid->epin_buf, len); +} + +uint8_t tud_hid_n_interface_protocol(uint8_t instance) +{ + return _hidd_itf[instance].itf_protocol; +} + +uint8_t tud_hid_n_get_protocol(uint8_t instance) +{ + return _hidd_itf[instance].protocol_mode; +} + +bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modifier, uint8_t keycode[6]) +{ + hid_keyboard_report_t report; + + report.modifier = modifier; + report.reserved = 0; + + if ( keycode ) + { + memcpy(report.keycode, keycode, 6); + }else + { + tu_memclr(report.keycode, 6); + } + + return tud_hid_n_report(instance, report_id, &report, sizeof(report)); +} + +bool tud_hid_n_mouse_report(uint8_t instance, uint8_t report_id, + uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal) +{ + hid_mouse_report_t report = + { + .buttons = buttons, + .x = x, + .y = y, + .wheel = vertical, + .pan = horizontal + }; + + return tud_hid_n_report(instance, report_id, &report, sizeof(report)); +} + +bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id, + int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons) +{ + hid_gamepad_report_t report = + { + .x = x, + .y = y, + .z = z, + .rz = rz, + .rx = rx, + .ry = ry, + .hat = hat, + .buttons = buttons, + }; + + return tud_hid_n_report(instance, report_id, &report, sizeof(report)); +} + +//--------------------------------------------------------------------+ +// USBD-CLASS API +//--------------------------------------------------------------------+ +void hidd_init(void) +{ + hidd_reset(TUD_OPT_RHPORT); +} + +void hidd_reset(uint8_t rhport) +{ + (void) rhport; + tu_memclr(_hidd_itf, sizeof(_hidd_itf)); +} + +uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint16_t max_len) +{ + TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass, 0); + + // len = interface + hid + n*endpoints + uint16_t const drv_len = sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) + desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t); + TU_ASSERT(max_len >= drv_len, 0); + + // Find available interface + hidd_interface_t * p_hid = NULL; + uint8_t hid_id; + for(hid_id=0; hid_idhid_descriptor = (tusb_hid_descriptor_hid_t const *) p_desc; + + //------------- Endpoint Descriptor -------------// + p_desc = tu_desc_next(p_desc); + TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, desc_itf->bNumEndpoints, TUSB_XFER_INTERRUPT, &p_hid->ep_out, &p_hid->ep_in), 0); + + if ( desc_itf->bInterfaceSubClass == HID_SUBCLASS_BOOT ) p_hid->itf_protocol = desc_itf->bInterfaceProtocol; + + p_hid->protocol_mode = HID_PROTOCOL_REPORT; // Per Specs: default is report mode + p_hid->itf_num = desc_itf->bInterfaceNumber; + + // Use offsetof to avoid pointer to the odd/misaligned address + p_hid->report_desc_len = tu_unaligned_read16((uint8_t const*) p_hid->hid_descriptor + offsetof(tusb_hid_descriptor_hid_t, wReportLength)); + + // Prepare for output endpoint + if (p_hid->ep_out) + { + if ( !usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf)) ) + { + TU_LOG_FAILED(); + TU_BREAKPOINT(); + } + } + + return drv_len; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE); + + uint8_t const hid_itf = get_index_by_itfnum((uint8_t) request->wIndex); + TU_VERIFY(hid_itf < CFG_TUD_HID); + + hidd_interface_t* p_hid = &_hidd_itf[hid_itf]; + + if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) + { + //------------- STD Request -------------// + if ( stage == CONTROL_STAGE_SETUP ) + { + uint8_t const desc_type = tu_u16_high(request->wValue); + //uint8_t const desc_index = tu_u16_low (request->wValue); + + if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_HID) + { + TU_VERIFY(p_hid->hid_descriptor); + TU_VERIFY(tud_control_xfer(rhport, request, (void*)(uintptr_t) p_hid->hid_descriptor, p_hid->hid_descriptor->bLength)); + } + else if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_REPORT) + { + uint8_t const * desc_report = tud_hid_descriptor_report_cb(hid_itf); + tud_control_xfer(rhport, request, (void*)(uintptr_t) desc_report, p_hid->report_desc_len); + } + else + { + return false; // stall unsupported request + } + } + } + else if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS) + { + //------------- Class Specific Request -------------// + switch( request->bRequest ) + { + case HID_REQ_CONTROL_GET_REPORT: + if ( stage == CONTROL_STAGE_SETUP ) + { + uint8_t const report_type = tu_u16_high(request->wValue); + uint8_t const report_id = tu_u16_low(request->wValue); + + uint8_t* report_buf = p_hid->epin_buf; + uint16_t req_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE); + + uint16_t xferlen = 0; + + // If host request a specific Report ID, add ID to as 1 byte of response + if ( (report_id != HID_REPORT_TYPE_INVALID) && (req_len > 1) ) + { + *report_buf++ = report_id; + req_len--; + + xferlen++; + } + + xferlen += tud_hid_get_report_cb(hid_itf, report_id, (hid_report_type_t) report_type, report_buf, req_len); + TU_ASSERT( xferlen > 0 ); + + tud_control_xfer(rhport, request, p_hid->epin_buf, xferlen); + } + break; + + case HID_REQ_CONTROL_SET_REPORT: + if ( stage == CONTROL_STAGE_SETUP ) + { + TU_VERIFY(request->wLength <= sizeof(p_hid->epout_buf)); + tud_control_xfer(rhport, request, p_hid->epout_buf, request->wLength); + } + else if ( stage == CONTROL_STAGE_ACK ) + { + uint8_t const report_type = tu_u16_high(request->wValue); + uint8_t const report_id = tu_u16_low(request->wValue); + + uint8_t const* report_buf = p_hid->epout_buf; + uint16_t report_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE); + + // If host request a specific Report ID, extract report ID in buffer before invoking callback + if ( (report_id != HID_REPORT_TYPE_INVALID) && (report_len > 1) && (report_id == report_buf[0]) ) + { + report_buf++; + report_len--; + } + + tud_hid_set_report_cb(hid_itf, report_id, (hid_report_type_t) report_type, report_buf, report_len); + } + break; + + case HID_REQ_CONTROL_SET_IDLE: + if ( stage == CONTROL_STAGE_SETUP ) + { + p_hid->idle_rate = tu_u16_high(request->wValue); + if ( tud_hid_set_idle_cb ) + { + // stall request if callback return false + TU_VERIFY( tud_hid_set_idle_cb( hid_itf, p_hid->idle_rate) ); + } + + tud_control_status(rhport, request); + } + break; + + case HID_REQ_CONTROL_GET_IDLE: + if ( stage == CONTROL_STAGE_SETUP ) + { + // TODO idle rate of report + tud_control_xfer(rhport, request, &p_hid->idle_rate, 1); + } + break; + + case HID_REQ_CONTROL_GET_PROTOCOL: + if ( stage == CONTROL_STAGE_SETUP ) + { + tud_control_xfer(rhport, request, &p_hid->protocol_mode, 1); + } + break; + + case HID_REQ_CONTROL_SET_PROTOCOL: + if ( stage == CONTROL_STAGE_SETUP ) + { + tud_control_status(rhport, request); + } + else if ( stage == CONTROL_STAGE_ACK ) + { + p_hid->protocol_mode = (uint8_t) request->wValue; + if (tud_hid_set_protocol_cb) + { + tud_hid_set_protocol_cb(hid_itf, p_hid->protocol_mode); + } + } + break; + + default: return false; // stall unsupported request + } + }else + { + return false; // stall unsupported request + } + + return true; +} + +bool hidd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) result; + + uint8_t instance = 0; + hidd_interface_t * p_hid = _hidd_itf; + + // Identify which interface to use + for (instance = 0; instance < CFG_TUD_HID; instance++) + { + p_hid = &_hidd_itf[instance]; + if ( (ep_addr == p_hid->ep_out) || (ep_addr == p_hid->ep_in) ) break; + } + TU_ASSERT(instance < CFG_TUD_HID); + + // Sent report successfully + if (ep_addr == p_hid->ep_in) + { + if (tud_hid_report_complete_cb) + { + tud_hid_report_complete_cb(instance, p_hid->epin_buf, (uint8_t) xferred_bytes); + } + } + // Received report + else if (ep_addr == p_hid->ep_out) + { + tud_hid_set_report_cb(instance, 0, HID_REPORT_TYPE_INVALID, p_hid->epout_buf, xferred_bytes); + TU_ASSERT(usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf))); + } + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_device.h new file mode 100644 index 000000000..078b67349 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_device.h @@ -0,0 +1,393 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_HID_DEVICE_H_ +#define _TUSB_HID_DEVICE_H_ + +#include "hid.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Driver Default Configure & Validation +//--------------------------------------------------------------------+ + +#if !defined(CFG_TUD_HID_EP_BUFSIZE) & defined(CFG_TUD_HID_BUFSIZE) + // TODO warn user to use new name later on + // #warning CFG_TUD_HID_BUFSIZE is renamed to CFG_TUD_HID_EP_BUFSIZE, please update to use the new name + #define CFG_TUD_HID_EP_BUFSIZE CFG_TUD_HID_BUFSIZE +#endif + +#ifndef CFG_TUD_HID_EP_BUFSIZE + #define CFG_TUD_HID_EP_BUFSIZE 64 +#endif + +//--------------------------------------------------------------------+ +// Application API (Multiple Instances) +// CFG_TUD_HID > 1 +//--------------------------------------------------------------------+ + +// Check if the interface is ready to use +bool tud_hid_n_ready(uint8_t instance); + +// Get interface supported protocol (bInterfaceProtocol) check out hid_interface_protocol_enum_t for possible values +uint8_t tud_hid_n_interface_protocol(uint8_t instance); + +// Get current active protocol: HID_PROTOCOL_BOOT (0) or HID_PROTOCOL_REPORT (1) +uint8_t tud_hid_n_get_protocol(uint8_t instance); + +// Send report to host +bool tud_hid_n_report(uint8_t instance, uint8_t report_id, void const* report, uint8_t len); + +// KEYBOARD: convenient helper to send keyboard report if application +// use template layout report as defined by hid_keyboard_report_t +bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modifier, uint8_t keycode[6]); + +// MOUSE: convenient helper to send mouse report if application +// use template layout report as defined by hid_mouse_report_t +bool tud_hid_n_mouse_report(uint8_t instance, uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal); + +// Gamepad: convenient helper to send gamepad report if application +// use template layout report TUD_HID_REPORT_DESC_GAMEPAD +bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons); + +//--------------------------------------------------------------------+ +// Application API (Single Port) +//--------------------------------------------------------------------+ +static inline bool tud_hid_ready(void); +static inline uint8_t tud_hid_interface_protocol(void); +static inline uint8_t tud_hid_get_protocol(void); +static inline bool tud_hid_report(uint8_t report_id, void const* report, uint8_t len); +static inline bool tud_hid_keyboard_report(uint8_t report_id, uint8_t modifier, uint8_t keycode[6]); +static inline bool tud_hid_mouse_report(uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal); +static inline bool tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons); + +//--------------------------------------------------------------------+ +// Callbacks (Weak is optional) +//--------------------------------------------------------------------+ + +// Invoked when received GET HID REPORT DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete +uint8_t const * tud_hid_descriptor_report_cb(uint8_t instance); + +// Invoked when received GET_REPORT control request +// Application must fill buffer report's content and return its length. +// Return zero will cause the stack to STALL request +uint16_t tud_hid_get_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen); + +// Invoked when received SET_REPORT control request or +// received data on OUT endpoint ( Report ID = 0, Type = 0 ) +void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_t report_type, uint8_t const* buffer, uint16_t bufsize); + +// Invoked when received SET_PROTOCOL request +// protocol is either HID_PROTOCOL_BOOT (0) or HID_PROTOCOL_REPORT (1) +TU_ATTR_WEAK void tud_hid_set_protocol_cb(uint8_t instance, uint8_t protocol); + +// Invoked when received SET_IDLE request. return false will stall the request +// - Idle Rate = 0 : only send report if there is changes, i.e skip duplication +// - Idle Rate > 0 : skip duplication, but send at least 1 report every idle rate (in unit of 4 ms). +TU_ATTR_WEAK bool tud_hid_set_idle_cb(uint8_t instance, uint8_t idle_rate); + +// Invoked when sent REPORT successfully to host +// Application can use this to send the next report +// Note: For composite reports, report[0] is report ID +TU_ATTR_WEAK void tud_hid_report_complete_cb(uint8_t instance, uint8_t const* report, uint8_t len); + + +//--------------------------------------------------------------------+ +// Inline Functions +//--------------------------------------------------------------------+ +static inline bool tud_hid_ready(void) +{ + return tud_hid_n_ready(0); +} + +static inline uint8_t tud_hid_interface_protocol(void) +{ + return tud_hid_n_interface_protocol(0); +} + +static inline uint8_t tud_hid_get_protocol(void) +{ + return tud_hid_n_get_protocol(0); +} + +static inline bool tud_hid_report(uint8_t report_id, void const* report, uint8_t len) +{ + return tud_hid_n_report(0, report_id, report, len); +} + +static inline bool tud_hid_keyboard_report(uint8_t report_id, uint8_t modifier, uint8_t keycode[6]) +{ + return tud_hid_n_keyboard_report(0, report_id, modifier, keycode); +} + +static inline bool tud_hid_mouse_report(uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal) +{ + return tud_hid_n_mouse_report(0, report_id, buttons, x, y, vertical, horizontal); +} + +static inline bool tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons) +{ + return tud_hid_n_gamepad_report(0, report_id, x, y, z, rz, rx, ry, hat, buttons); +} + +/* --------------------------------------------------------------------+ + * HID Report Descriptor Template + * + * Convenient for declaring popular HID device (keyboard, mouse, consumer, + * gamepad etc...). Templates take "HID_REPORT_ID(n)" as input, leave + * empty if multiple reports is not used + * + * - Only 1 report: no parameter + * uint8_t const report_desc[] = { TUD_HID_REPORT_DESC_KEYBOARD() }; + * + * - Multiple Reports: "HID_REPORT_ID(ID)" must be passed to template + * uint8_t const report_desc[] = + * { + * TUD_HID_REPORT_DESC_KEYBOARD( HID_REPORT_ID(1) ) , + * TUD_HID_REPORT_DESC_MOUSE ( HID_REPORT_ID(2) ) + * }; + *--------------------------------------------------------------------*/ + +// Keyboard Report Descriptor Template +#define TUD_HID_REPORT_DESC_KEYBOARD(...) \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_KEYBOARD ) ,\ + HID_COLLECTION ( HID_COLLECTION_APPLICATION ) ,\ + /* Report ID if any */\ + __VA_ARGS__ \ + /* 8 bits Modifier Keys (Shfit, Control, Alt) */ \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_KEYBOARD ) ,\ + HID_USAGE_MIN ( 224 ) ,\ + HID_USAGE_MAX ( 231 ) ,\ + HID_LOGICAL_MIN ( 0 ) ,\ + HID_LOGICAL_MAX ( 1 ) ,\ + HID_REPORT_COUNT ( 8 ) ,\ + HID_REPORT_SIZE ( 1 ) ,\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\ + /* 8 bit reserved */ \ + HID_REPORT_COUNT ( 1 ) ,\ + HID_REPORT_SIZE ( 8 ) ,\ + HID_INPUT ( HID_CONSTANT ) ,\ + /* Output 5-bit LED Indicator Kana | Compose | ScrollLock | CapsLock | NumLock */ \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_LED ) ,\ + HID_USAGE_MIN ( 1 ) ,\ + HID_USAGE_MAX ( 5 ) ,\ + HID_REPORT_COUNT ( 5 ) ,\ + HID_REPORT_SIZE ( 1 ) ,\ + HID_OUTPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\ + /* led padding */ \ + HID_REPORT_COUNT ( 1 ) ,\ + HID_REPORT_SIZE ( 3 ) ,\ + HID_OUTPUT ( HID_CONSTANT ) ,\ + /* 6-byte Keycodes */ \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_KEYBOARD ) ,\ + HID_USAGE_MIN ( 0 ) ,\ + HID_USAGE_MAX_N ( 255, 2 ) ,\ + HID_LOGICAL_MIN ( 0 ) ,\ + HID_LOGICAL_MAX_N( 255, 2 ) ,\ + HID_REPORT_COUNT ( 6 ) ,\ + HID_REPORT_SIZE ( 8 ) ,\ + HID_INPUT ( HID_DATA | HID_ARRAY | HID_ABSOLUTE ) ,\ + HID_COLLECTION_END \ + +// Mouse Report Descriptor Template +#define TUD_HID_REPORT_DESC_MOUSE(...) \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_MOUSE ) ,\ + HID_COLLECTION ( HID_COLLECTION_APPLICATION ) ,\ + /* Report ID if any */\ + __VA_ARGS__ \ + HID_USAGE ( HID_USAGE_DESKTOP_POINTER ) ,\ + HID_COLLECTION ( HID_COLLECTION_PHYSICAL ) ,\ + HID_USAGE_PAGE ( HID_USAGE_PAGE_BUTTON ) ,\ + HID_USAGE_MIN ( 1 ) ,\ + HID_USAGE_MAX ( 5 ) ,\ + HID_LOGICAL_MIN ( 0 ) ,\ + HID_LOGICAL_MAX ( 1 ) ,\ + /* Left, Right, Middle, Backward, Forward buttons */ \ + HID_REPORT_COUNT( 5 ) ,\ + HID_REPORT_SIZE ( 1 ) ,\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\ + /* 3 bit padding */ \ + HID_REPORT_COUNT( 1 ) ,\ + HID_REPORT_SIZE ( 3 ) ,\ + HID_INPUT ( HID_CONSTANT ) ,\ + HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP ) ,\ + /* X, Y position [-127, 127] */ \ + HID_USAGE ( HID_USAGE_DESKTOP_X ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_Y ) ,\ + HID_LOGICAL_MIN ( 0x81 ) ,\ + HID_LOGICAL_MAX ( 0x7f ) ,\ + HID_REPORT_COUNT( 2 ) ,\ + HID_REPORT_SIZE ( 8 ) ,\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_RELATIVE ) ,\ + /* Verital wheel scroll [-127, 127] */ \ + HID_USAGE ( HID_USAGE_DESKTOP_WHEEL ) ,\ + HID_LOGICAL_MIN ( 0x81 ) ,\ + HID_LOGICAL_MAX ( 0x7f ) ,\ + HID_REPORT_COUNT( 1 ) ,\ + HID_REPORT_SIZE ( 8 ) ,\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_RELATIVE ) ,\ + HID_USAGE_PAGE ( HID_USAGE_PAGE_CONSUMER ), \ + /* Horizontal wheel scroll [-127, 127] */ \ + HID_USAGE_N ( HID_USAGE_CONSUMER_AC_PAN, 2 ), \ + HID_LOGICAL_MIN ( 0x81 ), \ + HID_LOGICAL_MAX ( 0x7f ), \ + HID_REPORT_COUNT( 1 ), \ + HID_REPORT_SIZE ( 8 ), \ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_RELATIVE ), \ + HID_COLLECTION_END , \ + HID_COLLECTION_END \ + +// Consumer Control Report Descriptor Template +#define TUD_HID_REPORT_DESC_CONSUMER(...) \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_CONSUMER ) ,\ + HID_USAGE ( HID_USAGE_CONSUMER_CONTROL ) ,\ + HID_COLLECTION ( HID_COLLECTION_APPLICATION ) ,\ + /* Report ID if any */\ + __VA_ARGS__ \ + HID_LOGICAL_MIN ( 0x00 ) ,\ + HID_LOGICAL_MAX_N( 0x03FF, 2 ) ,\ + HID_USAGE_MIN ( 0x00 ) ,\ + HID_USAGE_MAX_N ( 0x03FF, 2 ) ,\ + HID_REPORT_COUNT ( 1 ) ,\ + HID_REPORT_SIZE ( 16 ) ,\ + HID_INPUT ( HID_DATA | HID_ARRAY | HID_ABSOLUTE ) ,\ + HID_COLLECTION_END \ + +/* System Control Report Descriptor Template + * 0x00 - do nothing + * 0x01 - Power Off + * 0x02 - Standby + * 0x03 - Wake Host + */ +#define TUD_HID_REPORT_DESC_SYSTEM_CONTROL(...) \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_SYSTEM_CONTROL ) ,\ + HID_COLLECTION ( HID_COLLECTION_APPLICATION ) ,\ + /* Report ID if any */\ + __VA_ARGS__ \ + /* 2 bit system power control */ \ + HID_LOGICAL_MIN ( 1 ) ,\ + HID_LOGICAL_MAX ( 3 ) ,\ + HID_REPORT_COUNT ( 1 ) ,\ + HID_REPORT_SIZE ( 2 ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_SYSTEM_POWER_DOWN ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_SYSTEM_SLEEP ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_SYSTEM_WAKE_UP ) ,\ + HID_INPUT ( HID_DATA | HID_ARRAY | HID_ABSOLUTE ) ,\ + /* 6 bit padding */ \ + HID_REPORT_COUNT ( 1 ) ,\ + HID_REPORT_SIZE ( 6 ) ,\ + HID_INPUT ( HID_CONSTANT ) ,\ + HID_COLLECTION_END \ + +// Gamepad Report Descriptor Template +// with 32 buttons, 2 joysticks and 1 hat/dpad with following layout +// | X | Y | Z | Rz | Rx | Ry (1 byte each) | hat/DPAD (1 byte) | Button Map (4 bytes) | +#define TUD_HID_REPORT_DESC_GAMEPAD(...) \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_GAMEPAD ) ,\ + HID_COLLECTION ( HID_COLLECTION_APPLICATION ) ,\ + /* Report ID if any */\ + __VA_ARGS__ \ + /* 8 bit X, Y, Z, Rz, Rx, Ry (min -127, max 127 ) */ \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_X ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_Y ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_Z ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_RZ ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_RX ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_RY ) ,\ + HID_LOGICAL_MIN ( 0x81 ) ,\ + HID_LOGICAL_MAX ( 0x7f ) ,\ + HID_REPORT_COUNT ( 6 ) ,\ + HID_REPORT_SIZE ( 8 ) ,\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\ + /* 8 bit DPad/Hat Button Map */ \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP ) ,\ + HID_USAGE ( HID_USAGE_DESKTOP_HAT_SWITCH ) ,\ + HID_LOGICAL_MIN ( 1 ) ,\ + HID_LOGICAL_MAX ( 8 ) ,\ + HID_PHYSICAL_MIN ( 0 ) ,\ + HID_PHYSICAL_MAX_N ( 315, 2 ) ,\ + HID_REPORT_COUNT ( 1 ) ,\ + HID_REPORT_SIZE ( 8 ) ,\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\ + /* 32 bit Button Map */ \ + HID_USAGE_PAGE ( HID_USAGE_PAGE_BUTTON ) ,\ + HID_USAGE_MIN ( 1 ) ,\ + HID_USAGE_MAX ( 32 ) ,\ + HID_LOGICAL_MIN ( 0 ) ,\ + HID_LOGICAL_MAX ( 1 ) ,\ + HID_REPORT_COUNT ( 32 ) ,\ + HID_REPORT_SIZE ( 1 ) ,\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\ + HID_COLLECTION_END \ + +// HID Generic Input & Output +// - 1st parameter is report size (mandatory) +// - 2nd parameter is report id HID_REPORT_ID(n) (optional) +#define TUD_HID_REPORT_DESC_GENERIC_INOUT(report_size, ...) \ + HID_USAGE_PAGE_N ( HID_USAGE_PAGE_VENDOR, 2 ),\ + HID_USAGE ( 0x01 ),\ + HID_COLLECTION ( HID_COLLECTION_APPLICATION ),\ + /* Report ID if any */\ + __VA_ARGS__ \ + /* Input */ \ + HID_USAGE ( 0x02 ),\ + HID_LOGICAL_MIN ( 0x00 ),\ + HID_LOGICAL_MAX_N ( 0xff, 2 ),\ + HID_REPORT_SIZE ( 8 ),\ + HID_REPORT_COUNT( report_size ),\ + HID_INPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ),\ + /* Output */ \ + HID_USAGE ( 0x03 ),\ + HID_LOGICAL_MIN ( 0x00 ),\ + HID_LOGICAL_MAX_N ( 0xff, 2 ),\ + HID_REPORT_SIZE ( 8 ),\ + HID_REPORT_COUNT( report_size ),\ + HID_OUTPUT ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ),\ + HID_COLLECTION_END \ + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void hidd_init (void); +void hidd_reset (uint8_t rhport); +uint16_t hidd_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +bool hidd_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_HID_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_host.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_host.c new file mode 100644 index 000000000..f19f1ba81 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_host.c @@ -0,0 +1,636 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_HOST_ENABLED && CFG_TUH_HID) + +#include "host/usbh.h" +#include "host/usbh_classdriver.h" + +#include "hid_host.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +typedef struct +{ + uint8_t itf_num; + uint8_t ep_in; + uint8_t ep_out; + + uint8_t itf_protocol; // None, Keyboard, Mouse + uint8_t protocol_mode; // Boot (0) or Report protocol (1) + + uint8_t report_desc_type; + uint16_t report_desc_len; + + uint16_t epin_size; + uint16_t epout_size; + + uint8_t epin_buf[CFG_TUH_HID_EPIN_BUFSIZE]; + uint8_t epout_buf[CFG_TUH_HID_EPOUT_BUFSIZE]; +} hidh_interface_t; + +typedef struct +{ + uint8_t inst_count; + hidh_interface_t instances[CFG_TUH_HID]; +} hidh_device_t; + +static hidh_device_t _hidh_dev[CFG_TUH_DEVICE_MAX]; + +//------------- Internal prototypes -------------// + +// Get HID device & interface +TU_ATTR_ALWAYS_INLINE static inline hidh_device_t* get_dev(uint8_t dev_addr); +TU_ATTR_ALWAYS_INLINE static inline hidh_interface_t* get_instance(uint8_t dev_addr, uint8_t instance); +static uint8_t get_instance_id_by_itfnum(uint8_t dev_addr, uint8_t itf); +static uint8_t get_instance_id_by_epaddr(uint8_t dev_addr, uint8_t ep_addr); + +//--------------------------------------------------------------------+ +// Interface API +//--------------------------------------------------------------------+ + +uint8_t tuh_hid_instance_count(uint8_t dev_addr) +{ + return get_dev(dev_addr)->inst_count; +} + +bool tuh_hid_mounted(uint8_t dev_addr, uint8_t instance) +{ + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + return (hid_itf->ep_in != 0) || (hid_itf->ep_out != 0); +} + +uint8_t tuh_hid_interface_protocol(uint8_t dev_addr, uint8_t instance) +{ + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + return hid_itf->itf_protocol; +} + +//--------------------------------------------------------------------+ +// Control Endpoint API +//--------------------------------------------------------------------+ + +uint8_t tuh_hid_get_protocol(uint8_t dev_addr, uint8_t instance) +{ + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + return hid_itf->protocol_mode; +} + +static bool set_protocol_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + uint8_t const itf_num = (uint8_t) request->wIndex; + uint8_t const instance = get_instance_id_by_itfnum(dev_addr, itf_num); + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + + if (XFER_RESULT_SUCCESS == result) hid_itf->protocol_mode = (uint8_t) request->wValue; + + if (tuh_hid_set_protocol_complete_cb) + { + tuh_hid_set_protocol_complete_cb(dev_addr, instance, hid_itf->protocol_mode); + } + + return true; +} + +bool tuh_hid_set_protocol(uint8_t dev_addr, uint8_t instance, uint8_t protocol) +{ + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + TU_VERIFY(hid_itf->itf_protocol != HID_ITF_PROTOCOL_NONE); + + TU_LOG2("HID Set Protocol = %d\r\n", protocol); + + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = HID_REQ_CONTROL_SET_PROTOCOL, + .wValue = protocol, + .wIndex = hid_itf->itf_num, + .wLength = 0 + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &request, NULL, set_protocol_complete) ); + return true; +} + +static bool set_report_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + TU_LOG2("HID Set Report complete\r\n"); + + if (tuh_hid_set_report_complete_cb) + { + uint8_t const itf_num = (uint8_t) request->wIndex; + uint8_t const instance = get_instance_id_by_itfnum(dev_addr, itf_num); + + uint8_t const report_type = tu_u16_high(request->wValue); + uint8_t const report_id = tu_u16_low(request->wValue); + + tuh_hid_set_report_complete_cb(dev_addr, instance, report_id, report_type, (result == XFER_RESULT_SUCCESS) ? request->wLength : 0); + } + + return true; +} + +bool tuh_hid_set_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t report_type, void* report, uint16_t len) +{ + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + TU_LOG2("HID Set Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len); + + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = HID_REQ_CONTROL_SET_REPORT, + .wValue = tu_u16(report_type, report_id), + .wIndex = hid_itf->itf_num, + .wLength = len + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &request, report, set_report_complete) ); + return true; +} + +//--------------------------------------------------------------------+ +// Interrupt Endpoint API +//--------------------------------------------------------------------+ + +bool tuh_hid_receive_report(uint8_t dev_addr, uint8_t instance) +{ + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + + // claim endpoint + TU_VERIFY( usbh_edpt_claim(dev_addr, hid_itf->ep_in) ); + + return usbh_edpt_xfer(dev_addr, hid_itf->ep_in, hid_itf->epin_buf, hid_itf->epin_size); +} + +//bool tuh_n_hid_n_ready(uint8_t dev_addr, uint8_t instance) +//{ +// TU_VERIFY(tuh_n_hid_n_mounted(dev_addr, instance)); +// +// hidh_interface_t* hid_itf = get_instance(dev_addr, instance); +// return !usbh_edpt_busy(dev_addr, hid_itf->ep_in); +//} + +//void tuh_hid_send_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t const* report, uint16_t len); + +//--------------------------------------------------------------------+ +// USBH API +//--------------------------------------------------------------------+ +void hidh_init(void) +{ + tu_memclr(_hidh_dev, sizeof(_hidh_dev)); +} + +bool hidh_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) result; + + uint8_t const dir = tu_edpt_dir(ep_addr); + uint8_t const instance = get_instance_id_by_epaddr(dev_addr, ep_addr); + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + + if ( dir == TUSB_DIR_IN ) + { + TU_LOG2(" Get Report callback (%u, %u)\r\n", dev_addr, instance); + TU_LOG3_MEM(hid_itf->epin_buf, xferred_bytes, 2); + tuh_hid_report_received_cb(dev_addr, instance, hid_itf->epin_buf, xferred_bytes); + }else + { + if (tuh_hid_report_sent_cb) tuh_hid_report_sent_cb(dev_addr, instance, hid_itf->epout_buf, xferred_bytes); + } + + return true; +} + +void hidh_close(uint8_t dev_addr) +{ + TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, ); + + hidh_device_t* hid_dev = get_dev(dev_addr); + + if (tuh_hid_umount_cb) + { + for (uint8_t inst = 0; inst < hid_dev->inst_count; inst++ ) tuh_hid_umount_cb(dev_addr, inst); + } + + tu_memclr(hid_dev, sizeof(hidh_device_t)); +} + +//--------------------------------------------------------------------+ +// Enumeration +//--------------------------------------------------------------------+ + +static bool config_set_protocol (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool config_get_report_desc (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool config_get_report_desc_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); + +static void config_driver_mount_complete(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len); + +bool hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len) +{ + (void) max_len; + + TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass); + + TU_LOG2("HID opening Interface %u (addr = %u)\r\n", desc_itf->bInterfaceNumber, dev_addr); + + // len = interface + hid + n*endpoints + uint16_t const drv_len = sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) + desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t); + TU_ASSERT(max_len >= drv_len); + + uint8_t const *p_desc = (uint8_t const *) desc_itf; + + //------------- HID descriptor -------------// + p_desc = tu_desc_next(p_desc); + tusb_hid_descriptor_hid_t const *desc_hid = (tusb_hid_descriptor_hid_t const *) p_desc; + TU_ASSERT(HID_DESC_TYPE_HID == desc_hid->bDescriptorType); + + // not enough interface, try to increase CFG_TUH_HID + // TODO multiple devices + hidh_device_t* hid_dev = get_dev(dev_addr); + TU_ASSERT(hid_dev->inst_count < CFG_TUH_HID, 0); + + //------------- Endpoint Descriptor -------------// + p_desc = tu_desc_next(p_desc); + tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc; + TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType); + + // first endpoint may be OUT, skip to IN endpoint + // TODO also open endpoint OUT + if(tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_OUT) + { + p_desc = tu_desc_next(p_desc); + desc_ep = (tusb_desc_endpoint_t const *) p_desc; + TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType); + } + + TU_ASSERT( usbh_edpt_open(rhport, dev_addr, desc_ep) ); + + hidh_interface_t* hid_itf = get_instance(dev_addr, hid_dev->inst_count); + hid_dev->inst_count++; + + hid_itf->itf_num = desc_itf->bInterfaceNumber; + hid_itf->ep_in = desc_ep->bEndpointAddress; + hid_itf->epin_size = tu_edpt_packet_size(desc_ep); + + // Assume bNumDescriptors = 1 + hid_itf->report_desc_type = desc_hid->bReportType; + hid_itf->report_desc_len = tu_unaligned_read16(&desc_hid->wReportLength); + + // Per HID Specs: default is Report protocol, though we will force Boot protocol when set_config + hid_itf->protocol_mode = HID_PROTOCOL_BOOT; + if ( HID_SUBCLASS_BOOT == desc_itf->bInterfaceSubClass ) hid_itf->itf_protocol = desc_itf->bInterfaceProtocol; + + return true; +} + +bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num) +{ + uint8_t const instance = get_instance_id_by_itfnum(dev_addr, itf_num); + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + + // Idle rate = 0 mean only report when there is changes + uint16_t const idle_rate = 0; + + // SET IDLE request, device can stall if not support this request + TU_LOG2("HID Set Idle \r\n"); + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = HID_REQ_CONTROL_SET_IDLE, + .wValue = idle_rate, + .wIndex = itf_num, + .wLength = 0 + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &request, NULL, (hid_itf->itf_protocol != HID_ITF_PROTOCOL_NONE) ? config_set_protocol : config_get_report_desc) ); + + return true; +} + +// Force device to work in BOOT protocol +static bool config_set_protocol(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + // Stall is a valid response for SET_IDLE, therefore we could ignore its result + (void) result; + + uint8_t const itf_num = (uint8_t) request->wIndex; + uint8_t const instance = get_instance_id_by_itfnum(dev_addr, itf_num); + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + + TU_LOG2("HID Set Protocol to Boot Mode\r\n"); + hid_itf->protocol_mode = HID_PROTOCOL_BOOT; + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = HID_REQ_CONTROL_SET_PROTOCOL, + .wValue = HID_PROTOCOL_BOOT, + .wIndex = hid_itf->itf_num, + .wLength = 0 + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &new_request, NULL, config_get_report_desc) ); + return true; +} + +static bool config_get_report_desc(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + // We can be here after SET_IDLE or SET_PROTOCOL (boot device) + // Trigger assert if result is not successful with set protocol + if ( request->bRequest != HID_REQ_CONTROL_SET_IDLE ) + { + TU_ASSERT(result == XFER_RESULT_SUCCESS); + } + + uint8_t const itf_num = (uint8_t) request->wIndex; + uint8_t const instance = get_instance_id_by_itfnum(dev_addr, itf_num); + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + + // Get Report Descriptor if possible + // using usbh enumeration buffer since report descriptor can be very long + if( hid_itf->report_desc_len > CFG_TUH_ENUMERATION_BUFSIZE ) + { + TU_LOG2("HID Skip Report Descriptor since it is too large %u bytes\r\n", hid_itf->report_desc_len); + + // Driver is mounted without report descriptor + config_driver_mount_complete(dev_addr, instance, NULL, 0); + }else + { + TU_LOG2("HID Get Report Descriptor\r\n"); + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_STANDARD, + .direction = TUSB_DIR_IN + }, + .bRequest = TUSB_REQ_GET_DESCRIPTOR, + .wValue = tu_u16(hid_itf->report_desc_type, 0), + .wIndex = itf_num, + .wLength = hid_itf->report_desc_len + }; + + TU_ASSERT(tuh_control_xfer(dev_addr, &new_request, usbh_get_enum_buf(), config_get_report_desc_complete)); + } + + return true; +} + +static bool config_get_report_desc_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + uint8_t const itf_num = (uint8_t) request->wIndex; + uint8_t const instance = get_instance_id_by_itfnum(dev_addr, itf_num); + + uint8_t const* desc_report = usbh_get_enum_buf(); + uint16_t const desc_len = request->wLength; + + config_driver_mount_complete(dev_addr, instance, desc_report, desc_len); + + return true; +} + +static void config_driver_mount_complete(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len) +{ + hidh_interface_t* hid_itf = get_instance(dev_addr, instance); + + // enumeration is complete + tuh_hid_mount_cb(dev_addr, instance, desc_report, desc_len); + + // notify usbh that driver enumeration is complete + usbh_driver_set_config_complete(dev_addr, hid_itf->itf_num); +} + +//--------------------------------------------------------------------+ +// Report Descriptor Parser +//--------------------------------------------------------------------+ + +uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr, uint8_t arr_count, uint8_t const* desc_report, uint16_t desc_len) +{ + // Report Item 6.2.2.2 USB HID 1.11 + union TU_ATTR_PACKED + { + uint8_t byte; + struct TU_ATTR_PACKED + { + uint8_t size : 2; + uint8_t type : 2; + uint8_t tag : 4; + }; + } header; + + tu_memclr(report_info_arr, arr_count*sizeof(tuh_hid_report_info_t)); + + uint8_t report_num = 0; + tuh_hid_report_info_t* info = report_info_arr; + + // current parsed report count & size from descriptor +// uint8_t ri_report_count = 0; +// uint8_t ri_report_size = 0; + + uint8_t ri_collection_depth = 0; + + while(desc_len && report_num < arr_count) + { + header.byte = *desc_report++; + desc_len--; + + uint8_t const tag = header.tag; + uint8_t const type = header.type; + uint8_t const size = header.size; + + uint8_t const data8 = desc_report[0]; + + TU_LOG(3, "tag = %d, type = %d, size = %d, data = ", tag, type, size); + for(uint32_t i=0; iusage_page, desc_report, size); + break; + + case RI_GLOBAL_LOGICAL_MIN : break; + case RI_GLOBAL_LOGICAL_MAX : break; + case RI_GLOBAL_PHYSICAL_MIN : break; + case RI_GLOBAL_PHYSICAL_MAX : break; + + case RI_GLOBAL_REPORT_ID: + info->report_id = data8; + break; + + case RI_GLOBAL_REPORT_SIZE: +// ri_report_size = data8; + break; + + case RI_GLOBAL_REPORT_COUNT: +// ri_report_count = data8; + break; + + case RI_GLOBAL_UNIT_EXPONENT : break; + case RI_GLOBAL_UNIT : break; + case RI_GLOBAL_PUSH : break; + case RI_GLOBAL_POP : break; + + default: break; + } + break; + + case RI_TYPE_LOCAL: + switch(tag) + { + case RI_LOCAL_USAGE: + // only take in account the "usage" before starting REPORT ID + if ( ri_collection_depth == 0 ) info->usage = data8; + break; + + case RI_LOCAL_USAGE_MIN : break; + case RI_LOCAL_USAGE_MAX : break; + case RI_LOCAL_DESIGNATOR_INDEX : break; + case RI_LOCAL_DESIGNATOR_MIN : break; + case RI_LOCAL_DESIGNATOR_MAX : break; + case RI_LOCAL_STRING_INDEX : break; + case RI_LOCAL_STRING_MIN : break; + case RI_LOCAL_STRING_MAX : break; + case RI_LOCAL_DELIMITER : break; + default: break; + } + break; + + // error + default: break; + } + + desc_report += size; + desc_len -= size; + } + + for ( uint8_t i = 0; i < report_num; i++ ) + { + info = report_info_arr+i; + TU_LOG2("%u: id = %u, usage_page = %u, usage = %u\r\n", i, info->report_id, info->usage_page, info->usage); + } + + return report_num; +} + +//--------------------------------------------------------------------+ +// Helper +//--------------------------------------------------------------------+ + +// Get Device by address +TU_ATTR_ALWAYS_INLINE static inline hidh_device_t* get_dev(uint8_t dev_addr) +{ + return &_hidh_dev[dev_addr-1]; +} + +// Get Interface by instance number +TU_ATTR_ALWAYS_INLINE static inline hidh_interface_t* get_instance(uint8_t dev_addr, uint8_t instance) +{ + return &_hidh_dev[dev_addr-1].instances[instance]; +} + +// Get instance ID by interface number +static uint8_t get_instance_id_by_itfnum(uint8_t dev_addr, uint8_t itf) +{ + for ( uint8_t inst = 0; inst < CFG_TUH_HID; inst++ ) + { + hidh_interface_t *hid = get_instance(dev_addr, inst); + + if ( (hid->itf_num == itf) && (hid->ep_in || hid->ep_out) ) return inst; + } + + return 0xff; +} + +// Get instance ID by endpoint address +static uint8_t get_instance_id_by_epaddr(uint8_t dev_addr, uint8_t ep_addr) +{ + for ( uint8_t inst = 0; inst < CFG_TUH_HID; inst++ ) + { + hidh_interface_t *hid = get_instance(dev_addr, inst); + + if ( (ep_addr == hid->ep_in) || ( ep_addr == hid->ep_out) ) return inst; + } + + return 0xff; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_host.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_host.h new file mode 100644 index 000000000..fe09b03b2 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/hid/hid_host.h @@ -0,0 +1,152 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_HID_HOST_H_ +#define _TUSB_HID_HOST_H_ + +#include "hid.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Driver Configuration +//--------------------------------------------------------------------+ + +// TODO Highspeed interrupt can be up to 512 bytes +#ifndef CFG_TUH_HID_EPIN_BUFSIZE +#define CFG_TUH_HID_EPIN_BUFSIZE 64 +#endif + +#ifndef CFG_TUH_HID_EPOUT_BUFSIZE +#define CFG_TUH_HID_EPOUT_BUFSIZE 64 +#endif + + +typedef struct +{ + uint8_t report_id; + uint8_t usage; + uint16_t usage_page; + + // TODO still use the endpoint size for now +// uint8_t in_len; // length of IN report +// uint8_t out_len; // length of OUT report +} tuh_hid_report_info_t; + +//--------------------------------------------------------------------+ +// Interface API +//--------------------------------------------------------------------+ + +// Get the number of HID instances +uint8_t tuh_hid_instance_count(uint8_t dev_addr); + +// Check if HID instance is mounted +bool tuh_hid_mounted(uint8_t dev_addr, uint8_t instance); + +// Get interface supported protocol (bInterfaceProtocol) check out hid_interface_protocol_enum_t for possible values +uint8_t tuh_hid_interface_protocol(uint8_t dev_addr, uint8_t instance); + +// Parse report descriptor into array of report_info struct and return number of reports. +// For complicated report, application should write its own parser. +uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* reports_info_arr, uint8_t arr_count, uint8_t const* desc_report, uint16_t desc_len) TU_ATTR_UNUSED; + +//--------------------------------------------------------------------+ +// Control Endpoint API +//--------------------------------------------------------------------+ + +// Get current protocol: HID_PROTOCOL_BOOT (0) or HID_PROTOCOL_REPORT (1) +// Note: Device will be initialized in Boot protocol for simplicity. +// Application can use set_protocol() to switch back to Report protocol. +uint8_t tuh_hid_get_protocol(uint8_t dev_addr, uint8_t instance); + +// Set protocol to HID_PROTOCOL_BOOT (0) or HID_PROTOCOL_REPORT (1) +// This function is only supported by Boot interface (tuh_n_hid_interface_protocol() != NONE) +bool tuh_hid_set_protocol(uint8_t dev_addr, uint8_t instance, uint8_t protocol); + +// Set Report using control endpoint +// report_type is either Intput, Output or Feature, (value from hid_report_type_t) +bool tuh_hid_set_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t report_type, void* report, uint16_t len); + +//--------------------------------------------------------------------+ +// Interrupt Endpoint API +//--------------------------------------------------------------------+ + +// Check if the interface is ready to use +//bool tuh_n_hid_n_ready(uint8_t dev_addr, uint8_t instance); + +// Try to receive next report on Interrupt Endpoint. Immediately return +// - true If succeeded, tuh_hid_report_received_cb() callback will be invoked when report is available +// - false if failed to queue the transfer e.g endpoint is busy +bool tuh_hid_receive_report(uint8_t dev_addr, uint8_t instance); + +// Send report using interrupt endpoint +// If report_id > 0 (composite), it will be sent as 1st byte, then report contents. Otherwise only report content is sent. +//void tuh_hid_send_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t const* report, uint16_t len); + +//--------------------------------------------------------------------+ +// Callbacks (Weak is optional) +//--------------------------------------------------------------------+ + +// Invoked when device with hid interface is mounted +// Report descriptor is also available for use. tuh_hid_parse_report_descriptor() +// can be used to parse common/simple enough descriptor. +// Note: if report descriptor length > CFG_TUH_ENUMERATION_BUFSIZE, it will be skipped +// therefore report_desc = NULL, desc_len = 0 +void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report_desc, uint16_t desc_len); + +// Invoked when device with hid interface is un-mounted +TU_ATTR_WEAK void tuh_hid_umount_cb(uint8_t dev_addr, uint8_t instance); + +// Invoked when received report from device via interrupt endpoint +// Note: if there is report ID (composite), it is 1st byte of report +void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len); + +// Invoked when sent report to device successfully via interrupt endpoint +TU_ATTR_WEAK void tuh_hid_report_sent_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len); + +// Invoked when Sent Report to device via either control endpoint +// len = 0 indicate there is error in the transfer e.g stalled response +TU_ATTR_WEAK void tuh_hid_set_report_complete_cb(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t report_type, uint16_t len); + +// Invoked when Set Protocol request is complete +TU_ATTR_WEAK void tuh_hid_set_protocol_complete_cb(uint8_t dev_addr, uint8_t instance, uint8_t protocol); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void hidh_init (void); +bool hidh_open (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len); +bool hidh_set_config (uint8_t dev_addr, uint8_t itf_num); +bool hidh_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); +void hidh_close (uint8_t dev_addr); + +#ifdef __cplusplus +} +#endif + +#endif /* _TUSB_HID_HOST_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi.h new file mode 100644 index 000000000..74dc41749 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi.h @@ -0,0 +1,212 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup group_class + * \defgroup ClassDriver_CDC Communication Device Class (CDC) + * Currently only Abstract Control Model subclass is supported + * @{ */ + +#ifndef _TUSB_MIDI_H__ +#define _TUSB_MIDI_H__ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Specific Descriptor +//--------------------------------------------------------------------+ + +typedef enum +{ + MIDI_CS_INTERFACE_HEADER = 0x01, + MIDI_CS_INTERFACE_IN_JACK = 0x02, + MIDI_CS_INTERFACE_OUT_JACK = 0x03, + MIDI_CS_INTERFACE_ELEMENT = 0x04, +} midi_cs_interface_subtype_t; + +typedef enum +{ + MIDI_CS_ENDPOINT_GENERAL = 0x01 +} midi_cs_endpoint_subtype_t; + +typedef enum +{ + MIDI_JACK_EMBEDDED = 0x01, + MIDI_JACK_EXTERNAL = 0x02 +} midi_jack_type_t; + +typedef enum +{ + MIDI_CIN_MISC = 0, + MIDI_CIN_CABLE_EVENT = 1, + MIDI_CIN_SYSCOM_2BYTE = 2, // 2 byte system common message e.g MTC, SongSelect + MIDI_CIN_SYSCOM_3BYTE = 3, // 3 byte system common message e.g SPP + MIDI_CIN_SYSEX_START = 4, // SysEx starts or continue + MIDI_CIN_SYSEX_END_1BYTE = 5, // SysEx ends with 1 data, or 1 byte system common message + MIDI_CIN_SYSEX_END_2BYTE = 6, // SysEx ends with 2 data + MIDI_CIN_SYSEX_END_3BYTE = 7, // SysEx ends with 3 data + MIDI_CIN_NOTE_ON = 8, + MIDI_CIN_NOTE_OFF = 9, + MIDI_CIN_POLY_KEYPRESS = 10, + MIDI_CIN_CONTROL_CHANGE = 11, + MIDI_CIN_PROGRAM_CHANGE = 12, + MIDI_CIN_CHANNEL_PRESSURE = 13, + MIDI_CIN_PITCH_BEND_CHANGE = 14, + MIDI_CIN_1BYTE_DATA = 15 +} midi_code_index_number_t; + +// MIDI 1.0 status byte +enum +{ + //------------- System Exclusive -------------// + MIDI_STATUS_SYSEX_START = 0xF0, + MIDI_STATUS_SYSEX_END = 0xF7, + + //------------- System Common -------------// + MIDI_STATUS_SYSCOM_TIME_CODE_QUARTER_FRAME = 0xF1, + MIDI_STATUS_SYSCOM_SONG_POSITION_POINTER = 0xF2, + MIDI_STATUS_SYSCOM_SONG_SELECT = 0xF3, + // F4, F5 is undefined + MIDI_STATUS_SYSCOM_TUNE_REQUEST = 0xF6, + + //------------- System RealTime -------------// + MIDI_STATUS_SYSREAL_TIMING_CLOCK = 0xF8, + // 0xF9 is undefined + MIDI_STATUS_SYSREAL_START = 0xFA, + MIDI_STATUS_SYSREAL_CONTINUE = 0xFB, + MIDI_STATUS_SYSREAL_STOP = 0xFC, + // 0xFD is undefined + MIDI_STATUS_SYSREAL_ACTIVE_SENSING = 0xFE, + MIDI_STATUS_SYSREAL_SYSTEM_RESET = 0xFF, +}; + +/// MIDI Interface Header Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType + uint16_t bcdMSC ; ///< MidiStreaming SubClass release number in Binary-Coded Decimal + uint16_t wTotalLength ; +} midi_desc_header_t; + +/// MIDI In Jack Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType + uint8_t bJackType ; ///< Embedded or External + uint8_t bJackID ; ///< Unique ID for MIDI IN Jack + uint8_t iJack ; ///< string descriptor +} midi_desc_in_jack_t; + + +/// MIDI Out Jack Descriptor with single pin +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType + uint8_t bJackType ; ///< Embedded or External + uint8_t bJackID ; ///< Unique ID for MIDI IN Jack + uint8_t bNrInputPins; + + uint8_t baSourceID; + uint8_t baSourcePin; + + uint8_t iJack ; ///< string descriptor +} midi_desc_out_jack_t ; + +/// MIDI Out Jack Descriptor with multiple pins +#define midi_desc_out_jack_n_t(input_num) \ + struct TU_ATTR_PACKED { \ + uint8_t bLength ; \ + uint8_t bDescriptorType ; \ + uint8_t bDescriptorSubType ; \ + uint8_t bJackType ; \ + uint8_t bJackID ; \ + uint8_t bNrInputPins ; \ + struct TU_ATTR_PACKED { \ + uint8_t baSourceID; \ + uint8_t baSourcePin; \ + } pins[input_num]; \ + uint8_t iJack ; \ + } + +/// MIDI Element Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< Descriptor Type, must be Class-Specific + uint8_t bDescriptorSubType ; ///< Descriptor SubType + uint8_t bElementID; + + uint8_t bNrInputPins; + uint8_t baSourceID; + uint8_t baSourcePin; + + uint8_t bNrOutputPins; + uint8_t bInTerminalLink; + uint8_t bOutTerminalLink; + uint8_t bElCapsSize; + + uint16_t bmElementCaps; + uint8_t iElement; +} midi_desc_element_t; + +/// MIDI Element Descriptor with multiple pins +#define midi_desc_element_n_t(input_num) \ + struct TU_ATTR_PACKED { \ + uint8_t bLength; \ + uint8_t bDescriptorType; \ + uint8_t bDescriptorSubType; \ + uint8_t bElementID; \ + uint8_t bNrInputPins; \ + struct TU_ATTR_PACKED { \ + uint8_t baSourceID; \ + uint8_t baSourcePin; \ + } pins[input_num]; \ + uint8_t bNrOutputPins; \ + uint8_t bInTerminalLink; \ + uint8_t bOutTerminalLink; \ + uint8_t bElCapsSize; \ + uint16_t bmElementCaps; \ + uint8_t iElement; \ + } + +/** @} */ + +#ifdef __cplusplus + } +#endif + +#endif + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi_device.c new file mode 100644 index 000000000..b08b362f9 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi_device.c @@ -0,0 +1,545 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_MIDI) + +//--------------------------------------------------------------------+ +// INCLUDE +//--------------------------------------------------------------------+ +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "midi_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +typedef struct +{ + uint8_t buffer[4]; + uint8_t index; + uint8_t total; +}midid_stream_t; + +typedef struct +{ + uint8_t itf_num; + uint8_t ep_in; + uint8_t ep_out; + + // For Stream read()/write() API + // Messages are always 4 bytes long, queue them for reading and writing so the + // callers can use the Stream interface with single-byte read/write calls. + midid_stream_t stream_write; + midid_stream_t stream_read; + + /*------------- From this point, data is not cleared by bus reset -------------*/ + // FIFO + tu_fifo_t rx_ff; + tu_fifo_t tx_ff; + uint8_t rx_ff_buf[CFG_TUD_MIDI_RX_BUFSIZE]; + uint8_t tx_ff_buf[CFG_TUD_MIDI_TX_BUFSIZE]; + + #if CFG_FIFO_MUTEX + osal_mutex_def_t rx_ff_mutex; + osal_mutex_def_t tx_ff_mutex; + #endif + + // Endpoint Transfer buffer + CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_MIDI_EP_BUFSIZE]; + CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_MIDI_EP_BUFSIZE]; + +} midid_interface_t; + +#define ITF_MEM_RESET_SIZE offsetof(midid_interface_t, rx_ff) + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +CFG_TUSB_MEM_SECTION midid_interface_t _midid_itf[CFG_TUD_MIDI]; + +bool tud_midi_n_mounted (uint8_t itf) +{ + midid_interface_t* midi = &_midid_itf[itf]; + return midi->ep_in && midi->ep_out; +} + +static void _prep_out_transaction (midid_interface_t* p_midi) +{ + uint8_t const rhport = TUD_OPT_RHPORT; + uint16_t available = tu_fifo_remaining(&p_midi->rx_ff); + + // Prepare for incoming data but only allow what we can store in the ring buffer. + // TODO Actually we can still carry out the transfer, keeping count of received bytes + // and slowly move it to the FIFO when read(). + // This pre-check reduces endpoint claiming + TU_VERIFY(available >= sizeof(p_midi->epout_buf), ); + + // claim endpoint + TU_VERIFY(usbd_edpt_claim(rhport, p_midi->ep_out), ); + + // fifo can be changed before endpoint is claimed + available = tu_fifo_remaining(&p_midi->rx_ff); + + if ( available >= sizeof(p_midi->epout_buf) ) { + usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, sizeof(p_midi->epout_buf)); + }else + { + // Release endpoint since we don't make any transfer + usbd_edpt_release(rhport, p_midi->ep_out); + } +} + +//--------------------------------------------------------------------+ +// READ API +//--------------------------------------------------------------------+ +uint32_t tud_midi_n_available(uint8_t itf, uint8_t cable_num) +{ + (void) cable_num; + + midid_interface_t* midi = &_midid_itf[itf]; + midid_stream_t const* stream = &midi->stream_read; + + // when using with packet API stream total & index are both zero + return tu_fifo_count(&midi->rx_ff) + (stream->total - stream->index); +} + +uint32_t tud_midi_n_stream_read(uint8_t itf, uint8_t cable_num, void* buffer, uint32_t bufsize) +{ + (void) cable_num; + TU_VERIFY(bufsize, 0); + + uint8_t* buf8 = (uint8_t*) buffer; + + midid_interface_t* midi = &_midid_itf[itf]; + midid_stream_t* stream = &midi->stream_read; + + uint32_t total_read = 0; + while( bufsize ) + { + // Get new packet from fifo, then set packet expected bytes + if ( stream->total == 0 ) + { + // return if there is no more data from fifo + if ( !tud_midi_n_packet_read(itf, stream->buffer) ) return total_read; + + uint8_t const code_index = stream->buffer[0] & 0x0f; + + // MIDI 1.0 Table 4-1: Code Index Number Classifications + switch(code_index) + { + case MIDI_CIN_MISC: + case MIDI_CIN_CABLE_EVENT: + // These are reserved and unused, possibly issue somewhere, skip this packet + return 0; + break; + + case MIDI_CIN_SYSEX_END_1BYTE: + case MIDI_CIN_1BYTE_DATA: + stream->total = 1; + break; + + case MIDI_CIN_SYSCOM_2BYTE : + case MIDI_CIN_SYSEX_END_2BYTE : + case MIDI_CIN_PROGRAM_CHANGE : + case MIDI_CIN_CHANNEL_PRESSURE : + stream->total = 2; + break; + + default: + stream->total = 3; + break; + } + } + + // Copy data up to bufsize + uint32_t const count = tu_min32(stream->total - stream->index, bufsize); + + // Skip the header (1st byte) in the buffer + memcpy(buf8, stream->buffer + 1 + stream->index, count); + + total_read += count; + stream->index += count; + buf8 += count; + bufsize -= count; + + // complete current event packet, reset stream + if ( stream->total == stream->index ) + { + stream->index = 0; + stream->total = 0; + } + } + + return total_read; +} + +bool tud_midi_n_packet_read (uint8_t itf, uint8_t packet[4]) +{ + midid_interface_t* midi = &_midid_itf[itf]; + TU_VERIFY(midi->ep_out); + + uint32_t const num_read = tu_fifo_read_n(&midi->rx_ff, packet, 4); + _prep_out_transaction(midi); + return (num_read == 4); +} + +//--------------------------------------------------------------------+ +// WRITE API +//--------------------------------------------------------------------+ + +static uint32_t write_flush(midid_interface_t* midi) +{ + // No data to send + if ( !tu_fifo_count(&midi->tx_ff) ) return 0; + + uint8_t const rhport = TUD_OPT_RHPORT; + + // skip if previous transfer not complete + TU_VERIFY( usbd_edpt_claim(rhport, midi->ep_in), 0 ); + + uint16_t count = tu_fifo_read_n(&midi->tx_ff, midi->epin_buf, CFG_TUD_MIDI_EP_BUFSIZE); + + if (count) + { + TU_ASSERT( usbd_edpt_xfer(rhport, midi->ep_in, midi->epin_buf, count), 0 ); + return count; + }else + { + // Release endpoint since we don't make any transfer + usbd_edpt_release(rhport, midi->ep_in); + return 0; + } +} + +uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, uint8_t const* buffer, uint32_t bufsize) +{ + midid_interface_t* midi = &_midid_itf[itf]; + TU_VERIFY(midi->ep_in, 0); + + midid_stream_t* stream = &midi->stream_write; + + uint32_t i = 0; + while ( (i < bufsize) && (tu_fifo_remaining(&midi->tx_ff) >= 4) ) + { + uint8_t const data = buffer[i]; + i++; + + if ( stream->index == 0 ) + { + //------------- New event packet -------------// + + uint8_t const msg = data >> 4; + + stream->index = 2; + stream->buffer[1] = data; + + // Check to see if we're still in a SysEx transmit. + if ( stream->buffer[0] == MIDI_CIN_SYSEX_START ) + { + if ( data == MIDI_STATUS_SYSEX_END ) + { + stream->buffer[0] = MIDI_CIN_SYSEX_END_1BYTE; + stream->total = 2; + } + else + { + stream->total = 4; + } + } + else if ( (msg >= 0x8 && msg <= 0xB) || msg == 0xE ) + { + // Channel Voice Messages + stream->buffer[0] = (cable_num << 4) | msg; + stream->total = 4; + } + else if ( msg == 0xC || msg == 0xD) + { + // Channel Voice Messages, two-byte variants (Program Change and Channel Pressure) + stream->buffer[0] = (cable_num << 4) | msg; + stream->total = 3; + } + else if ( msg == 0xf ) + { + // System message + if ( data == MIDI_STATUS_SYSEX_START ) + { + stream->buffer[0] = MIDI_CIN_SYSEX_START; + stream->total = 4; + } + else if ( data == MIDI_STATUS_SYSCOM_TIME_CODE_QUARTER_FRAME || data == MIDI_STATUS_SYSCOM_SONG_SELECT ) + { + stream->buffer[0] = MIDI_CIN_SYSCOM_2BYTE; + stream->total = 3; + } + else if ( data == MIDI_STATUS_SYSCOM_SONG_POSITION_POINTER ) + { + stream->buffer[0] = MIDI_CIN_SYSCOM_3BYTE; + stream->total = 4; + } + else + { + stream->buffer[0] = MIDI_CIN_SYSEX_END_1BYTE; + stream->total = 2; + } + } + else + { + // Pack individual bytes if we don't support packing them into words. + stream->buffer[0] = cable_num << 4 | 0xf; + stream->buffer[2] = 0; + stream->buffer[3] = 0; + stream->index = 2; + stream->total = 2; + } + } + else + { + //------------- On-going (buffering) packet -------------// + + TU_ASSERT(stream->index < 4, i); + stream->buffer[stream->index] = data; + stream->index++; + + // See if this byte ends a SysEx. + if ( stream->buffer[0] == MIDI_CIN_SYSEX_START && data == MIDI_STATUS_SYSEX_END ) + { + stream->buffer[0] = MIDI_CIN_SYSEX_START + (stream->index - 1); + stream->total = stream->index; + } + } + + // Send out packet + if ( stream->index == stream->total ) + { + // zeroes unused bytes + for(uint8_t idx = stream->total; idx < 4; idx++) stream->buffer[idx] = 0; + + uint16_t const count = tu_fifo_write_n(&midi->tx_ff, stream->buffer, 4); + + // complete current event packet, reset stream + stream->index = stream->total = 0; + + // FIFO overflown, since we already check fifo remaining. It is probably race condition + TU_ASSERT(count == 4, i); + } + } + + write_flush(midi); + + return i; +} + +bool tud_midi_n_packet_write (uint8_t itf, uint8_t const packet[4]) +{ + midid_interface_t* midi = &_midid_itf[itf]; + TU_VERIFY(midi->ep_in); + + if (tu_fifo_remaining(&midi->tx_ff) < 4) return false; + + tu_fifo_write_n(&midi->tx_ff, packet, 4); + write_flush(midi); + + return true; +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void midid_init(void) +{ + tu_memclr(_midid_itf, sizeof(_midid_itf)); + + for(uint8_t i=0; irx_ff, midi->rx_ff_buf, CFG_TUD_MIDI_RX_BUFSIZE, 1, false); // true, true + tu_fifo_config(&midi->tx_ff, midi->tx_ff_buf, CFG_TUD_MIDI_TX_BUFSIZE, 1, false); // OBVS. + + #if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&midi->rx_ff, NULL, osal_mutex_create(&midi->rx_ff_mutex)); + tu_fifo_config_mutex(&midi->tx_ff, osal_mutex_create(&midi->tx_ff_mutex), NULL); + #endif + } +} + +void midid_reset(uint8_t rhport) +{ + (void) rhport; + + for(uint8_t i=0; irx_ff); + tu_fifo_clear(&midi->tx_ff); + } +} + +uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint16_t max_len) +{ + // 1st Interface is Audio Control v1 + TU_VERIFY(TUSB_CLASS_AUDIO == desc_itf->bInterfaceClass && + AUDIO_SUBCLASS_CONTROL == desc_itf->bInterfaceSubClass && + AUDIO_FUNC_PROTOCOL_CODE_UNDEF == desc_itf->bInterfaceProtocol, 0); + + uint16_t drv_len = tu_desc_len(desc_itf); + uint8_t const * p_desc = tu_desc_next(desc_itf); + + // Skip Class Specific descriptors + while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len ) + { + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + // 2nd Interface is MIDI Streaming + TU_VERIFY(TUSB_DESC_INTERFACE == tu_desc_type(p_desc), 0); + tusb_desc_interface_t const * desc_midi = (tusb_desc_interface_t const *) p_desc; + + TU_VERIFY(TUSB_CLASS_AUDIO == desc_midi->bInterfaceClass && + AUDIO_SUBCLASS_MIDI_STREAMING == desc_midi->bInterfaceSubClass && + AUDIO_FUNC_PROTOCOL_CODE_UNDEF == desc_midi->bInterfaceProtocol, 0); + + // Find available interface + midid_interface_t * p_midi = NULL; + for(uint8_t i=0; iitf_num = desc_midi->bInterfaceNumber; + (void) p_midi->itf_num; + + // next descriptor + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + + // Find and open endpoint descriptors + uint8_t found_endpoints = 0; + while ( (found_endpoints < desc_midi->bNumEndpoints) && (drv_len <= max_len) ) + { + if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) ) + { + TU_ASSERT(usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0); + uint8_t ep_addr = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress; + + if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN) + { + p_midi->ep_in = ep_addr; + } else { + p_midi->ep_out = ep_addr; + } + + // Class Specific MIDI Stream endpoint descriptor + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + + found_endpoints += 1; + } + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + // Prepare for incoming data + _prep_out_transaction(p_midi); + + return drv_len; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool midid_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + (void) rhport; + (void) stage; + (void) request; + + // driver doesn't support any request yet + return false; +} + +bool midid_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) result; + (void) rhport; + + uint8_t itf; + midid_interface_t* p_midi; + + // Identify which interface to use + for (itf = 0; itf < CFG_TUD_MIDI; itf++) + { + p_midi = &_midid_itf[itf]; + if ( ( ep_addr == p_midi->ep_out ) || ( ep_addr == p_midi->ep_in ) ) break; + } + TU_ASSERT(itf < CFG_TUD_MIDI); + + // receive new data + if ( ep_addr == p_midi->ep_out ) + { + tu_fifo_write_n(&p_midi->rx_ff, p_midi->epout_buf, xferred_bytes); + + // invoke receive callback if available + if (tud_midi_rx_cb) tud_midi_rx_cb(itf); + + // prepare for next + // TODO for now ep_out is not used by public API therefore there is no race condition, + // and does not need to claim like ep_in + _prep_out_transaction(p_midi); + } + else if ( ep_addr == p_midi->ep_in ) + { + if (0 == write_flush(p_midi)) + { + // If there is no data left, a ZLP should be sent if + // xferred_bytes is multiple of EP size and not zero + if ( !tu_fifo_count(&p_midi->tx_ff) && xferred_bytes && (0 == (xferred_bytes % CFG_TUD_MIDI_EP_BUFSIZE)) ) + { + if ( usbd_edpt_claim(rhport, p_midi->ep_in) ) + { + usbd_edpt_xfer(rhport, p_midi->ep_in, NULL, 0); + } + } + } + } + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi_device.h new file mode 100644 index 000000000..211edc8d1 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/midi/midi_device.h @@ -0,0 +1,173 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_MIDI_DEVICE_H_ +#define _TUSB_MIDI_DEVICE_H_ + +#include "class/audio/audio.h" +#include "midi.h" + +//--------------------------------------------------------------------+ +// Class Driver Configuration +//--------------------------------------------------------------------+ + +#if !defined(CFG_TUD_MIDI_EP_BUFSIZE) && defined(CFG_TUD_MIDI_EPSIZE) + #warning CFG_TUD_MIDI_EPSIZE is renamed to CFG_TUD_MIDI_EP_BUFSIZE, please update to use the new name + #define CFG_TUD_MIDI_EP_BUFSIZE CFG_TUD_MIDI_EPSIZE +#endif + +#ifndef CFG_TUD_MIDI_EP_BUFSIZE + #define CFG_TUD_MIDI_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +/** \addtogroup MIDI_Serial Serial + * @{ + * \defgroup MIDI_Serial_Device Device + * @{ */ + +//--------------------------------------------------------------------+ +// Application API (Multiple Interfaces) +// CFG_TUD_MIDI > 1 +//--------------------------------------------------------------------+ + +// Check if midi interface is mounted +bool tud_midi_n_mounted (uint8_t itf); + +// Get the number of bytes available for reading +uint32_t tud_midi_n_available (uint8_t itf, uint8_t cable_num); + +// Read byte stream (legacy) +uint32_t tud_midi_n_stream_read (uint8_t itf, uint8_t cable_num, void* buffer, uint32_t bufsize); + +// Write byte Stream (legacy) +uint32_t tud_midi_n_stream_write (uint8_t itf, uint8_t cable_num, uint8_t const* buffer, uint32_t bufsize); + +// Read event packet (4 bytes) +bool tud_midi_n_packet_read (uint8_t itf, uint8_t packet[4]); + +// Write event packet (4 bytes) +bool tud_midi_n_packet_write (uint8_t itf, uint8_t const packet[4]); + +//--------------------------------------------------------------------+ +// Application API (Single Interface) +//--------------------------------------------------------------------+ +static inline bool tud_midi_mounted (void); +static inline uint32_t tud_midi_available (void); + +static inline uint32_t tud_midi_stream_read (void* buffer, uint32_t bufsize); +static inline uint32_t tud_midi_stream_write (uint8_t cable_num, uint8_t const* buffer, uint32_t bufsize); + +static inline bool tud_midi_packet_read (uint8_t packet[4]); +static inline bool tud_midi_packet_write (uint8_t const packet[4]); + +//------------- Deprecated API name -------------// +// TODO remove after 0.10.0 release + +TU_ATTR_DEPRECATED("tud_midi_read() is renamed to tud_midi_stream_read()") +static inline uint32_t tud_midi_read (void* buffer, uint32_t bufsize) +{ + return tud_midi_stream_read(buffer, bufsize); +} + +TU_ATTR_DEPRECATED("tud_midi_write() is renamed to tud_midi_stream_write()") +static inline uint32_t tud_midi_write(uint8_t cable_num, uint8_t const* buffer, uint32_t bufsize) +{ + return tud_midi_stream_write(cable_num, buffer, bufsize); +} + + +TU_ATTR_DEPRECATED("tud_midi_send() is renamed to tud_midi_packet_write()") +static inline bool tud_midi_send(uint8_t packet[4]) +{ + return tud_midi_packet_write(packet); +} + +TU_ATTR_DEPRECATED("tud_midi_receive() is renamed to tud_midi_packet_read()") +static inline bool tud_midi_receive(uint8_t packet[4]) +{ + return tud_midi_packet_read(packet); +} + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ +TU_ATTR_WEAK void tud_midi_rx_cb(uint8_t itf); + +//--------------------------------------------------------------------+ +// Inline Functions +//--------------------------------------------------------------------+ + +static inline bool tud_midi_mounted (void) +{ + return tud_midi_n_mounted(0); +} + +static inline uint32_t tud_midi_available (void) +{ + return tud_midi_n_available(0, 0); +} + +static inline uint32_t tud_midi_stream_read (void* buffer, uint32_t bufsize) +{ + return tud_midi_n_stream_read(0, 0, buffer, bufsize); +} + +static inline uint32_t tud_midi_stream_write (uint8_t cable_num, uint8_t const* buffer, uint32_t bufsize) +{ + return tud_midi_n_stream_write(0, cable_num, buffer, bufsize); +} + +static inline bool tud_midi_packet_read (uint8_t packet[4]) +{ + return tud_midi_n_packet_read(0, packet); +} + +static inline bool tud_midi_packet_write (uint8_t const packet[4]) +{ + return tud_midi_n_packet_write(0, packet); +} + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void midid_init (void); +void midid_reset (uint8_t rhport); +uint16_t midid_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool midid_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +bool midid_xfer_cb (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_MIDI_DEVICE_H_ */ + +/** @} */ +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc.h new file mode 100644 index 000000000..84b6e4d79 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc.h @@ -0,0 +1,382 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_MSC_H_ +#define _TUSB_MSC_H_ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Mass Storage Class Constant +//--------------------------------------------------------------------+ +/// MassStorage Subclass +typedef enum +{ + MSC_SUBCLASS_RBC = 1 , ///< Reduced Block Commands (RBC) T10 Project 1240-D + MSC_SUBCLASS_SFF_MMC , ///< SFF-8020i, MMC-2 (ATAPI). Typically used by a CD/DVD device + MSC_SUBCLASS_QIC , ///< QIC-157. Typically used by a tape device + MSC_SUBCLASS_UFI , ///< UFI. Typically used by Floppy Disk Drive (FDD) device + MSC_SUBCLASS_SFF , ///< SFF-8070i. Can be used by Floppy Disk Drive (FDD) device + MSC_SUBCLASS_SCSI ///< SCSI transparent command set +}msc_subclass_type_t; + +enum { + MSC_CBW_SIGNATURE = 0x43425355, ///< Constant value of 43425355h (little endian) + MSC_CSW_SIGNATURE = 0x53425355 ///< Constant value of 53425355h (little endian) +}; + +/// \brief MassStorage Protocol. +/// \details CBI only approved to use with full-speed floopy disk & should not used with highspeed or device other than floopy +typedef enum +{ + MSC_PROTOCOL_CBI = 0 , ///< Control/Bulk/Interrupt protocol (with command completion interrupt) + MSC_PROTOCOL_CBI_NO_INTERRUPT = 1 , ///< Control/Bulk/Interrupt protocol (without command completion interrupt) + MSC_PROTOCOL_BOT = 0x50 ///< Bulk-Only Transport +}msc_protocol_type_t; + +/// MassStorage Class-Specific Control Request +typedef enum +{ + MSC_REQ_GET_MAX_LUN = 254, ///< The Get Max LUN device request is used to determine the number of logical units supported by the device. Logical Unit Numbers on the device shall be numbered contiguously starting from LUN 0 to a maximum LUN of 15 + MSC_REQ_RESET = 255 ///< This request is used to reset the mass storage device and its associated interface. This class-specific request shall ready the device for the next CBW from the host. +}msc_request_type_t; + +/// \brief Command Block Status Values +/// \details Indicates the success or failure of the command. The device shall set this byte to zero if the command completed +/// successfully. A non-zero value shall indicate a failure during command execution according to the following +typedef enum +{ + MSC_CSW_STATUS_PASSED = 0 , ///< MSC_CSW_STATUS_PASSED + MSC_CSW_STATUS_FAILED , ///< MSC_CSW_STATUS_FAILED + MSC_CSW_STATUS_PHASE_ERROR ///< MSC_CSW_STATUS_PHASE_ERROR +}msc_csw_status_t; + +/// Command Block Wrapper +typedef struct TU_ATTR_PACKED +{ + uint32_t signature; ///< Signature that helps identify this data packet as a CBW. The signature field shall contain the value 43425355h (little endian), indicating a CBW. + uint32_t tag; ///< Tag sent by the host. The device shall echo the contents of this field back to the host in the dCSWTagfield of the associated CSW. The dCSWTagpositively associates a CSW with the corresponding CBW. + uint32_t total_bytes; ///< The number of bytes of data that the host expects to transfer on the Bulk-In or Bulk-Out endpoint (as indicated by the Direction bit) during the execution of this command. If this field is zero, the device and the host shall transfer no data between the CBW and the associated CSW, and the device shall ignore the value of the Direction bit in bmCBWFlags. + uint8_t dir; ///< Bit 7 of this field define transfer direction \n - 0 : Data-Out from host to the device. \n - 1 : Data-In from the device to the host. + uint8_t lun; ///< The device Logical Unit Number (LUN) to which the command block is being sent. For devices that support multiple LUNs, the host shall place into this field the LUN to which this command block is addressed. Otherwise, the host shall set this field to zero. + uint8_t cmd_len; ///< The valid length of the CBWCBin bytes. This defines the valid length of the command block. The only legal values are 1 through 16 + uint8_t command[16]; ///< The command block to be executed by the device. The device shall interpret the first cmd_len bytes in this field as a command block +}msc_cbw_t; + +TU_VERIFY_STATIC(sizeof(msc_cbw_t) == 31, "size is not correct"); + +/// Command Status Wrapper +typedef struct TU_ATTR_PACKED +{ + uint32_t signature ; ///< Signature that helps identify this data packet as a CSW. The signature field shall contain the value 53425355h (little endian), indicating CSW. + uint32_t tag ; ///< The device shall set this field to the value received in the dCBWTag of the associated CBW. + uint32_t data_residue ; ///< For Data-Out the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLength, and the actual amount of data processed by the device. For Data-In the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLengthand the actual amount of relevant data sent by the device + uint8_t status ; ///< indicates the success or failure of the command. Values from \ref msc_csw_status_t +}msc_csw_t; + +TU_VERIFY_STATIC(sizeof(msc_csw_t) == 13, "size is not correct"); + +//--------------------------------------------------------------------+ +// SCSI Constant +//--------------------------------------------------------------------+ + +/// SCSI Command Operation Code +typedef enum +{ + SCSI_CMD_TEST_UNIT_READY = 0x00, ///< The SCSI Test Unit Ready command is used to determine if a device is ready to transfer data (read/write), i.e. if a disk has spun up, if a tape is loaded and ready etc. The device does not perform a self-test operation. + SCSI_CMD_INQUIRY = 0x12, ///< The SCSI Inquiry command is used to obtain basic information from a target device. + SCSI_CMD_MODE_SELECT_6 = 0x15, ///< provides a means for the application client to specify medium, logical unit, or peripheral device parameters to the device server. Device servers that implement the MODE SELECT(6) command shall also implement the MODE SENSE(6) command. Application clients should issue MODE SENSE(6) prior to each MODE SELECT(6) to determine supported mode pages, page lengths, and other parameters. + SCSI_CMD_MODE_SENSE_6 = 0x1A, ///< provides a means for a device server to report parameters to an application client. It is a complementary command to the MODE SELECT(6) command. Device servers that implement the MODE SENSE(6) command shall also implement the MODE SELECT(6) command. + SCSI_CMD_START_STOP_UNIT = 0x1B, + SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL = 0x1E, + SCSI_CMD_READ_CAPACITY_10 = 0x25, ///< The SCSI Read Capacity command is used to obtain data capacity information from a target device. + SCSI_CMD_REQUEST_SENSE = 0x03, ///< The SCSI Request Sense command is part of the SCSI computer protocol standard. This command is used to obtain sense data -- status/error information -- from a target device. + SCSI_CMD_READ_FORMAT_CAPACITY = 0x23, ///< The command allows the Host to request a list of the possible format capacities for an installed writable media. This command also has the capability to report the writable capacity for a media when it is installed + SCSI_CMD_READ_10 = 0x28, ///< The READ (10) command requests that the device server read the specified logical block(s) and transfer them to the data-in buffer. + SCSI_CMD_WRITE_10 = 0x2A, ///< The WRITE (10) command requests thatthe device server transfer the specified logical block(s) from the data-out buffer and write them. +}scsi_cmd_type_t; + +/// SCSI Sense Key +typedef enum +{ + SCSI_SENSE_NONE = 0x00, ///< no specific Sense Key. This would be the case for a successful command + SCSI_SENSE_RECOVERED_ERROR = 0x01, ///< ndicates the last command completed successfully with some recovery action performed by the disc drive. + SCSI_SENSE_NOT_READY = 0x02, ///< Indicates the logical unit addressed cannot be accessed. + SCSI_SENSE_MEDIUM_ERROR = 0x03, ///< Indicates the command terminated with a non-recovered error condition. + SCSI_SENSE_HARDWARE_ERROR = 0x04, ///< Indicates the disc drive detected a nonrecoverable hardware failure while performing the command or during a self test. + SCSI_SENSE_ILLEGAL_REQUEST = 0x05, ///< Indicates an illegal parameter in the command descriptor block or in the additional parameters + SCSI_SENSE_UNIT_ATTENTION = 0x06, ///< Indicates the disc drive may have been reset. + SCSI_SENSE_DATA_PROTECT = 0x07, ///< Indicates that a command that reads or writes the medium was attempted on a block that is protected from this operation. The read or write operation is not performed. + SCSI_SENSE_FIRMWARE_ERROR = 0x08, ///< Vendor specific sense key. + SCSI_SENSE_ABORTED_COMMAND = 0x0b, ///< Indicates the disc drive aborted the command. + SCSI_SENSE_EQUAL = 0x0c, ///< Indicates a SEARCH DATA command has satisfied an equal comparison. + SCSI_SENSE_VOLUME_OVERFLOW = 0x0d, ///< Indicates a buffered peripheral device has reached the end of medium partition and data remains in the buffer that has not been written to the medium. + SCSI_SENSE_MISCOMPARE = 0x0e ///< ndicates that the source data did not match the data read from the medium. +}scsi_sense_key_type_t; + +//--------------------------------------------------------------------+ +// SCSI Primary Command (SPC-4) +//--------------------------------------------------------------------+ + +/// SCSI Test Unit Ready Command +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code ; ///< SCSI OpCode for \ref SCSI_CMD_TEST_UNIT_READY + uint8_t lun ; ///< Logical Unit + uint8_t reserved[3] ; + uint8_t control ; +} scsi_test_unit_ready_t; + +TU_VERIFY_STATIC(sizeof(scsi_test_unit_ready_t) == 6, "size is not correct"); + +/// SCSI Inquiry Command +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code ; ///< SCSI OpCode for \ref SCSI_CMD_INQUIRY + uint8_t reserved1 ; + uint8_t page_code ; + uint8_t reserved2 ; + uint8_t alloc_length ; ///< specifies the maximum number of bytes that USB host has allocated in the Data-In Buffer. An allocation length of zero specifies that no data shall be transferred. + uint8_t control ; +} scsi_inquiry_t, scsi_request_sense_t; + +TU_VERIFY_STATIC(sizeof(scsi_inquiry_t) == 6, "size is not correct"); + +/// SCSI Inquiry Response Data +typedef struct TU_ATTR_PACKED +{ + uint8_t peripheral_device_type : 5; + uint8_t peripheral_qualifier : 3; + + uint8_t : 7; + uint8_t is_removable : 1; + + uint8_t version; + + uint8_t response_data_format : 4; + uint8_t hierarchical_support : 1; + uint8_t normal_aca : 1; + uint8_t : 2; + + uint8_t additional_length; + + uint8_t protect : 1; + uint8_t : 2; + uint8_t third_party_copy : 1; + uint8_t target_port_group_support : 2; + uint8_t access_control_coordinator : 1; + uint8_t scc_support : 1; + + uint8_t addr16 : 1; + uint8_t : 3; + uint8_t multi_port : 1; + uint8_t : 1; // vendor specific + uint8_t enclosure_service : 1; + uint8_t : 1; + + uint8_t : 1; // vendor specific + uint8_t cmd_que : 1; + uint8_t : 2; + uint8_t sync : 1; + uint8_t wbus16 : 1; + uint8_t : 2; + + uint8_t vendor_id[8] ; ///< 8 bytes of ASCII data identifying the vendor of the product. + uint8_t product_id[16]; ///< 16 bytes of ASCII data defined by the vendor. + uint8_t product_rev[4]; ///< 4 bytes of ASCII data defined by the vendor. +} scsi_inquiry_resp_t; + +TU_VERIFY_STATIC(sizeof(scsi_inquiry_resp_t) == 36, "size is not correct"); + + +typedef struct TU_ATTR_PACKED +{ + uint8_t response_code : 7; ///< 70h - current errors, Fixed Format 71h - deferred errors, Fixed Format + uint8_t valid : 1; + + uint8_t reserved; + + uint8_t sense_key : 4; + uint8_t : 1; + uint8_t ili : 1; ///< Incorrect length indicator + uint8_t end_of_medium : 1; + uint8_t filemark : 1; + + uint32_t information; + uint8_t add_sense_len; + uint32_t command_specific_info; + uint8_t add_sense_code; + uint8_t add_sense_qualifier; + uint8_t field_replaceable_unit_code; + + uint8_t sense_key_specific[3]; ///< sense key specific valid bit is bit 7 of key[0], aka MSB in Big Endian layout + +} scsi_sense_fixed_resp_t; + +TU_VERIFY_STATIC(sizeof(scsi_sense_fixed_resp_t) == 18, "size is not correct"); + +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code ; ///< SCSI OpCode for \ref SCSI_CMD_MODE_SENSE_6 + + uint8_t : 3; + uint8_t disable_block_descriptor : 1; + uint8_t : 4; + + uint8_t page_code : 6; + uint8_t page_control : 2; + + uint8_t subpage_code; + uint8_t alloc_length; + uint8_t control; +} scsi_mode_sense6_t; + +TU_VERIFY_STATIC( sizeof(scsi_mode_sense6_t) == 6, "size is not correct"); + +// This is only a Mode parameter header(6). +typedef struct TU_ATTR_PACKED +{ + uint8_t data_len; + uint8_t medium_type; + + uint8_t reserved : 7; + bool write_protected : 1; + + uint8_t block_descriptor_len; +} scsi_mode_sense6_resp_t; + +TU_VERIFY_STATIC( sizeof(scsi_mode_sense6_resp_t) == 4, "size is not correct"); + +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code; ///< SCSI OpCode for \ref SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL + uint8_t reserved[3]; + uint8_t prohibit_removal; + uint8_t control; +} scsi_prevent_allow_medium_removal_t; + +TU_VERIFY_STATIC( sizeof(scsi_prevent_allow_medium_removal_t) == 6, "size is not correct"); + +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code; + + uint8_t immded : 1; + uint8_t : 7; + + uint8_t TU_RESERVED; + + uint8_t power_condition_mod : 4; + uint8_t : 4; + + uint8_t start : 1; + uint8_t load_eject : 1; + uint8_t no_flush : 1; + uint8_t : 1; + uint8_t power_condition : 4; + + uint8_t control; +} scsi_start_stop_unit_t; + +TU_VERIFY_STATIC( sizeof(scsi_start_stop_unit_t) == 6, "size is not correct"); + +//--------------------------------------------------------------------+ +// SCSI MMC +//--------------------------------------------------------------------+ +/// SCSI Read Format Capacity: Write Capacity +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code; + uint8_t reserved[6]; + uint16_t alloc_length; + uint8_t control; +} scsi_read_format_capacity_t; + +TU_VERIFY_STATIC( sizeof(scsi_read_format_capacity_t) == 10, "size is not correct"); + +typedef struct TU_ATTR_PACKED{ + uint8_t reserved[3]; + uint8_t list_length; /// must be 8*n, length in bytes of formattable capacity descriptor followed it. + + uint32_t block_num; /// Number of Logical Blocks + uint8_t descriptor_type; // 00: reserved, 01 unformatted media , 10 Formatted media, 11 No media present + + uint8_t reserved2; + uint16_t block_size_u16; + +} scsi_read_format_capacity_data_t; + +TU_VERIFY_STATIC( sizeof(scsi_read_format_capacity_data_t) == 12, "size is not correct"); + +//--------------------------------------------------------------------+ +// SCSI Block Command (SBC-3) +// NOTE: All data in SCSI command are in Big Endian +//--------------------------------------------------------------------+ + +/// SCSI Read Capacity 10 Command: Read Capacity +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code ; ///< SCSI OpCode for \ref SCSI_CMD_READ_CAPACITY_10 + uint8_t reserved1 ; + uint32_t lba ; ///< The first Logical Block Address (LBA) accessed by this command + uint16_t reserved2 ; + uint8_t partial_medium_indicator ; + uint8_t control ; +} scsi_read_capacity10_t; + +TU_VERIFY_STATIC(sizeof(scsi_read_capacity10_t) == 10, "size is not correct"); + +/// SCSI Read Capacity 10 Response Data +typedef struct { + uint32_t last_lba ; ///< The last Logical Block Address of the device + uint32_t block_size ; ///< Block size in bytes +} scsi_read_capacity10_resp_t; + +TU_VERIFY_STATIC(sizeof(scsi_read_capacity10_resp_t) == 8, "size is not correct"); + +/// SCSI Read 10 Command +typedef struct TU_ATTR_PACKED +{ + uint8_t cmd_code ; ///< SCSI OpCode + uint8_t reserved ; // has LUN according to wiki + uint32_t lba ; ///< The first Logical Block Address (LBA) accessed by this command + uint8_t reserved2 ; + uint16_t block_count ; ///< Number of Blocks used by this command + uint8_t control ; +} scsi_read10_t, scsi_write10_t; + +TU_VERIFY_STATIC(sizeof(scsi_read10_t) == 10, "size is not correct"); +TU_VERIFY_STATIC(sizeof(scsi_write10_t) == 10, "size is not correct"); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_MSC_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_device.c new file mode 100644 index 000000000..e09a2b11a --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_device.c @@ -0,0 +1,950 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_MSC) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" +#include "device/dcd.h" // for faking dcd_event_xfer_complete + +#include "msc_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +// Can be selectively disabled to reduce logging when troubleshooting other driver +#define MSC_DEBUG 2 + +enum +{ + MSC_STAGE_CMD = 0, + MSC_STAGE_DATA, + MSC_STAGE_STATUS, + MSC_STAGE_STATUS_SENT, + MSC_STAGE_NEED_RESET, +}; + +typedef struct +{ + // TODO optimize alignment + CFG_TUSB_MEM_ALIGN msc_cbw_t cbw; + CFG_TUSB_MEM_ALIGN msc_csw_t csw; + + uint8_t itf_num; + uint8_t ep_in; + uint8_t ep_out; + + // Bulk Only Transfer (BOT) Protocol + uint8_t stage; + uint32_t total_len; // byte to be transferred, can be smaller than total_bytes in cbw + uint32_t xferred_len; // numbered of bytes transferred so far in the Data Stage + + // Sense Response Data + uint8_t sense_key; + uint8_t add_sense_code; + uint8_t add_sense_qualifier; +}mscd_interface_t; + +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static mscd_interface_t _mscd_itf; +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_EP_BUFSIZE]; + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_t* buffer, uint32_t bufsize); +static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc); + +static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc); +static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint32_t xferred_bytes); + +TU_ATTR_ALWAYS_INLINE static inline bool is_data_in(uint8_t dir) +{ + return tu_bit_test(dir, 7); +} + +static inline bool send_csw(uint8_t rhport, mscd_interface_t* p_msc) +{ + // Data residue is always = host expect - actual transferred + p_msc->csw.data_residue = p_msc->cbw.total_bytes - p_msc->xferred_len; + + p_msc->stage = MSC_STAGE_STATUS_SENT; + return usbd_edpt_xfer(rhport, p_msc->ep_in , (uint8_t*) &p_msc->csw, sizeof(msc_csw_t)); +} + +static inline bool prepare_cbw(uint8_t rhport, mscd_interface_t* p_msc) +{ + p_msc->stage = MSC_STAGE_CMD; + return usbd_edpt_xfer(rhport, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t)); +} + +static void fail_scsi_op(uint8_t rhport, mscd_interface_t* p_msc, uint8_t status) +{ + msc_cbw_t const * p_cbw = &p_msc->cbw; + msc_csw_t * p_csw = &p_msc->csw; + + p_csw->status = status; + p_csw->data_residue = p_msc->cbw.total_bytes - p_msc->xferred_len; + p_msc->stage = MSC_STAGE_STATUS; + + // failed but sense key is not set: default to Illegal Request + if ( p_msc->sense_key == 0 ) tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00); + + // If there is data stage and not yet complete, stall it + if ( p_cbw->total_bytes && p_csw->data_residue ) + { + if ( is_data_in(p_cbw->dir) ) + { + usbd_edpt_stall(rhport, p_msc->ep_in); + } + else + { + usbd_edpt_stall(rhport, p_msc->ep_out); + } + } +} + +static inline uint32_t rdwr10_get_lba(uint8_t const command[]) +{ + // use offsetof to avoid pointer to the odd/unaligned address + uint32_t const lba = tu_unaligned_read32(command + offsetof(scsi_write10_t, lba)); + + // lba is in Big Endian + return tu_ntohl(lba); +} + +static inline uint16_t rdwr10_get_blockcount(msc_cbw_t const* cbw) +{ + uint16_t const block_count = tu_unaligned_read16(cbw->command + offsetof(scsi_write10_t, block_count)); + return tu_ntohs(block_count); +} + +static inline uint16_t rdwr10_get_blocksize(msc_cbw_t const* cbw) +{ + // first extract block count in the command + uint16_t const block_count = rdwr10_get_blockcount(cbw); + + // invalid block count + if (block_count == 0) return 0; + + return (uint16_t) (cbw->total_bytes / block_count); +} + +uint8_t rdwr10_validate_cmd(msc_cbw_t const* cbw) +{ + uint8_t status = MSC_CSW_STATUS_PASSED; + uint16_t const block_count = rdwr10_get_blockcount(cbw); + + if ( cbw->total_bytes == 0 ) + { + if ( block_count ) + { + TU_LOG(MSC_DEBUG, " SCSI case 2 (Hn < Di) or case 3 (Hn < Do) \r\n"); + status = MSC_CSW_STATUS_PHASE_ERROR; + }else + { + // no data transfer, only exist in complaint test suite + } + }else + { + if ( SCSI_CMD_READ_10 == cbw->command[0] && !is_data_in(cbw->dir) ) + { + TU_LOG(MSC_DEBUG, " SCSI case 10 (Ho <> Di)\r\n"); + status = MSC_CSW_STATUS_PHASE_ERROR; + } + else if ( SCSI_CMD_WRITE_10 == cbw->command[0] && is_data_in(cbw->dir) ) + { + TU_LOG(MSC_DEBUG, " SCSI case 8 (Hi <> Do)\r\n"); + status = MSC_CSW_STATUS_PHASE_ERROR; + } + else if ( 0 == block_count ) + { + TU_LOG(MSC_DEBUG, " SCSI case 4 Hi > Dn (READ10) or case 9 Ho > Dn (WRITE10) \r\n"); + status = MSC_CSW_STATUS_FAILED; + } + else if ( cbw->total_bytes / block_count == 0 ) + { + TU_LOG(MSC_DEBUG, " Computed block size = 0. SCSI case 7 Hi < Di (READ10) or case 13 Ho < Do (WRIT10)\r\n"); + status = MSC_CSW_STATUS_PHASE_ERROR; + } + } + + return status; +} + +//--------------------------------------------------------------------+ +// Debug +//--------------------------------------------------------------------+ +#if CFG_TUSB_DEBUG >= 2 + +TU_ATTR_UNUSED static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] = +{ + { .key = SCSI_CMD_TEST_UNIT_READY , .data = "Test Unit Ready" }, + { .key = SCSI_CMD_INQUIRY , .data = "Inquiry" }, + { .key = SCSI_CMD_MODE_SELECT_6 , .data = "Mode_Select 6" }, + { .key = SCSI_CMD_MODE_SENSE_6 , .data = "Mode_Sense 6" }, + { .key = SCSI_CMD_START_STOP_UNIT , .data = "Start Stop Unit" }, + { .key = SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL , .data = "Prevent/Allow Medium Removal" }, + { .key = SCSI_CMD_READ_CAPACITY_10 , .data = "Read Capacity10" }, + { .key = SCSI_CMD_REQUEST_SENSE , .data = "Request Sense" }, + { .key = SCSI_CMD_READ_FORMAT_CAPACITY , .data = "Read Format Capacity" }, + { .key = SCSI_CMD_READ_10 , .data = "Read10" }, + { .key = SCSI_CMD_WRITE_10 , .data = "Write10" } +}; + +TU_ATTR_UNUSED static tu_lookup_table_t const _msc_scsi_cmd_table = +{ + .count = TU_ARRAY_SIZE(_msc_scsi_cmd_lookup), + .items = _msc_scsi_cmd_lookup +}; + +#endif + +//--------------------------------------------------------------------+ +// APPLICATION API +//--------------------------------------------------------------------+ +bool tud_msc_set_sense(uint8_t lun, uint8_t sense_key, uint8_t add_sense_code, uint8_t add_sense_qualifier) +{ + (void) lun; + + _mscd_itf.sense_key = sense_key; + _mscd_itf.add_sense_code = add_sense_code; + _mscd_itf.add_sense_qualifier = add_sense_qualifier; + + return true; +} + +static inline void set_sense_medium_not_present(uint8_t lun) +{ + // default sense is NOT READY, MEDIUM NOT PRESENT + tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x3A, 0x00); +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void mscd_init(void) +{ + tu_memclr(&_mscd_itf, sizeof(mscd_interface_t)); +} + +void mscd_reset(uint8_t rhport) +{ + (void) rhport; + tu_memclr(&_mscd_itf, sizeof(mscd_interface_t)); +} + +uint16_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + // only support SCSI's BOT protocol + TU_VERIFY(TUSB_CLASS_MSC == itf_desc->bInterfaceClass && + MSC_SUBCLASS_SCSI == itf_desc->bInterfaceSubClass && + MSC_PROTOCOL_BOT == itf_desc->bInterfaceProtocol, 0); + + // msc driver length is fixed + uint16_t const drv_len = sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t); + + // Max length must be at least 1 interface + 2 endpoints + TU_ASSERT(max_len >= drv_len, 0); + + mscd_interface_t * p_msc = &_mscd_itf; + p_msc->itf_num = itf_desc->bInterfaceNumber; + + // Open endpoint pair + TU_ASSERT( usbd_open_edpt_pair(rhport, tu_desc_next(itf_desc), 2, TUSB_XFER_BULK, &p_msc->ep_out, &p_msc->ep_in), 0 ); + + // Prepare for Command Block Wrapper + TU_ASSERT( prepare_cbw(rhport, p_msc), drv_len); + + return drv_len; +} + +static void proc_bot_reset(mscd_interface_t* p_msc) +{ + p_msc->stage = MSC_STAGE_CMD; + p_msc->total_len = 0; + p_msc->xferred_len = 0; + + p_msc->sense_key = 0; + p_msc->add_sense_code = 0; + p_msc->add_sense_qualifier = 0; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + // nothing to do with DATA & ACK stage + if (stage != CONTROL_STAGE_SETUP) return true; + + mscd_interface_t* p_msc = &_mscd_itf; + + // Clear Endpoint Feature (stall) for recovery + if ( TUSB_REQ_TYPE_STANDARD == request->bmRequestType_bit.type && + TUSB_REQ_RCPT_ENDPOINT == request->bmRequestType_bit.recipient && + TUSB_REQ_CLEAR_FEATURE == request->bRequest && + TUSB_REQ_FEATURE_EDPT_HALT == request->wValue ) + { + uint8_t const ep_addr = tu_u16_low(request->wIndex); + + if ( p_msc->stage == MSC_STAGE_NEED_RESET ) + { + // reset recovery is required to recover from this stage + // Clear Stall request cannot resolve this -> continue to stall endpoint + usbd_edpt_stall(rhport, ep_addr); + } + else + { + if ( ep_addr == p_msc->ep_in ) + { + if ( p_msc->stage == MSC_STAGE_STATUS ) + { + // resume sending SCSI status if we are in this stage previously before stalled + TU_ASSERT( send_csw(rhport, p_msc) ); + } + } + else if ( ep_addr == p_msc->ep_out ) + { + if ( p_msc->stage == MSC_STAGE_CMD ) + { + // part of reset recovery (probably due to invalid CBW) -> prepare for new command + // Note: skip if already queued previously + if ( usbd_edpt_ready(rhport, p_msc->ep_out) ) + { + TU_ASSERT( prepare_cbw(rhport, p_msc) ); + } + } + } + } + + return true; + } + + // From this point only handle class request only + TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS); + + switch ( request->bRequest ) + { + case MSC_REQ_RESET: + TU_LOG(MSC_DEBUG, " MSC BOT Reset\r\n"); + TU_VERIFY(request->wValue == 0 && request->wLength == 0); + + // driver state reset + proc_bot_reset(p_msc); + + tud_control_status(rhport, request); + break; + + case MSC_REQ_GET_MAX_LUN: + { + TU_LOG(MSC_DEBUG, " MSC Get Max Lun\r\n"); + TU_VERIFY(request->wValue == 0 && request->wLength == 1); + + uint8_t maxlun = 1; + if (tud_msc_get_maxlun_cb) maxlun = tud_msc_get_maxlun_cb(); + TU_VERIFY(maxlun); + + // MAX LUN is minus 1 by specs + maxlun--; + + tud_control_xfer(rhport, request, &maxlun, 1); + } + break; + + default: return false; // stall unsupported request + } + + return true; +} + +bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) +{ + (void) event; + + mscd_interface_t* p_msc = &_mscd_itf; + msc_cbw_t const * p_cbw = &p_msc->cbw; + msc_csw_t * p_csw = &p_msc->csw; + + switch (p_msc->stage) + { + case MSC_STAGE_CMD: + //------------- new CBW received -------------// + // Complete IN while waiting for CMD is usually Status of previous SCSI op, ignore it + if(ep_addr != p_msc->ep_out) return true; + + if ( !(xferred_bytes == sizeof(msc_cbw_t) && p_cbw->signature == MSC_CBW_SIGNATURE) ) + { + TU_LOG(MSC_DEBUG, " SCSI CBW is not valid\r\n"); + + // BOT 6.6.1 If CBW is not valid stall both endpoints until reset recovery + p_msc->stage = MSC_STAGE_NEED_RESET; + + // invalid CBW stall both endpoints + usbd_edpt_stall(rhport, p_msc->ep_in); + usbd_edpt_stall(rhport, p_msc->ep_out); + + return false; + } + + TU_LOG(MSC_DEBUG, " SCSI Command [Lun%u]: %s\r\n", p_cbw->lun, tu_lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0])); + //TU_LOG_MEM(MSC_DEBUG, p_cbw, xferred_bytes, 2); + + p_csw->signature = MSC_CSW_SIGNATURE; + p_csw->tag = p_cbw->tag; + p_csw->data_residue = 0; + p_csw->status = MSC_CSW_STATUS_PASSED; + + /*------------- Parse command and prepare DATA -------------*/ + p_msc->stage = MSC_STAGE_DATA; + p_msc->total_len = p_cbw->total_bytes; + p_msc->xferred_len = 0; + + // Read10 or Write10 + if ( (SCSI_CMD_READ_10 == p_cbw->command[0]) || (SCSI_CMD_WRITE_10 == p_cbw->command[0]) ) + { + uint8_t const status = rdwr10_validate_cmd(p_cbw); + + if ( status != MSC_CSW_STATUS_PASSED) + { + fail_scsi_op(rhport, p_msc, status); + }else if ( p_cbw->total_bytes ) + { + if (SCSI_CMD_READ_10 == p_cbw->command[0]) + { + proc_read10_cmd(rhport, p_msc); + }else + { + proc_write10_cmd(rhport, p_msc); + } + }else + { + // no data transfer, only exist in complaint test suite + p_msc->stage = MSC_STAGE_STATUS; + } + } + else + { + // For other SCSI commands + // 1. OUT : queue transfer (invoke app callback after done) + // 2. IN & Zero: Process if is built-in, else Invoke app callback. Skip DATA if zero length + if ( (p_cbw->total_bytes > 0 ) && !is_data_in(p_cbw->dir) ) + { + if (p_cbw->total_bytes > sizeof(_mscd_buf)) + { + TU_LOG(MSC_DEBUG, " SCSI reject non READ10/WRITE10 with large data\r\n"); + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + }else + { + // Didn't check for case 9 (Ho > Dn), which requires examining scsi command first + // but it is OK to just receive data then responded with failed status + TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_buf, p_msc->total_len) ); + } + }else + { + // First process if it is a built-in commands + int32_t resplen = proc_builtin_scsi(p_cbw->lun, p_cbw->command, _mscd_buf, sizeof(_mscd_buf)); + + // Invoke user callback if not built-in + if ( (resplen < 0) && (p_msc->sense_key == 0) ) + { + resplen = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, p_msc->total_len); + } + + if ( resplen < 0 ) + { + // unsupported command + TU_LOG(MSC_DEBUG, " SCSI unsupported or failed command\r\n"); + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + } + else if (resplen == 0) + { + if (p_cbw->total_bytes) + { + // 6.7 The 13 Cases: case 4 (Hi > Dn) + // TU_LOG(MSC_DEBUG, " SCSI case 4 (Hi > Dn): %lu\r\n", p_cbw->total_bytes); + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + }else + { + // case 1 Hn = Dn: all good + p_msc->stage = MSC_STAGE_STATUS; + } + } + else + { + if ( p_cbw->total_bytes == 0 ) + { + // 6.7 The 13 Cases: case 2 (Hn < Di) + // TU_LOG(MSC_DEBUG, " SCSI case 2 (Hn < Di): %lu\r\n", p_cbw->total_bytes); + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + }else + { + // cannot return more than host expect + p_msc->total_len = tu_min32((uint32_t) resplen, p_cbw->total_bytes); + TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, p_msc->total_len) ); + } + } + } + } + break; + + case MSC_STAGE_DATA: + TU_LOG(MSC_DEBUG, " SCSI Data [Lun%u]\r\n", p_cbw->lun); + //TU_LOG_MEM(MSC_DEBUG, _mscd_buf, xferred_bytes, 2); + + if (SCSI_CMD_READ_10 == p_cbw->command[0]) + { + p_msc->xferred_len += xferred_bytes; + + if ( p_msc->xferred_len >= p_msc->total_len ) + { + // Data Stage is complete + p_msc->stage = MSC_STAGE_STATUS; + }else + { + proc_read10_cmd(rhport, p_msc); + } + } + else if (SCSI_CMD_WRITE_10 == p_cbw->command[0]) + { + proc_write10_new_data(rhport, p_msc, xferred_bytes); + } + else + { + p_msc->xferred_len += xferred_bytes; + + // OUT transfer, invoke callback if needed + if ( !is_data_in(p_cbw->dir) ) + { + int32_t cb_result = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, p_msc->total_len); + + if ( cb_result < 0 ) + { + // unsupported command + TU_LOG(MSC_DEBUG, " SCSI unsupported command\r\n"); + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + }else + { + // TODO haven't implement this scenario any further yet + } + } + + if ( p_msc->xferred_len >= p_msc->total_len ) + { + // Data Stage is complete + p_msc->stage = MSC_STAGE_STATUS; + } + else + { + // This scenario with command that take more than one transfer is already rejected at Command stage + TU_BREAKPOINT(); + } + } + break; + + case MSC_STAGE_STATUS: + // processed immediately after this switch, supposedly to be empty + break; + + case MSC_STAGE_STATUS_SENT: + // Wait for the Status phase to complete + if( (ep_addr == p_msc->ep_in) && (xferred_bytes == sizeof(msc_csw_t)) ) + { + TU_LOG(MSC_DEBUG, " SCSI Status [Lun%u] = %u\r\n", p_cbw->lun, p_csw->status); + // TU_LOG_MEM(MSC_DEBUG, p_csw, xferred_bytes, 2); + + // Invoke complete callback if defined + // Note: There is racing issue with samd51 + qspi flash testing with arduino + // if complete_cb() is invoked after queuing the status. + switch(p_cbw->command[0]) + { + case SCSI_CMD_READ_10: + if ( tud_msc_read10_complete_cb ) tud_msc_read10_complete_cb(p_cbw->lun); + break; + + case SCSI_CMD_WRITE_10: + if ( tud_msc_write10_complete_cb ) tud_msc_write10_complete_cb(p_cbw->lun); + break; + + default: + if ( tud_msc_scsi_complete_cb ) tud_msc_scsi_complete_cb(p_cbw->lun, p_cbw->command); + break; + } + + TU_ASSERT( prepare_cbw(rhport, p_msc) ); + }else + { + // Any xfer ended here is consider unknown error, ignore it + TU_LOG1(" Warning expect SCSI Status but received unknown data\r\n"); + } + break; + + default : break; + } + + if ( p_msc->stage == MSC_STAGE_STATUS ) + { + // skip status if epin is currently stalled, will do it when received Clear Stall request + if ( !usbd_edpt_stalled(rhport, p_msc->ep_in) ) + { + if ( (p_cbw->total_bytes > p_msc->xferred_len) && is_data_in(p_cbw->dir) ) + { + // 6.7 The 13 Cases: case 5 (Hi > Di): STALL before status + // TU_LOG(MSC_DEBUG, " SCSI case 5 (Hi > Di): %lu > %lu\r\n", p_cbw->total_bytes, p_msc->xferred_len); + usbd_edpt_stall(rhport, p_msc->ep_in); + }else + { + TU_ASSERT( send_csw(rhport, p_msc) ); + } + } + + #if TU_CHECK_MCU(OPT_MCU_CXD56) + // WORKAROUND: cxd56 has its own nuttx usb stack which does not forward Set/ClearFeature(Endpoint) to DCD. + // There is no way for us to know when EP is un-stall, therefore we will unconditionally un-stall here and + // hope everything will work + if ( usbd_edpt_stalled(rhport, p_msc->ep_in) ) + { + usbd_edpt_clear_stall(rhport, p_msc->ep_in); + send_csw(rhport, p_msc); + } + #endif + } + + return true; +} + +/*------------------------------------------------------------------*/ +/* SCSI Command Process + *------------------------------------------------------------------*/ + +// return response's length (copied to buffer). Negative if it is not an built-in command or indicate Failed status (CSW) +// In case of a failed status, sense key must be set for reason of failure +static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_t* buffer, uint32_t bufsize) +{ + (void) bufsize; // TODO refractor later + int32_t resplen; + + mscd_interface_t* p_msc = &_mscd_itf; + + switch ( scsi_cmd[0] ) + { + case SCSI_CMD_TEST_UNIT_READY: + resplen = 0; + if ( !tud_msc_test_unit_ready_cb(lun) ) + { + // Failed status response + resplen = - 1; + + // set default sense if not set by callback + if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun); + } + break; + + case SCSI_CMD_START_STOP_UNIT: + resplen = 0; + + if (tud_msc_start_stop_cb) + { + scsi_start_stop_unit_t const * start_stop = (scsi_start_stop_unit_t const *) scsi_cmd; + if ( !tud_msc_start_stop_cb(lun, start_stop->power_condition, start_stop->start, start_stop->load_eject) ) + { + // Failed status response + resplen = - 1; + + // set default sense if not set by callback + if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun); + } + } + break; + + case SCSI_CMD_READ_CAPACITY_10: + { + uint32_t block_count; + uint32_t block_size; + uint16_t block_size_u16; + + tud_msc_capacity_cb(lun, &block_count, &block_size_u16); + block_size = (uint32_t) block_size_u16; + + // Invalid block size/count from callback, possibly unit is not ready + // stall this request, set sense key to NOT READY + if (block_count == 0 || block_size == 0) + { + resplen = -1; + + // set default sense if not set by callback + if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun); + }else + { + scsi_read_capacity10_resp_t read_capa10; + + read_capa10.last_lba = tu_htonl(block_count-1); + read_capa10.block_size = tu_htonl(block_size); + + resplen = sizeof(read_capa10); + memcpy(buffer, &read_capa10, resplen); + } + } + break; + + case SCSI_CMD_READ_FORMAT_CAPACITY: + { + scsi_read_format_capacity_data_t read_fmt_capa = + { + .list_length = 8, + .block_num = 0, + .descriptor_type = 2, // formatted media + .block_size_u16 = 0 + }; + + uint32_t block_count; + uint16_t block_size; + + tud_msc_capacity_cb(lun, &block_count, &block_size); + + // Invalid block size/count from callback, possibly unit is not ready + // stall this request, set sense key to NOT READY + if (block_count == 0 || block_size == 0) + { + resplen = -1; + + // set default sense if not set by callback + if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun); + }else + { + read_fmt_capa.block_num = tu_htonl(block_count); + read_fmt_capa.block_size_u16 = tu_htons(block_size); + + resplen = sizeof(read_fmt_capa); + memcpy(buffer, &read_fmt_capa, resplen); + } + } + break; + + case SCSI_CMD_INQUIRY: + { + scsi_inquiry_resp_t inquiry_rsp = + { + .is_removable = 1, + .version = 2, + .response_data_format = 2, + }; + + // vendor_id, product_id, product_rev is space padded string + memset(inquiry_rsp.vendor_id , ' ', sizeof(inquiry_rsp.vendor_id)); + memset(inquiry_rsp.product_id , ' ', sizeof(inquiry_rsp.product_id)); + memset(inquiry_rsp.product_rev, ' ', sizeof(inquiry_rsp.product_rev)); + + tud_msc_inquiry_cb(lun, inquiry_rsp.vendor_id, inquiry_rsp.product_id, inquiry_rsp.product_rev); + + resplen = sizeof(inquiry_rsp); + memcpy(buffer, &inquiry_rsp, resplen); + } + break; + + case SCSI_CMD_MODE_SENSE_6: + { + scsi_mode_sense6_resp_t mode_resp = + { + .data_len = 3, + .medium_type = 0, + .write_protected = false, + .reserved = 0, + .block_descriptor_len = 0 // no block descriptor are included + }; + + bool writable = true; + if ( tud_msc_is_writable_cb ) + { + writable = tud_msc_is_writable_cb(lun); + } + + mode_resp.write_protected = !writable; + + resplen = sizeof(mode_resp); + memcpy(buffer, &mode_resp, resplen); + } + break; + + case SCSI_CMD_REQUEST_SENSE: + { + scsi_sense_fixed_resp_t sense_rsp = + { + .response_code = 0x70, // current, fixed format + .valid = 1 + }; + + sense_rsp.add_sense_len = sizeof(scsi_sense_fixed_resp_t) - 8; + sense_rsp.sense_key = p_msc->sense_key; + sense_rsp.add_sense_code = p_msc->add_sense_code; + sense_rsp.add_sense_qualifier = p_msc->add_sense_qualifier; + + resplen = sizeof(sense_rsp); + memcpy(buffer, &sense_rsp, resplen); + + // request sense callback could overwrite the sense data + if (tud_msc_request_sense_cb) + { + resplen = tud_msc_request_sense_cb(lun, buffer, bufsize); + } + + // Clear sense data after copy + tud_msc_set_sense(lun, 0, 0, 0); + } + break; + + default: resplen = -1; break; + } + + return resplen; +} + +static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc) +{ + msc_cbw_t const * p_cbw = &p_msc->cbw; + + // block size already verified not zero + uint16_t const block_sz = rdwr10_get_blocksize(p_cbw); + + // Adjust lba with transferred bytes + uint32_t const lba = rdwr10_get_lba(p_cbw->command) + (p_msc->xferred_len / block_sz); + + // remaining bytes capped at class buffer + int32_t nbytes = (int32_t) tu_min32(sizeof(_mscd_buf), p_cbw->total_bytes-p_msc->xferred_len); + + // Application can consume smaller bytes + uint32_t const offset = p_msc->xferred_len % block_sz; + nbytes = tud_msc_read10_cb(p_cbw->lun, lba, offset, _mscd_buf, (uint32_t) nbytes); + + if ( nbytes < 0 ) + { + // negative means error -> endpoint is stalled & status in CSW set to failed + TU_LOG(MSC_DEBUG, " tud_msc_read10_cb() return -1\r\n"); + + // set sense + set_sense_medium_not_present(p_cbw->lun); + + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + } + else if ( nbytes == 0 ) + { + // zero means not ready -> simulate an transfer complete so that this driver callback will fired again + dcd_event_xfer_complete(rhport, p_msc->ep_in, 0, XFER_RESULT_SUCCESS, false); + } + else + { + TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, nbytes), ); + } +} + +static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc) +{ + msc_cbw_t const * p_cbw = &p_msc->cbw; + bool writable = true; + + if ( tud_msc_is_writable_cb ) + { + writable = tud_msc_is_writable_cb(p_cbw->lun); + } + + if ( !writable ) + { + // Not writable, complete this SCSI op with error + // Sense = Write protected + tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_DATA_PROTECT, 0x27, 0x00); + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + return; + } + + // remaining bytes capped at class buffer + int32_t nbytes = (int32_t) tu_min32(sizeof(_mscd_buf), p_cbw->total_bytes-p_msc->xferred_len); + + // Write10 callback will be called later when usb transfer complete + TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_buf, nbytes), ); +} + +// process new data arrived from WRITE10 +static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint32_t xferred_bytes) +{ + msc_cbw_t const * p_cbw = &p_msc->cbw; + + // block size already verified not zero + uint16_t const block_sz = rdwr10_get_blocksize(p_cbw); + + // Adjust lba with transferred bytes + uint32_t const lba = rdwr10_get_lba(p_cbw->command) + (p_msc->xferred_len / block_sz); + + // Invoke callback to consume new data + uint32_t const offset = p_msc->xferred_len % block_sz; + int32_t nbytes = tud_msc_write10_cb(p_cbw->lun, lba, offset, _mscd_buf, xferred_bytes); + + if ( nbytes < 0 ) + { + // negative means error -> failed this scsi op + TU_LOG(MSC_DEBUG, " tud_msc_write10_cb() return -1\r\n"); + + // update actual byte before failed + p_msc->xferred_len += xferred_bytes; + + // Set sense + set_sense_medium_not_present(p_cbw->lun); + + fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED); + }else + { + // Application consume less than what we got (including zero) + if ( (uint32_t) nbytes < xferred_bytes ) + { + if ( nbytes > 0 ) + { + p_msc->xferred_len += nbytes; + memmove(_mscd_buf, _mscd_buf+nbytes, xferred_bytes-nbytes); + } + + // simulate an transfer complete with adjusted parameters --> callback will be invoked with adjusted parameter + dcd_event_xfer_complete(rhport, p_msc->ep_out, xferred_bytes-nbytes, XFER_RESULT_SUCCESS, false); + } + else + { + // Application consume all bytes in our buffer + p_msc->xferred_len += xferred_bytes; + + if ( p_msc->xferred_len >= p_msc->total_len ) + { + // Data Stage is complete + p_msc->stage = MSC_STAGE_STATUS; + }else + { + // prepare to receive more data from host + proc_write10_cmd(rhport, p_msc); + } + } + } +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_device.h new file mode 100644 index 000000000..5839b168d --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_device.h @@ -0,0 +1,162 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_MSC_DEVICE_H_ +#define _TUSB_MSC_DEVICE_H_ + +#include "common/tusb_common.h" +#include "msc.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Driver Configuration +//--------------------------------------------------------------------+ + +#if !defined(CFG_TUD_MSC_EP_BUFSIZE) & defined(CFG_TUD_MSC_BUFSIZE) + // TODO warn user to use new name later on + // #warning CFG_TUD_MSC_BUFSIZE is renamed to CFG_TUD_MSC_EP_BUFSIZE, please update to use the new name + #define CFG_TUD_MSC_EP_BUFSIZE CFG_TUD_MSC_BUFSIZE +#endif + +#ifndef CFG_TUD_MSC_EP_BUFSIZE + #error CFG_TUD_MSC_EP_BUFSIZE must be defined, value of a block size should work well, the more the better +#endif + +TU_VERIFY_STATIC(CFG_TUD_MSC_EP_BUFSIZE < UINT16_MAX, "Size is not correct"); + +//--------------------------------------------------------------------+ +// Application API +//--------------------------------------------------------------------+ + +// Set SCSI sense response +bool tud_msc_set_sense(uint8_t lun, uint8_t sense_key, uint8_t add_sense_code, uint8_t add_sense_qualifier); + +//--------------------------------------------------------------------+ +// Application Callbacks (WEAK is optional) +//--------------------------------------------------------------------+ + +// Invoked when received SCSI READ10 command +// - Address = lba * BLOCK_SIZE + offset +// - offset is only needed if CFG_TUD_MSC_EP_BUFSIZE is smaller than BLOCK_SIZE. +// +// - Application fill the buffer (up to bufsize) with address contents and return number of read byte. If +// - read < bufsize : These bytes are transferred first and callback invoked again for remaining data. +// +// - read == 0 : Indicate application is not ready yet e.g disk I/O busy. +// Callback invoked again with the same parameters later on. +// +// - read < 0 : Indicate application error e.g invalid address. This request will be STALLed +// and return failed status in command status wrapper phase. +int32_t tud_msc_read10_cb (uint8_t lun, uint32_t lba, uint32_t offset, void* buffer, uint32_t bufsize); + +// Invoked when received SCSI WRITE10 command +// - Address = lba * BLOCK_SIZE + offset +// - offset is only needed if CFG_TUD_MSC_EP_BUFSIZE is smaller than BLOCK_SIZE. +// +// - Application write data from buffer to address contents (up to bufsize) and return number of written byte. If +// - write < bufsize : callback invoked again with remaining data later on. +// +// - write == 0 : Indicate application is not ready yet e.g disk I/O busy. +// Callback invoked again with the same parameters later on. +// +// - write < 0 : Indicate application error e.g invalid address. This request will be STALLed +// and return failed status in command status wrapper phase. +// +// TODO change buffer to const uint8_t* +int32_t tud_msc_write10_cb (uint8_t lun, uint32_t lba, uint32_t offset, uint8_t* buffer, uint32_t bufsize); + +// Invoked when received SCSI_CMD_INQUIRY +// Application fill vendor id, product id and revision with string up to 8, 16, 4 characters respectively +void tud_msc_inquiry_cb(uint8_t lun, uint8_t vendor_id[8], uint8_t product_id[16], uint8_t product_rev[4]); + +// Invoked when received Test Unit Ready command. +// return true allowing host to read/write this LUN e.g SD card inserted +bool tud_msc_test_unit_ready_cb(uint8_t lun); + +// Invoked when received SCSI_CMD_READ_CAPACITY_10 and SCSI_CMD_READ_FORMAT_CAPACITY to determine the disk size +// Application update block count and block size +void tud_msc_capacity_cb(uint8_t lun, uint32_t* block_count, uint16_t* block_size); + +/** + * Invoked when received an SCSI command not in built-in list below. + * - READ_CAPACITY10, READ_FORMAT_CAPACITY, INQUIRY, TEST_UNIT_READY, START_STOP_UNIT, MODE_SENSE6, REQUEST_SENSE + * - READ10 and WRITE10 has their own callbacks + * + * \param[in] lun Logical unit number + * \param[in] scsi_cmd SCSI command contents which application must examine to response accordingly + * \param[out] buffer Buffer for SCSI Data Stage. + * - For INPUT: application must fill this with response. + * - For OUTPUT it holds the Data from host + * \param[in] bufsize Buffer's length. + * + * \return Actual bytes processed, can be zero for no-data command. + * \retval negative Indicate error e.g unsupported command, tinyusb will \b STALL the corresponding + * endpoint and return failed status in command status wrapper phase. + */ +int32_t tud_msc_scsi_cb (uint8_t lun, uint8_t const scsi_cmd[16], void* buffer, uint16_t bufsize); + +/*------------- Optional callbacks -------------*/ + +// Invoked when received GET_MAX_LUN request, required for multiple LUNs implementation +TU_ATTR_WEAK uint8_t tud_msc_get_maxlun_cb(void); + +// Invoked when received Start Stop Unit command +// - Start = 0 : stopped power mode, if load_eject = 1 : unload disk storage +// - Start = 1 : active mode, if load_eject = 1 : load disk storage +TU_ATTR_WEAK bool tud_msc_start_stop_cb(uint8_t lun, uint8_t power_condition, bool start, bool load_eject); + +// Invoked when received REQUEST_SENSE +TU_ATTR_WEAK int32_t tud_msc_request_sense_cb(uint8_t lun, void* buffer, uint16_t bufsize); + +// Invoked when Read10 command is complete +TU_ATTR_WEAK void tud_msc_read10_complete_cb(uint8_t lun); + +// Invoke when Write10 command is complete, can be used to flush flash caching +TU_ATTR_WEAK void tud_msc_write10_complete_cb(uint8_t lun); + +// Invoked when command in tud_msc_scsi_cb is complete +TU_ATTR_WEAK void tud_msc_scsi_complete_cb(uint8_t lun, uint8_t const scsi_cmd[16]); + +// Invoked to check if device is writable as part of SCSI WRITE10 +TU_ATTR_WEAK bool tud_msc_is_writable_cb(uint8_t lun); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void mscd_init (void); +void mscd_reset (uint8_t rhport); +uint16_t mscd_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool mscd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * p_request); +bool mscd_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_MSC_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_host.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_host.c new file mode 100644 index 000000000..fa6519956 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_host.c @@ -0,0 +1,491 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if TUSB_OPT_HOST_ENABLED & CFG_TUH_MSC + +#include "host/usbh.h" +#include "host/usbh_classdriver.h" + +#include "msc_host.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +enum +{ + MSC_STAGE_IDLE = 0, + MSC_STAGE_CMD, + MSC_STAGE_DATA, + MSC_STAGE_STATUS, +}; + +typedef struct +{ + uint8_t itf_num; + uint8_t ep_in; + uint8_t ep_out; + + uint8_t max_lun; + + volatile bool configured; // Receive SET_CONFIGURE + volatile bool mounted; // Enumeration is complete + + struct { + uint32_t block_size; + uint32_t block_count; + } capacity[CFG_TUH_MSC_MAXLUN]; + + //------------- SCSI -------------// + uint8_t stage; + void* buffer; + tuh_msc_complete_cb_t complete_cb; + + msc_cbw_t cbw; + msc_csw_t csw; +}msch_interface_t; + +CFG_TUSB_MEM_SECTION static msch_interface_t _msch_itf[CFG_TUH_DEVICE_MAX]; + +// buffer used to read scsi information when mounted +// largest response data currently is inquiry TODO Inquiry is not part of enum anymore +CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4) +static uint8_t _msch_buffer[sizeof(scsi_inquiry_resp_t)]; + +TU_ATTR_ALWAYS_INLINE +static inline msch_interface_t* get_itf(uint8_t dev_addr) +{ + return &_msch_itf[dev_addr-1]; +} + +//--------------------------------------------------------------------+ +// PUBLIC API +//--------------------------------------------------------------------+ +uint8_t tuh_msc_get_maxlun(uint8_t dev_addr) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + return p_msc->max_lun; +} + +uint32_t tuh_msc_get_block_count(uint8_t dev_addr, uint8_t lun) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + return p_msc->capacity[lun].block_count; +} + +uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + return p_msc->capacity[lun].block_size; +} + +bool tuh_msc_mounted(uint8_t dev_addr) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + return p_msc->mounted; +} + +bool tuh_msc_ready(uint8_t dev_addr) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + return p_msc->mounted && !usbh_edpt_busy(dev_addr, p_msc->ep_in); +} + +//--------------------------------------------------------------------+ +// PUBLIC API: SCSI COMMAND +//--------------------------------------------------------------------+ +static inline void cbw_init(msc_cbw_t *cbw, uint8_t lun) +{ + tu_memclr(cbw, sizeof(msc_cbw_t)); + cbw->signature = MSC_CBW_SIGNATURE; + cbw->tag = 0x54555342; // TUSB + cbw->lun = lun; +} + +bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + TU_VERIFY(p_msc->configured); + + // TODO claim endpoint + + p_msc->cbw = *cbw; + p_msc->stage = MSC_STAGE_CMD; + p_msc->buffer = data; + p_msc->complete_cb = complete_cb; + + TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t))); + + return true; +} + +bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + TU_VERIFY(p_msc->configured); + + msc_cbw_t cbw; + cbw_init(&cbw, lun); + + cbw.total_bytes = sizeof(scsi_read_capacity10_resp_t); + cbw.dir = TUSB_DIR_IN_MASK; + cbw.cmd_len = sizeof(scsi_read_capacity10_t); + cbw.command[0] = SCSI_CMD_READ_CAPACITY_10; + + return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb); +} + +bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + TU_VERIFY(p_msc->mounted); + + msc_cbw_t cbw; + cbw_init(&cbw, lun); + + cbw.total_bytes = sizeof(scsi_inquiry_resp_t); + cbw.dir = TUSB_DIR_IN_MASK; + cbw.cmd_len = sizeof(scsi_inquiry_t); + + scsi_inquiry_t const cmd_inquiry = + { + .cmd_code = SCSI_CMD_INQUIRY, + .alloc_length = sizeof(scsi_inquiry_resp_t) + }; + memcpy(cbw.command, &cmd_inquiry, cbw.cmd_len); + + return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb); +} + +bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + TU_VERIFY(p_msc->configured); + + msc_cbw_t cbw; + cbw_init(&cbw, lun); + + cbw.total_bytes = 0; + cbw.dir = TUSB_DIR_OUT; + cbw.cmd_len = sizeof(scsi_test_unit_ready_t); + cbw.command[0] = SCSI_CMD_TEST_UNIT_READY; + cbw.command[1] = lun; // according to wiki TODO need verification + + return tuh_msc_scsi_command(dev_addr, &cbw, NULL, complete_cb); +} + +bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb) +{ + msc_cbw_t cbw; + cbw_init(&cbw, lun); + + cbw.total_bytes = 18; // TODO sense response + cbw.dir = TUSB_DIR_IN_MASK; + cbw.cmd_len = sizeof(scsi_request_sense_t); + + scsi_request_sense_t const cmd_request_sense = + { + .cmd_code = SCSI_CMD_REQUEST_SENSE, + .alloc_length = 18 + }; + + memcpy(cbw.command, &cmd_request_sense, cbw.cmd_len); + + return tuh_msc_scsi_command(dev_addr, &cbw, resposne, complete_cb); +} + +bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + TU_VERIFY(p_msc->mounted); + + msc_cbw_t cbw; + cbw_init(&cbw, lun); + + cbw.total_bytes = block_count*p_msc->capacity[lun].block_size; + cbw.dir = TUSB_DIR_IN_MASK; + cbw.cmd_len = sizeof(scsi_read10_t); + + scsi_read10_t const cmd_read10 = + { + .cmd_code = SCSI_CMD_READ_10, + .lba = tu_htonl(lba), + .block_count = tu_htons(block_count) + }; + + memcpy(cbw.command, &cmd_read10, cbw.cmd_len); + + return tuh_msc_scsi_command(dev_addr, &cbw, buffer, complete_cb); +} + +bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + TU_VERIFY(p_msc->mounted); + + msc_cbw_t cbw; + cbw_init(&cbw, lun); + + cbw.total_bytes = block_count*p_msc->capacity[lun].block_size; + cbw.dir = TUSB_DIR_OUT; + cbw.cmd_len = sizeof(scsi_write10_t); + + scsi_write10_t const cmd_write10 = + { + .cmd_code = SCSI_CMD_WRITE_10, + .lba = tu_htonl(lba), + .block_count = tu_htons(block_count) + }; + + memcpy(cbw.command, &cmd_write10, cbw.cmd_len); + + return tuh_msc_scsi_command(dev_addr, &cbw, (void*)(uintptr_t) buffer, complete_cb); +} + +#if 0 +// MSC interface Reset (not used now) +bool tuh_msc_reset(uint8_t dev_addr) +{ + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = MSC_REQ_RESET, + .wValue = 0, + .wIndex = p_msc->itf_num, + .wLength = 0 + }; + TU_ASSERT( usbh_control_xfer( dev_addr, &new_request, NULL ) ); +} +#endif + +//--------------------------------------------------------------------+ +// CLASS-USBH API +//--------------------------------------------------------------------+ +void msch_init(void) +{ + tu_memclr(_msch_itf, sizeof(_msch_itf)); +} + +void msch_close(uint8_t dev_addr) +{ + TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, ); + + msch_interface_t* p_msc = get_itf(dev_addr); + + // invoke Application Callback + if (p_msc->mounted && tuh_msc_umount_cb) tuh_msc_umount_cb(dev_addr); + + tu_memclr(p_msc, sizeof(msch_interface_t)); +} + +bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + msc_cbw_t const * cbw = &p_msc->cbw; + msc_csw_t * csw = &p_msc->csw; + + switch (p_msc->stage) + { + case MSC_STAGE_CMD: + // Must be Command Block + TU_ASSERT(ep_addr == p_msc->ep_out && event == XFER_RESULT_SUCCESS && xferred_bytes == sizeof(msc_cbw_t)); + + if ( cbw->total_bytes && p_msc->buffer ) + { + // Data stage if any + p_msc->stage = MSC_STAGE_DATA; + + uint8_t const ep_data = (cbw->dir & TUSB_DIR_IN_MASK) ? p_msc->ep_in : p_msc->ep_out; + TU_ASSERT(usbh_edpt_xfer(dev_addr, ep_data, p_msc->buffer, cbw->total_bytes)); + }else + { + // Status stage + p_msc->stage = MSC_STAGE_STATUS; + TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, sizeof(msc_csw_t))); + } + break; + + case MSC_STAGE_DATA: + // Status stage + p_msc->stage = MSC_STAGE_STATUS; + TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, sizeof(msc_csw_t))); + break; + + case MSC_STAGE_STATUS: + // SCSI op is complete + p_msc->stage = MSC_STAGE_IDLE; + + if (p_msc->complete_cb) p_msc->complete_cb(dev_addr, cbw, csw); + break; + + // unknown state + default: break; + } + + return true; +} + +//--------------------------------------------------------------------+ +// MSC Enumeration +//--------------------------------------------------------------------+ + +static bool config_get_maxlun_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw); +static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw); +static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw); + +bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len) +{ + TU_VERIFY (MSC_SUBCLASS_SCSI == desc_itf->bInterfaceSubClass && + MSC_PROTOCOL_BOT == desc_itf->bInterfaceProtocol); + + // msc driver length is fixed + uint16_t const drv_len = sizeof(tusb_desc_interface_t) + desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t); + TU_ASSERT(drv_len <= max_len); + + msch_interface_t* p_msc = get_itf(dev_addr); + tusb_desc_endpoint_t const * ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(desc_itf); + + for(uint32_t i=0; i<2; i++) + { + TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType && TUSB_XFER_BULK == ep_desc->bmAttributes.xfer); + TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc)); + + if ( tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN ) + { + p_msc->ep_in = ep_desc->bEndpointAddress; + }else + { + p_msc->ep_out = ep_desc->bEndpointAddress; + } + + ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(ep_desc); + } + + p_msc->itf_num = desc_itf->bInterfaceNumber; + + return true; +} + +bool msch_set_config(uint8_t dev_addr, uint8_t itf_num) +{ + msch_interface_t* p_msc = get_itf(dev_addr); + TU_ASSERT(p_msc->itf_num == itf_num); + + p_msc->configured = true; + + //------------- Get Max Lun -------------// + TU_LOG2("MSC Get Max Lun\r\n"); + tusb_control_request_t request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_IN + }, + .bRequest = MSC_REQ_GET_MAX_LUN, + .wValue = 0, + .wIndex = itf_num, + .wLength = 1 + }; + TU_ASSERT(tuh_control_xfer(dev_addr, &request, &p_msc->max_lun, config_get_maxlun_complete)); + + return true; +} + +static bool config_get_maxlun_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) request; + + msch_interface_t* p_msc = get_itf(dev_addr); + + // STALL means zero + p_msc->max_lun = (XFER_RESULT_SUCCESS == result) ? _msch_buffer[0] : 0; + p_msc->max_lun++; // MAX LUN is minus 1 by specs + + // TODO multiple LUN support + TU_LOG2("SCSI Test Unit Ready\r\n"); + uint8_t const lun = 0; + tuh_msc_test_unit_ready(dev_addr, lun, config_test_unit_ready_complete); + + return true; +} + +static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw) +{ + if (csw->status == 0) + { + // Unit is ready, read its capacity + TU_LOG2("SCSI Read Capacity\r\n"); + tuh_msc_read_capacity(dev_addr, cbw->lun, (scsi_read_capacity10_resp_t*) ((void*) _msch_buffer), config_read_capacity_complete); + }else + { + // Note: During enumeration, some device fails Test Unit Ready and require a few retries + // with Request Sense to start working !! + // TODO limit number of retries + TU_LOG2("SCSI Request Sense\r\n"); + TU_ASSERT(tuh_msc_request_sense(dev_addr, cbw->lun, _msch_buffer, config_request_sense_complete)); + } + + return true; +} + +static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw) +{ + TU_ASSERT(csw->status == 0); + TU_ASSERT(tuh_msc_test_unit_ready(dev_addr, cbw->lun, config_test_unit_ready_complete)); + return true; +} + +static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw) +{ + TU_ASSERT(csw->status == 0); + + msch_interface_t* p_msc = get_itf(dev_addr); + + // Capacity response field: Block size and Last LBA are both Big-Endian + scsi_read_capacity10_resp_t* resp = (scsi_read_capacity10_resp_t*) ((void*) _msch_buffer); + p_msc->capacity[cbw->lun].block_count = tu_ntohl(resp->last_lba) + 1; + p_msc->capacity[cbw->lun].block_size = tu_ntohl(resp->block_size); + + // Mark enumeration is complete + p_msc->mounted = true; + if (tuh_msc_mount_cb) tuh_msc_mount_cb(dev_addr); + + // notify usbh that driver enumeration is complete + usbh_driver_set_config_complete(dev_addr, p_msc->itf_num); + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_host.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_host.h new file mode 100644 index 000000000..7718ad4fe --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/msc/msc_host.h @@ -0,0 +1,119 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_MSC_HOST_H_ +#define _TUSB_MSC_HOST_H_ + +#include "msc.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Driver Configuration +//--------------------------------------------------------------------+ + +#ifndef CFG_TUH_MSC_MAXLUN +#define CFG_TUH_MSC_MAXLUN 4 +#endif + +typedef bool (*tuh_msc_complete_cb_t)(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw); + +//--------------------------------------------------------------------+ +// Application API +//--------------------------------------------------------------------+ + +// Check if device supports MassStorage interface. +// This function true after tuh_msc_mounted_cb() and false after tuh_msc_unmounted_cb() +bool tuh_msc_mounted(uint8_t dev_addr); + +// Check if the interface is currently ready or busy transferring data +bool tuh_msc_ready(uint8_t dev_addr); + +// Get Max Lun +uint8_t tuh_msc_get_maxlun(uint8_t dev_addr); + +// Get number of block +uint32_t tuh_msc_get_block_count(uint8_t dev_addr, uint8_t lun); + +// Get block size in bytes +uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun); + +// Perform a full SCSI command (cbw, data, csw) in non-blocking manner. +// Complete callback is invoked when SCSI op is complete. +// return true if success, false if there is already pending operation. +bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb); + +// Perform SCSI Inquiry command +// Complete callback is invoked when SCSI op is complete. +bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb); + +// Perform SCSI Test Unit Ready command +// Complete callback is invoked when SCSI op is complete. +bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb); + +// Perform SCSI Request Sense 10 command +// Complete callback is invoked when SCSI op is complete. +bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb); + +// Perform SCSI Read 10 command. Read n blocks starting from LBA to buffer +// Complete callback is invoked when SCSI op is complete. +bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb); + +// Perform SCSI Write 10 command. Write n blocks starting from LBA to device +// Complete callback is invoked when SCSI op is complete. +bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb); + +// Perform SCSI Read Capacity 10 command +// Complete callback is invoked when SCSI op is complete. +// Note: during enumeration, host stack already carried out this request. Application can retrieve capacity by +// simply call tuh_msc_get_block_count() and tuh_msc_get_block_size() +bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb); + +//------------- Application Callback -------------// + +// Invoked when a device with MassStorage interface is mounted +TU_ATTR_WEAK void tuh_msc_mount_cb(uint8_t dev_addr); + +// Invoked when a device with MassStorage interface is unmounted +TU_ATTR_WEAK void tuh_msc_umount_cb(uint8_t dev_addr); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ + +void msch_init (void); +bool msch_open (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len); +bool msch_set_config (uint8_t dev_addr, uint8_t itf_num); +void msch_close (uint8_t dev_addr); +bool msch_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_MSC_HOST_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ecm_rndis_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ecm_rndis_device.c new file mode 100644 index 000000000..c6cd388e3 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ecm_rndis_device.c @@ -0,0 +1,445 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Peter Lawrence + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if ( TUSB_OPT_DEVICE_ENABLED && CFG_TUD_ECM_RNDIS ) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "net_device.h" +#include "rndis_protocol.h" + +void rndis_class_set_handler(uint8_t *data, int size); /* found in ./misc/networking/rndis_reports.c */ + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef struct +{ + uint8_t itf_num; // Index number of Management Interface, +1 for Data Interface + uint8_t itf_data_alt; // Alternate setting of Data Interface. 0 : inactive, 1 : active + + uint8_t ep_notif; + uint8_t ep_in; + uint8_t ep_out; + + bool ecm_mode; + + // Endpoint descriptor use to open/close when receving SetInterface + // TODO since configuration descriptor may not be long-lived memory, we should + // keep a copy of endpoint attribute instead + uint8_t const * ecm_desc_epdata; + +} netd_interface_t; + +#define CFG_TUD_NET_PACKET_PREFIX_LEN sizeof(rndis_data_packet_t) +#define CFG_TUD_NET_PACKET_SUFFIX_LEN 0 + +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t received[CFG_TUD_NET_PACKET_PREFIX_LEN + CFG_TUD_NET_MTU + CFG_TUD_NET_PACKET_PREFIX_LEN]; +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t transmitted[CFG_TUD_NET_PACKET_PREFIX_LEN + CFG_TUD_NET_MTU + CFG_TUD_NET_PACKET_PREFIX_LEN]; + +struct ecm_notify_struct +{ + tusb_control_request_t header; + uint32_t downlink, uplink; +}; + +static const struct ecm_notify_struct ecm_notify_nc = +{ + .header = { + .bmRequestType = 0xA1, + .bRequest = 0 /* NETWORK_CONNECTION aka NetworkConnection */, + .wValue = 1 /* Connected */, + .wLength = 0, + }, +}; + +static const struct ecm_notify_struct ecm_notify_csc = +{ + .header = { + .bmRequestType = 0xA1, + .bRequest = 0x2A /* CONNECTION_SPEED_CHANGE aka ConnectionSpeedChange */, + .wLength = 8, + }, + .downlink = 9728000, + .uplink = 9728000, +}; + +// TODO remove CFG_TUSB_MEM_SECTION, control internal buffer is already in this special section +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static union +{ + uint8_t rndis_buf[120]; + struct ecm_notify_struct ecm_buf; +} notify; + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +// TODO remove CFG_TUSB_MEM_SECTION +CFG_TUSB_MEM_SECTION static netd_interface_t _netd_itf; + +static bool can_xmit; + +void tud_network_recv_renew(void) +{ + usbd_edpt_xfer(TUD_OPT_RHPORT, _netd_itf.ep_out, received, sizeof(received)); +} + +static void do_in_xfer(uint8_t *buf, uint16_t len) +{ + can_xmit = false; + usbd_edpt_xfer(TUD_OPT_RHPORT, _netd_itf.ep_in, buf, len); +} + +void netd_report(uint8_t *buf, uint16_t len) +{ + // skip if previous report not yet acknowledged by host + if ( usbd_edpt_busy(TUD_OPT_RHPORT, _netd_itf.ep_notif) ) return; + usbd_edpt_xfer(TUD_OPT_RHPORT, _netd_itf.ep_notif, buf, len); +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void netd_init(void) +{ + tu_memclr(&_netd_itf, sizeof(_netd_itf)); +} + +void netd_reset(uint8_t rhport) +{ + (void) rhport; + + netd_init(); +} + +uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + bool const is_rndis = (TUD_RNDIS_ITF_CLASS == itf_desc->bInterfaceClass && + TUD_RNDIS_ITF_SUBCLASS == itf_desc->bInterfaceSubClass && + TUD_RNDIS_ITF_PROTOCOL == itf_desc->bInterfaceProtocol); + + bool const is_ecm = (TUSB_CLASS_CDC == itf_desc->bInterfaceClass && + CDC_COMM_SUBCLASS_ETHERNET_CONTROL_MODEL == itf_desc->bInterfaceSubClass && + 0x00 == itf_desc->bInterfaceProtocol); + + TU_VERIFY(is_rndis || is_ecm, 0); + + // confirm interface hasn't already been allocated + TU_ASSERT(0 == _netd_itf.ep_notif, 0); + + // sanity check the descriptor + _netd_itf.ecm_mode = is_ecm; + + //------------- Management Interface -------------// + _netd_itf.itf_num = itf_desc->bInterfaceNumber; + + uint16_t drv_len = sizeof(tusb_desc_interface_t); + uint8_t const * p_desc = tu_desc_next( itf_desc ); + + // Communication Functional Descriptors + while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len ) + { + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + // notification endpoint (if any) + if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) ) + { + TU_ASSERT( usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0 ); + + _netd_itf.ep_notif = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress; + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + //------------- Data Interface -------------// + // - RNDIS Data followed immediately by a pair of endpoints + // - CDC-ECM data interface has 2 alternate settings + // - 0 : zero endpoints for inactive (default) + // - 1 : IN & OUT endpoints for active networking + TU_ASSERT(TUSB_DESC_INTERFACE == tu_desc_type(p_desc), 0); + + do + { + tusb_desc_interface_t const * data_itf_desc = (tusb_desc_interface_t const *) p_desc; + TU_ASSERT(TUSB_CLASS_CDC_DATA == data_itf_desc->bInterfaceClass, 0); + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + }while( _netd_itf.ecm_mode && (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && (drv_len <= max_len) ); + + // Pair of endpoints + TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc), 0); + + if ( _netd_itf.ecm_mode ) + { + // ECM by default is in-active, save the endpoint attribute + // to open later when received setInterface + _netd_itf.ecm_desc_epdata = p_desc; + }else + { + // Open endpoint pair for RNDIS + TU_ASSERT( usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &_netd_itf.ep_in), 0 ); + + tud_network_init_cb(); + + // we are ready to transmit a packet + can_xmit = true; + + // prepare for incoming packets + tud_network_recv_renew(); + } + + drv_len += 2*sizeof(tusb_desc_endpoint_t); + + return drv_len; +} + +static void ecm_report(bool nc) +{ + notify.ecm_buf = (nc) ? ecm_notify_nc : ecm_notify_csc; + notify.ecm_buf.header.wIndex = _netd_itf.itf_num; + netd_report((uint8_t *)¬ify.ecm_buf, (nc) ? sizeof(notify.ecm_buf.header) : sizeof(notify.ecm_buf)); +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + if ( stage == CONTROL_STAGE_SETUP ) + { + switch ( request->bmRequestType_bit.type ) + { + case TUSB_REQ_TYPE_STANDARD: + switch ( request->bRequest ) + { + case TUSB_REQ_GET_INTERFACE: + { + uint8_t const req_itfnum = (uint8_t) request->wIndex; + TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum); + + tud_control_xfer(rhport, request, &_netd_itf.itf_data_alt, 1); + } + break; + + case TUSB_REQ_SET_INTERFACE: + { + uint8_t const req_itfnum = (uint8_t) request->wIndex; + uint8_t const req_alt = (uint8_t) request->wValue; + + // Only valid for Data Interface with Alternate is either 0 or 1 + TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum && req_alt < 2); + + // ACM-ECM only: qequest to enable/disable network activities + TU_VERIFY(_netd_itf.ecm_mode); + + _netd_itf.itf_data_alt = req_alt; + + if ( _netd_itf.itf_data_alt ) + { + // TODO since we don't actually close endpoint + // hack here to not re-open it + if ( _netd_itf.ep_in == 0 && _netd_itf.ep_out == 0 ) + { + TU_ASSERT(_netd_itf.ecm_desc_epdata); + TU_ASSERT( usbd_open_edpt_pair(rhport, _netd_itf.ecm_desc_epdata, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &_netd_itf.ep_in) ); + + // TODO should be merge with RNDIS's after endpoint opened + // Also should have opposite callback for application to disable network !! + tud_network_init_cb(); + can_xmit = true; // we are ready to transmit a packet + tud_network_recv_renew(); // prepare for incoming packets + } + }else + { + // TODO close the endpoint pair + // For now pretend that we did, this should have no harm since host won't try to + // communicate with the endpoints again + // _netd_itf.ep_in = _netd_itf.ep_out = 0 + } + + tud_control_status(rhport, request); + } + break; + + // unsupported request + default: return false; + } + break; + + case TUSB_REQ_TYPE_CLASS: + TU_VERIFY (_netd_itf.itf_num == request->wIndex); + + if (_netd_itf.ecm_mode) + { + /* the only required CDC-ECM Management Element Request is SetEthernetPacketFilter */ + if (0x43 /* SET_ETHERNET_PACKET_FILTER */ == request->bRequest) + { + tud_control_xfer(rhport, request, NULL, 0); + ecm_report(true); + } + } + else + { + if (request->bmRequestType_bit.direction == TUSB_DIR_IN) + { + rndis_generic_msg_t *rndis_msg = (rndis_generic_msg_t *) ((void*) notify.rndis_buf); + uint32_t msglen = tu_le32toh(rndis_msg->MessageLength); + TU_ASSERT(msglen <= sizeof(notify.rndis_buf)); + tud_control_xfer(rhport, request, notify.rndis_buf, msglen); + } + else + { + tud_control_xfer(rhport, request, notify.rndis_buf, sizeof(notify.rndis_buf)); + } + } + break; + + // unsupported request + default: return false; + } + } + else if ( stage == CONTROL_STAGE_DATA ) + { + // Handle RNDIS class control OUT only + if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS && + request->bmRequestType_bit.direction == TUSB_DIR_OUT && + _netd_itf.itf_num == request->wIndex) + { + if ( !_netd_itf.ecm_mode ) + { + rndis_class_set_handler(notify.rndis_buf, request->wLength); + } + } + } + + return true; +} + +static void handle_incoming_packet(uint32_t len) +{ + uint8_t *pnt = received; + uint32_t size = 0; + + if (_netd_itf.ecm_mode) + { + size = len; + } + else + { + rndis_data_packet_t *r = (rndis_data_packet_t *) ((void*) pnt); + if (len >= sizeof(rndis_data_packet_t)) + if ( (r->MessageType == REMOTE_NDIS_PACKET_MSG) && (r->MessageLength <= len)) + if ( (r->DataOffset + offsetof(rndis_data_packet_t, DataOffset) + r->DataLength) <= len) + { + pnt = &received[r->DataOffset + offsetof(rndis_data_packet_t, DataOffset)]; + size = r->DataLength; + } + } + + if (!tud_network_recv_cb(pnt, size)) + { + /* if a buffer was never handled by user code, we must renew on the user's behalf */ + tud_network_recv_renew(); + } +} + +bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) rhport; + (void) result; + + /* new packet received */ + if ( ep_addr == _netd_itf.ep_out ) + { + handle_incoming_packet(xferred_bytes); + } + + /* data transmission finished */ + if ( ep_addr == _netd_itf.ep_in ) + { + /* TinyUSB requires the class driver to implement ZLP (since ZLP usage is class-specific) */ + + if ( xferred_bytes && (0 == (xferred_bytes % CFG_TUD_NET_ENDPOINT_SIZE)) ) + { + do_in_xfer(NULL, 0); /* a ZLP is needed */ + } + else + { + /* we're finally finished */ + can_xmit = true; + } + } + + if ( _netd_itf.ecm_mode && (ep_addr == _netd_itf.ep_notif) ) + { + if (sizeof(notify.ecm_buf.header) == xferred_bytes) ecm_report(false); + } + + return true; +} + +bool tud_network_can_xmit(uint16_t size) +{ + (void)size; + + return can_xmit; +} + +void tud_network_xmit(void *ref, uint16_t arg) +{ + uint8_t *data; + uint16_t len; + + if (!can_xmit) + return; + + len = (_netd_itf.ecm_mode) ? 0 : CFG_TUD_NET_PACKET_PREFIX_LEN; + data = transmitted + len; + + len += tud_network_xmit_cb(data, ref, arg); + + if (!_netd_itf.ecm_mode) + { + rndis_data_packet_t *hdr = (rndis_data_packet_t *) ((void*) transmitted); + memset(hdr, 0, sizeof(rndis_data_packet_t)); + hdr->MessageType = REMOTE_NDIS_PACKET_MSG; + hdr->MessageLength = len; + hdr->DataOffset = sizeof(rndis_data_packet_t) - offsetof(rndis_data_packet_t, DataOffset); + hdr->DataLength = len - sizeof(rndis_data_packet_t); + } + + do_in_xfer(transmitted, len); +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ncm.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ncm.h new file mode 100644 index 000000000..96ba11fbc --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ncm.h @@ -0,0 +1,69 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021, Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + + +#ifndef _TUSB_NCM_H_ +#define _TUSB_NCM_H_ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +// Table 4.3 Data Class Interface Protocol Codes +typedef enum +{ + NCM_DATA_PROTOCOL_NETWORK_TRANSFER_BLOCK = 0x01 +} ncm_data_interface_protocol_code_t; + + +// Table 6.2 Class-Specific Request Codes for Network Control Model subclass +typedef enum +{ + NCM_SET_ETHERNET_MULTICAST_FILTERS = 0x40, + NCM_SET_ETHERNET_POWER_MANAGEMENT_PATTERN_FILTER = 0x41, + NCM_GET_ETHERNET_POWER_MANAGEMENT_PATTERN_FILTER = 0x42, + NCM_SET_ETHERNET_PACKET_FILTER = 0x43, + NCM_GET_ETHERNET_STATISTIC = 0x44, + NCM_GET_NTB_PARAMETERS = 0x80, + NCM_GET_NET_ADDRESS = 0x81, + NCM_SET_NET_ADDRESS = 0x82, + NCM_GET_NTB_FORMAT = 0x83, + NCM_SET_NTB_FORMAT = 0x84, + NCM_GET_NTB_INPUT_SIZE = 0x85, + NCM_SET_NTB_INPUT_SIZE = 0x86, + NCM_GET_MAX_DATAGRAM_SIZE = 0x87, + NCM_SET_MAX_DATAGRAM_SIZE = 0x88, + NCM_GET_CRC_MODE = 0x89, + NCM_SET_CRC_MODE = 0x8A, +} ncm_request_code_t; + +#ifdef __cplusplus + } +#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ncm_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ncm_device.c new file mode 100644 index 000000000..3e131a85c --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/ncm_device.c @@ -0,0 +1,510 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Jacob Berg Potter + * Copyright (c) 2020 Peter Lawrence + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if ( TUSB_OPT_DEVICE_ENABLED && CFG_TUD_NCM ) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" +#include "net_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +#define NTH16_SIGNATURE 0x484D434E +#define NDP16_SIGNATURE_NCM0 0x304D434E +#define NDP16_SIGNATURE_NCM1 0x314D434E + +typedef struct TU_ATTR_PACKED +{ + uint16_t wLength; + uint16_t bmNtbFormatsSupported; + uint32_t dwNtbInMaxSize; + uint16_t wNdbInDivisor; + uint16_t wNdbInPayloadRemainder; + uint16_t wNdbInAlignment; + uint16_t wReserved; + uint32_t dwNtbOutMaxSize; + uint16_t wNdbOutDivisor; + uint16_t wNdbOutPayloadRemainder; + uint16_t wNdbOutAlignment; + uint16_t wNtbOutMaxDatagrams; +} ntb_parameters_t; + +typedef struct TU_ATTR_PACKED +{ + uint32_t dwSignature; + uint16_t wHeaderLength; + uint16_t wSequence; + uint16_t wBlockLength; + uint16_t wNdpIndex; +} nth16_t; + +typedef struct TU_ATTR_PACKED +{ + uint16_t wDatagramIndex; + uint16_t wDatagramLength; +} ndp16_datagram_t; + +typedef struct TU_ATTR_PACKED +{ + uint32_t dwSignature; + uint16_t wLength; + uint16_t wNextNdpIndex; + ndp16_datagram_t datagram[]; +} ndp16_t; + +typedef union TU_ATTR_PACKED { + struct { + nth16_t nth; + ndp16_t ndp; + }; + uint8_t data[CFG_TUD_NCM_IN_NTB_MAX_SIZE]; +} transmit_ntb_t; + +struct ecm_notify_struct +{ + tusb_control_request_t header; + uint32_t downlink, uplink; +}; + +typedef struct +{ + uint8_t itf_num; // Index number of Management Interface, +1 for Data Interface + uint8_t itf_data_alt; // Alternate setting of Data Interface. 0 : inactive, 1 : active + + uint8_t ep_notif; + uint8_t ep_in; + uint8_t ep_out; + + const ndp16_t *ndp; + uint8_t num_datagrams, current_datagram_index; + + enum { + REPORT_SPEED, + REPORT_CONNECTED, + REPORT_DONE + } report_state; + bool report_pending; + + uint8_t current_ntb; // Index in transmit_ntb[] that is currently being filled with datagrams + uint8_t datagram_count; // Number of datagrams in transmit_ntb[current_ntb] + uint16_t next_datagram_offset; // Offset in transmit_ntb[current_ntb].data to place the next datagram + uint16_t ntb_in_size; // Maximum size of transmitted (IN to host) NTBs; initially CFG_TUD_NCM_IN_NTB_MAX_SIZE + uint8_t max_datagrams_per_ntb; // Maximum number of datagrams per NTB; initially CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB + + uint16_t nth_sequence; // Sequence number counter for transmitted NTBs + + bool transferring; + +} ncm_interface_t; + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ + +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static const ntb_parameters_t ntb_parameters = { + .wLength = sizeof(ntb_parameters_t), + .bmNtbFormatsSupported = 0x01, + .dwNtbInMaxSize = CFG_TUD_NCM_IN_NTB_MAX_SIZE, + .wNdbInDivisor = 4, + .wNdbInPayloadRemainder = 0, + .wNdbInAlignment = CFG_TUD_NCM_ALIGNMENT, + .wReserved = 0, + .dwNtbOutMaxSize = CFG_TUD_NCM_OUT_NTB_MAX_SIZE, + .wNdbOutDivisor = 4, + .wNdbOutPayloadRemainder = 0, + .wNdbOutAlignment = CFG_TUD_NCM_ALIGNMENT, + .wNtbOutMaxDatagrams = 0 +}; + +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static transmit_ntb_t transmit_ntb[2]; + +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t receive_ntb[CFG_TUD_NCM_OUT_NTB_MAX_SIZE]; + +static ncm_interface_t ncm_interface; + +/* + * Set up the NTB state in ncm_interface to be ready to add datagrams. + */ +static void ncm_prepare_for_tx(void) { + ncm_interface.datagram_count = 0; + // datagrams start after all the headers + ncm_interface.next_datagram_offset = sizeof(nth16_t) + sizeof(ndp16_t) + + ((CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB + 1) * sizeof(ndp16_datagram_t)); +} + +/* + * If not already transmitting, start sending the current NTB to the host and swap buffers + * to start filling the other one with datagrams. + */ +static void ncm_start_tx(void) { + if (ncm_interface.transferring) { + return; + } + + transmit_ntb_t *ntb = &transmit_ntb[ncm_interface.current_ntb]; + size_t ntb_length = ncm_interface.next_datagram_offset; + + // Fill in NTB header + ntb->nth.dwSignature = NTH16_SIGNATURE; + ntb->nth.wHeaderLength = sizeof(nth16_t); + ntb->nth.wSequence = ncm_interface.nth_sequence++; + ntb->nth.wBlockLength = ntb_length; + ntb->nth.wNdpIndex = sizeof(nth16_t); + + // Fill in NDP16 header and terminator + ntb->ndp.dwSignature = NDP16_SIGNATURE_NCM0; + ntb->ndp.wLength = sizeof(ndp16_t) + (ncm_interface.datagram_count + 1) * sizeof(ndp16_datagram_t); + ntb->ndp.wNextNdpIndex = 0; + ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramIndex = 0; + ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramLength = 0; + + // Kick off an endpoint transfer + usbd_edpt_xfer(TUD_OPT_RHPORT, ncm_interface.ep_in, ntb->data, ntb_length); + ncm_interface.transferring = true; + + // Swap to the other NTB and clear it out + ncm_interface.current_ntb = 1 - ncm_interface.current_ntb; + ncm_prepare_for_tx(); +} + +static struct ecm_notify_struct ncm_notify_connected = +{ + .header = { + .bmRequestType_bit = { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_IN + }, + .bRequest = CDC_NOTIF_NETWORK_CONNECTION, + .wValue = 1 /* Connected */, + .wLength = 0, + }, +}; + +static struct ecm_notify_struct ncm_notify_speed_change = +{ + .header = { + .bmRequestType_bit = { + .recipient = TUSB_REQ_RCPT_INTERFACE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_IN + }, + .bRequest = CDC_NOTIF_CONNECTION_SPEED_CHANGE, + .wLength = 8, + }, + .downlink = 10000000, + .uplink = 10000000, +}; + +void tud_network_recv_renew(void) +{ + if (!ncm_interface.num_datagrams) + { + usbd_edpt_xfer(TUD_OPT_RHPORT, ncm_interface.ep_out, receive_ntb, sizeof(receive_ntb)); + return; + } + + const ndp16_t *ndp = ncm_interface.ndp; + const int i = ncm_interface.current_datagram_index; + ncm_interface.current_datagram_index++; + ncm_interface.num_datagrams--; + + tud_network_recv_cb(receive_ntb + ndp->datagram[i].wDatagramIndex, ndp->datagram[i].wDatagramLength); +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ + +void netd_init(void) +{ + tu_memclr(&ncm_interface, sizeof(ncm_interface)); + ncm_interface.ntb_in_size = CFG_TUD_NCM_IN_NTB_MAX_SIZE; + ncm_interface.max_datagrams_per_ntb = CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB; + ncm_prepare_for_tx(); +} + +void netd_reset(uint8_t rhport) +{ + (void) rhport; + + netd_init(); +} + +uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + // confirm interface hasn't already been allocated + TU_ASSERT(0 == ncm_interface.ep_notif, 0); + + //------------- Management Interface -------------// + ncm_interface.itf_num = itf_desc->bInterfaceNumber; + + uint16_t drv_len = sizeof(tusb_desc_interface_t); + uint8_t const * p_desc = tu_desc_next( itf_desc ); + + // Communication Functional Descriptors + while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len ) + { + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + // notification endpoint (if any) + if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) ) + { + TU_ASSERT( usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0 ); + + ncm_interface.ep_notif = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress; + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + //------------- Data Interface -------------// + // - CDC-NCM data interface has 2 alternate settings + // - 0 : zero endpoints for inactive (default) + // - 1 : IN & OUT endpoints for transfer of NTBs + TU_ASSERT(TUSB_DESC_INTERFACE == tu_desc_type(p_desc), 0); + + do + { + tusb_desc_interface_t const * data_itf_desc = (tusb_desc_interface_t const *) p_desc; + TU_ASSERT(TUSB_CLASS_CDC_DATA == data_itf_desc->bInterfaceClass, 0); + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } while((TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && (drv_len <= max_len)); + + // Pair of endpoints + TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc), 0); + + TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &ncm_interface.ep_out, &ncm_interface.ep_in) ); + + drv_len += 2*sizeof(tusb_desc_endpoint_t); + + return drv_len; +} + +static void ncm_report(void) +{ + if (ncm_interface.report_state == REPORT_SPEED) { + ncm_notify_speed_change.header.wIndex = ncm_interface.itf_num; + usbd_edpt_xfer(TUD_OPT_RHPORT, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_speed_change, sizeof(ncm_notify_speed_change)); + ncm_interface.report_state = REPORT_CONNECTED; + ncm_interface.report_pending = true; + } else if (ncm_interface.report_state == REPORT_CONNECTED) { + ncm_notify_connected.header.wIndex = ncm_interface.itf_num; + usbd_edpt_xfer(TUD_OPT_RHPORT, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_connected, sizeof(ncm_notify_connected)); + ncm_interface.report_state = REPORT_DONE; + ncm_interface.report_pending = true; + } +} + +TU_ATTR_WEAK void tud_network_link_state_cb(bool state) +{ + (void)state; +} + +// Handle class control request +// return false to stall control endpoint (e.g unsupported request) +bool netd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + if ( stage != CONTROL_STAGE_SETUP ) return true; + + switch ( request->bmRequestType_bit.type ) + { + case TUSB_REQ_TYPE_STANDARD: + switch ( request->bRequest ) + { + case TUSB_REQ_GET_INTERFACE: + { + uint8_t const req_itfnum = (uint8_t) request->wIndex; + TU_VERIFY(ncm_interface.itf_num + 1 == req_itfnum); + + tud_control_xfer(rhport, request, &ncm_interface.itf_data_alt, 1); + } + break; + + case TUSB_REQ_SET_INTERFACE: + { + uint8_t const req_itfnum = (uint8_t) request->wIndex; + uint8_t const req_alt = (uint8_t) request->wValue; + + // Only valid for Data Interface with Alternate is either 0 or 1 + TU_VERIFY(ncm_interface.itf_num + 1 == req_itfnum && req_alt < 2); + + if (req_alt != ncm_interface.itf_data_alt) { + ncm_interface.itf_data_alt = req_alt; + + if (ncm_interface.itf_data_alt) { + if (!usbd_edpt_busy(rhport, ncm_interface.ep_out)) { + tud_network_recv_renew(); // prepare for incoming datagrams + } + if (!ncm_interface.report_pending) { + ncm_report(); + } + } + + tud_network_link_state_cb(ncm_interface.itf_data_alt); + } + + tud_control_status(rhport, request); + } + break; + + // unsupported request + default: return false; + } + break; + + case TUSB_REQ_TYPE_CLASS: + TU_VERIFY (ncm_interface.itf_num == request->wIndex); + + if (NCM_GET_NTB_PARAMETERS == request->bRequest) + { + tud_control_xfer(rhport, request, (void*)&ntb_parameters, sizeof(ntb_parameters)); + } + + break; + + // unsupported request + default: return false; + } + + return true; +} + +static void handle_incoming_datagram(uint32_t len) +{ + uint32_t size = len; + + if (len == 0) { + return; + } + + TU_ASSERT(size >= sizeof(nth16_t), ); + + const nth16_t *hdr = (const nth16_t *)receive_ntb; + TU_ASSERT(hdr->dwSignature == NTH16_SIGNATURE, ); + TU_ASSERT(hdr->wNdpIndex >= sizeof(nth16_t) && (hdr->wNdpIndex + sizeof(ndp16_t)) <= len, ); + + const ndp16_t *ndp = (const ndp16_t *)(receive_ntb + hdr->wNdpIndex); + TU_ASSERT(ndp->dwSignature == NDP16_SIGNATURE_NCM0 || ndp->dwSignature == NDP16_SIGNATURE_NCM1, ); + TU_ASSERT(hdr->wNdpIndex + ndp->wLength <= len, ); + + int num_datagrams = (ndp->wLength - 12) / 4; + ncm_interface.current_datagram_index = 0; + ncm_interface.num_datagrams = 0; + ncm_interface.ndp = ndp; + for (int i = 0; i < num_datagrams && ndp->datagram[i].wDatagramIndex && ndp->datagram[i].wDatagramLength; i++) + { + ncm_interface.num_datagrams++; + } + + tud_network_recv_renew(); +} + +bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) rhport; + (void) result; + + /* new datagram receive_ntb */ + if (ep_addr == ncm_interface.ep_out ) + { + handle_incoming_datagram(xferred_bytes); + } + + /* data transmission finished */ + if (ep_addr == ncm_interface.ep_in ) + { + if (ncm_interface.transferring) { + ncm_interface.transferring = false; + } + + // If there are datagrams queued up that we tried to send while this NTB was being emitted, send them now + if (ncm_interface.datagram_count && ncm_interface.itf_data_alt == 1) { + ncm_start_tx(); + } + } + + if (ep_addr == ncm_interface.ep_notif ) + { + ncm_interface.report_pending = false; + ncm_report(); + } + + return true; +} + +// poll network driver for its ability to accept another packet to transmit +bool tud_network_can_xmit(uint16_t size) +{ + TU_VERIFY(ncm_interface.itf_data_alt == 1); + + if (ncm_interface.datagram_count >= ncm_interface.max_datagrams_per_ntb) { + TU_LOG2("NTB full [by count]\r\n"); + return false; + } + + size_t next_datagram_offset = ncm_interface.next_datagram_offset; + if (next_datagram_offset + size > ncm_interface.ntb_in_size) { + TU_LOG2("ntb full [by size]\r\n"); + return false; + } + + return true; +} + +void tud_network_xmit(void *ref, uint16_t arg) +{ + transmit_ntb_t *ntb = &transmit_ntb[ncm_interface.current_ntb]; + size_t next_datagram_offset = ncm_interface.next_datagram_offset; + + uint16_t size = tud_network_xmit_cb(ntb->data + next_datagram_offset, ref, arg); + + ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramIndex = ncm_interface.next_datagram_offset; + ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramLength = size; + + ncm_interface.datagram_count++; + next_datagram_offset += size; + + // round up so the next datagram is aligned correctly + next_datagram_offset += (CFG_TUD_NCM_ALIGNMENT - 1); + next_datagram_offset -= (next_datagram_offset % CFG_TUD_NCM_ALIGNMENT); + + ncm_interface.next_datagram_offset = next_datagram_offset; + + ncm_start_tx(); +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/net_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/net_device.h new file mode 100644 index 000000000..6e294465b --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/net/net_device.h @@ -0,0 +1,118 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Peter Lawrence + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_NET_DEVICE_H_ +#define _TUSB_NET_DEVICE_H_ + +#include "class/cdc/cdc.h" + +#if CFG_TUD_ECM_RNDIS && CFG_TUD_NCM +#error "Cannot enable both ECM_RNDIS and NCM network drivers" +#endif + +#include "ncm.h" + +/* declared here, NOT in usb_descriptors.c, so that the driver can intelligently ZLP as needed */ +#define CFG_TUD_NET_ENDPOINT_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) + +/* Maximum Transmission Unit (in bytes) of the network, including Ethernet header */ +#ifndef CFG_TUD_NET_MTU +#define CFG_TUD_NET_MTU 1514 +#endif + +#ifndef CFG_TUD_NCM_IN_NTB_MAX_SIZE +#define CFG_TUD_NCM_IN_NTB_MAX_SIZE 3200 +#endif + +#ifndef CFG_TUD_NCM_OUT_NTB_MAX_SIZE +#define CFG_TUD_NCM_OUT_NTB_MAX_SIZE 3200 +#endif + +#ifndef CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB +#define CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB 8 +#endif + +#ifndef CFG_TUD_NCM_ALIGNMENT +#define CFG_TUD_NCM_ALIGNMENT 4 +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Application API +//--------------------------------------------------------------------+ + +// indicate to network driver that client has finished with the packet provided to network_recv_cb() +void tud_network_recv_renew(void); + +// poll network driver for its ability to accept another packet to transmit +bool tud_network_can_xmit(uint16_t size); + +// if network_can_xmit() returns true, network_xmit() can be called once +void tud_network_xmit(void *ref, uint16_t arg); + +//--------------------------------------------------------------------+ +// Application Callbacks (WEAK is optional) +//--------------------------------------------------------------------+ + +// client must provide this: return false if the packet buffer was not accepted +bool tud_network_recv_cb(const uint8_t *src, uint16_t size); + +// client must provide this: copy from network stack packet pointer to dst +uint16_t tud_network_xmit_cb(uint8_t *dst, void *ref, uint16_t arg); + +//------------- ECM/RNDIS -------------// + +// client must provide this: initialize any network state back to the beginning +void tud_network_init_cb(void); + +// client must provide this: 48-bit MAC address +// TODO removed later since it is not part of tinyusb stack +extern const uint8_t tud_network_mac_address[6]; + +//------------- NCM -------------// + +// callback to client providing optional indication of internal state of network driver +void tud_network_link_state_cb(bool state); + +//--------------------------------------------------------------------+ +// INTERNAL USBD-CLASS DRIVER API +//--------------------------------------------------------------------+ +void netd_init (void); +void netd_reset (uint8_t rhport); +uint16_t netd_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +bool netd_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); +void netd_report (uint8_t *buf, uint16_t len); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_NET_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc.h new file mode 100644 index 000000000..7d7005c2e --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc.h @@ -0,0 +1,316 @@ + +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 N Conrad + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_USBTMC_H__ +#define _TUSB_USBTMC_H__ + +#include "common/tusb_common.h" + + +/* Implements USBTMC Revision 1.0, April 14, 2003 + + String descriptors must have a "LANGID=0x409"/US English string. + Characters must be 0x20 (' ') to 0x7E ('~') ASCII, + But MUST not contain: "/:?\* + Also must not have leading or trailing space (' ') + Device descriptor must state USB version 0x0200 or greater + + If USB488DeviceCapabilites.D2 = 1 (SR1), then there must be a INT endpoint. +*/ + +#define USBTMC_VERSION 0x0100 +#define USBTMC_488_VERSION 0x0100 + +typedef enum { + USBTMC_MSGID_DEV_DEP_MSG_OUT = 1u, + USBTMC_MSGID_DEV_DEP_MSG_IN = 2u, + USBTMC_MSGID_VENDOR_SPECIFIC_MSG_OUT = 126u, + USBTMC_MSGID_VENDOR_SPECIFIC_IN = 127u, + USBTMC_MSGID_USB488_TRIGGER = 128u, +} usbtmc_msgid_enum; + +/// \brief Message header (For BULK OUT and BULK IN); 4 bytes +typedef struct TU_ATTR_PACKED +{ + uint8_t MsgID ; ///< Message type ID (usbtmc_msgid_enum) + uint8_t bTag ; ///< Transfer ID 1<=bTag<=255 + uint8_t bTagInverse ; ///< Complement of the tag + uint8_t _reserved ; ///< Must be 0x00 +} usbtmc_msg_header_t; + +typedef struct TU_ATTR_PACKED +{ + usbtmc_msg_header_t header; + uint8_t data[8]; +} usbtmc_msg_generic_t; + +/* Uses on the bulk-out endpoint: */ +// Next 8 bytes are message-specific +typedef struct TU_ATTR_PACKED { + usbtmc_msg_header_t header ; ///< Header + uint32_t TransferSize ; ///< Transfer size; LSB first + struct TU_ATTR_PACKED + { + unsigned int EOM : 1 ; ///< EOM set on last byte + } bmTransferAttributes; + uint8_t _reserved[3]; +} usbtmc_msg_request_dev_dep_out; + +TU_VERIFY_STATIC(sizeof(usbtmc_msg_request_dev_dep_out) == 12u, "struct wrong length"); + +// Next 8 bytes are message-specific +typedef struct TU_ATTR_PACKED +{ + usbtmc_msg_header_t header ; ///< Header + uint32_t TransferSize ; ///< Transfer size; LSB first + struct TU_ATTR_PACKED + { + unsigned int TermCharEnabled : 1 ; ///< "The Bulk-IN transfer must terminate on the specified TermChar."; CAPABILITIES must list TermChar + } bmTransferAttributes; + uint8_t TermChar; + uint8_t _reserved[2]; +} usbtmc_msg_request_dev_dep_in; + +TU_VERIFY_STATIC(sizeof(usbtmc_msg_request_dev_dep_in) == 12u, "struct wrong length"); + +/* Bulk-in headers */ + +typedef struct TU_ATTR_PACKED +{ + usbtmc_msg_header_t header; + uint32_t TransferSize; + struct TU_ATTR_PACKED + { + uint8_t EOM: 1; ///< Last byte of transfer is the end of the message + uint8_t UsingTermChar: 1; ///< Support TermChar && Request.TermCharEnabled && last char in transfer is TermChar + } bmTransferAttributes; + uint8_t _reserved[3]; +} usbtmc_msg_dev_dep_msg_in_header_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_msg_dev_dep_msg_in_header_t) == 12u, "struct wrong length"); + +/* Unsupported vendor things.... Are these ever used?*/ + +typedef struct TU_ATTR_PACKED +{ + usbtmc_msg_header_t header ; ///< Header + uint32_t TransferSize ; ///< Transfer size; LSB first + uint8_t _reserved[4]; +} usbtmc_msg_request_vendor_specific_out; + + +TU_VERIFY_STATIC(sizeof(usbtmc_msg_request_vendor_specific_out) == 12u, "struct wrong length"); + +typedef struct TU_ATTR_PACKED +{ + usbtmc_msg_header_t header ; ///< Header + uint32_t TransferSize ; ///< Transfer size; LSB first + uint8_t _reserved[4]; +} usbtmc_msg_request_vendor_specific_in; + +TU_VERIFY_STATIC(sizeof(usbtmc_msg_request_vendor_specific_in) == 12u, "struct wrong length"); + +// Control request type should use tusb_control_request_t + +/* +typedef struct TU_ATTR_PACKED { + struct { + unsigned int Recipient : 5 ; ///< EOM set on last byte + unsigned int Type : 2 ; ///< EOM set on last byte + unsigned int DirectionToHost : 1 ; ///< 0 is OUT, 1 is IN + } bmRequestType; + uint8_t bRequest ; ///< If bmRequestType.Type = Class, see usmtmc_request_type_enum + uint16_t wValue ; + uint16_t wIndex ; + uint16_t wLength ; // Number of bytes in data stage +} usbtmc_class_specific_control_req; + +*/ +// bulk-in protocol errors +enum { + USBTMC_BULK_IN_ERR_INCOMPLETE_HEADER = 1u, + USBTMC_BULK_IN_ERR_UNSUPPORTED = 2u, + USBTMC_BULK_IN_ERR_BAD_PARAMETER = 3u, + USBTMC_BULK_IN_ERR_DATA_TOO_SHORT = 4u, + USBTMC_BULK_IN_ERR_DATA_TOO_LONG = 5u, +}; +// bult-in halt errors +enum { + USBTMC_BULK_IN_ERR = 1u, ///< receives a USBTMC command message that expects a response while a + /// Bulk-IN transfer is in progress +}; + +typedef enum { + USBTMC_bREQUEST_INITIATE_ABORT_BULK_OUT = 1u, + USBTMC_bREQUEST_CHECK_ABORT_BULK_OUT_STATUS = 2u, + USBTMC_bREQUEST_INITIATE_ABORT_BULK_IN = 3u, + USBTMC_bREQUEST_CHECK_ABORT_BULK_IN_STATUS = 4u, + USBTMC_bREQUEST_INITIATE_CLEAR = 5u, + USBTMC_bREQUEST_CHECK_CLEAR_STATUS = 6u, + USBTMC_bREQUEST_GET_CAPABILITIES = 7u, + + USBTMC_bREQUEST_INDICATOR_PULSE = 64u, // Optional + + /****** USBTMC 488 *************/ + USB488_bREQUEST_READ_STATUS_BYTE = 128u, + USB488_bREQUEST_REN_CONTROL = 160u, + USB488_bREQUEST_GO_TO_LOCAL = 161u, + USB488_bREQUEST_LOCAL_LOCKOUT = 162u, + +} usmtmc_request_type_enum; + +typedef enum { + USBTMC_STATUS_SUCCESS = 0x01, + USBTMC_STATUS_PENDING = 0x02, + USBTMC_STATUS_FAILED = 0x80, + USBTMC_STATUS_TRANSFER_NOT_IN_PROGRESS = 0x81, + USBTMC_STATUS_SPLIT_NOT_IN_PROGRESS = 0x82, + USBTMC_STATUS_SPLIT_IN_PROGRESS = 0x83 +} usbtmc_status_enum; + +/************************************************************ + * Control Responses + */ + +typedef struct TU_ATTR_PACKED { + uint8_t USBTMC_status; ///< usbtmc_status_enum + uint8_t _reserved; + uint16_t bcdUSBTMC; ///< USBTMC_VERSION + + struct TU_ATTR_PACKED + { + unsigned int listenOnly :1; + unsigned int talkOnly :1; + unsigned int supportsIndicatorPulse :1; + } bmIntfcCapabilities; + struct TU_ATTR_PACKED + { + unsigned int canEndBulkInOnTermChar :1; + } bmDevCapabilities; + uint8_t _reserved2[6]; + uint8_t _reserved3[12]; +} usbtmc_response_capabilities_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_response_capabilities_t) == 0x18, "struct wrong length"); + +typedef struct TU_ATTR_PACKED +{ + uint8_t USBTMC_status; + struct TU_ATTR_PACKED + { + unsigned int BulkInFifoBytes :1; + } bmClear; +} usbtmc_get_clear_status_rsp_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_get_clear_status_rsp_t) == 2u, "struct wrong length"); + +// Used for both abort bulk IN and bulk OUT +typedef struct TU_ATTR_PACKED +{ + uint8_t USBTMC_status; + uint8_t bTag; +} usbtmc_initiate_abort_rsp_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_get_clear_status_rsp_t) == 2u, "struct wrong length"); + +// Used for both check_abort_bulk_in_status and check_abort_bulk_out_status +typedef struct TU_ATTR_PACKED +{ + uint8_t USBTMC_status; + struct TU_ATTR_PACKED + { + unsigned int BulkInFifoBytes : 1; ///< Has queued data or a short packet that is queued + } bmAbortBulkIn; + uint8_t _reserved[2]; ///< Must be zero + uint32_t NBYTES_RXD_TXD; +} usbtmc_check_abort_bulk_rsp_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_check_abort_bulk_rsp_t) == 8u, "struct wrong length"); + +typedef struct TU_ATTR_PACKED +{ + uint8_t USBTMC_status; ///< usbtmc_status_enum + uint8_t _reserved; + uint16_t bcdUSBTMC; ///< USBTMC_VERSION + + struct TU_ATTR_PACKED + { + unsigned int listenOnly :1; + unsigned int talkOnly :1; + unsigned int supportsIndicatorPulse :1; + } bmIntfcCapabilities; + + struct TU_ATTR_PACKED + { + unsigned int canEndBulkInOnTermChar :1; + } bmDevCapabilities; + + uint8_t _reserved2[6]; + uint16_t bcdUSB488; + + struct TU_ATTR_PACKED + { + unsigned int is488_2 :1; + unsigned int supportsREN_GTL_LLO :1; + unsigned int supportsTrigger :1; + } bmIntfcCapabilities488; + + struct TU_ATTR_PACKED + { + unsigned int SCPI :1; + unsigned int SR1 :1; + unsigned int RL1 :1; + unsigned int DT1 :1; + } bmDevCapabilities488; + uint8_t _reserved3[8]; +} usbtmc_response_capabilities_488_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_response_capabilities_488_t) == 0x18, "struct wrong length"); + +typedef struct TU_ATTR_PACKED +{ + uint8_t USBTMC_status; + uint8_t bTag; + uint8_t statusByte; +} usbtmc_read_stb_rsp_488_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_read_stb_rsp_488_t) == 3u, "struct wrong length"); + +typedef struct TU_ATTR_PACKED +{ + struct TU_ATTR_PACKED + { + unsigned int bTag : 7; + unsigned int one : 1; + } bNotify1; + uint8_t StatusByte; +} usbtmc_read_stb_interrupt_488_t; + +TU_VERIFY_STATIC(sizeof(usbtmc_read_stb_interrupt_488_t) == 2u, "struct wrong length"); + +#endif + diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc_device.c new file mode 100644 index 000000000..26be987cf --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc_device.c @@ -0,0 +1,858 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Nathan Conrad + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/* + * This library is not fully reentrant, though it is reentrant from the view + * of either the application layer or the USB stack. Due to its locking, + * it is not safe to call its functions from interrupts. + * + * The one exception is that its functions may not be called from the application + * until the USB stack is initialized. This should not be a problem since the + * device shouldn't be sending messages until it receives a request from the + * host. + */ + + +/* + * In the case of single-CPU "no OS", this task is never preempted other than by + * interrupts, and the USBTMC code isn't called by interrupts, so all is OK. For "no OS", + * the mutex structure's main effect is to disable the USB interrupts. + * With an OS, this class driver uses the OSAL to perform locking. The code uses a single lock + * and does not call outside of this class with a lock held, so deadlocks won't happen. + */ + +//Limitations: +// "vendor-specific" commands are not handled. +// Dealing with "termchar" must be handled by the application layer, +// though additional error checking is does in this module. +// talkOnly and listenOnly are NOT supported. They're not permitted +// in USB488, anyway. + +/* Supported: + * + * Notification pulse + * Trigger + * Read status byte (both by interrupt endpoint and control message) + * + */ + + +// TODO: +// USBTMC 3.2.2 error conditions not strictly followed +// No local lock-out, REN, or GTL. +// Clear message available status byte at the correct time? (488 4.3.1.3) + + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_USBTMC) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "usbtmc_device.h" + +#ifdef xDEBUG +#include "uart_util.h" +static char logMsg[150]; +#endif + +/* + * The state machine does not allow simultaneous reading and writing. This is + * consistent with USBTMC. + */ + +typedef enum +{ + STATE_CLOSED, // Endpoints have not yet been opened since USB reset + STATE_NAK, // Bulk-out endpoint is in NAK state. + STATE_IDLE, // Bulk-out endpoint is waiting for CMD. + STATE_RCV, // Bulk-out is receiving DEV_DEP message + STATE_TX_REQUESTED, + STATE_TX_INITIATED, + STATE_TX_SHORTED, + STATE_CLEARING, + STATE_ABORTING_BULK_IN, + STATE_ABORTING_BULK_IN_SHORTED, // aborting, and short packet has been queued for transmission + STATE_ABORTING_BULK_IN_ABORTED, // aborting, and short packet has been transmitted + STATE_ABORTING_BULK_OUT, + STATE_NUM_STATES +} usbtmcd_state_enum; + +#if (CFG_TUD_USBTMC_ENABLE_488) + typedef usbtmc_response_capabilities_488_t usbtmc_capabilities_specific_t; +#else + typedef usbtmc_response_capabilities_t usbtmc_capabilities_specific_t; +#endif + + +typedef struct +{ + volatile usbtmcd_state_enum state; + + uint8_t itf_id; + uint8_t rhport; + uint8_t ep_bulk_in; + uint8_t ep_bulk_out; + uint8_t ep_int_in; + // IN buffer is only used for first packet, not the remainder + // in order to deal with prepending header + CFG_TUSB_MEM_ALIGN uint8_t ep_bulk_in_buf[USBTMCD_MAX_PACKET_SIZE]; + // OUT buffer receives one packet at a time + CFG_TUSB_MEM_ALIGN uint8_t ep_bulk_out_buf[USBTMCD_MAX_PACKET_SIZE]; + uint32_t transfer_size_remaining; // also used for requested length for bulk IN. + uint32_t transfer_size_sent; // To keep track of data bytes that have been queued in FIFO (not header bytes) + + uint8_t lastBulkOutTag; // used for aborts (mostly) + uint8_t lastBulkInTag; // used for aborts (mostly) + + uint8_t const * devInBuffer; // pointer to application-layer used for transmissions + + usbtmc_capabilities_specific_t const * capabilities; +} usbtmc_interface_state_t; + +CFG_TUSB_MEM_SECTION static usbtmc_interface_state_t usbtmc_state = +{ + .itf_id = 0xFF, +}; + +// We need all headers to fit in a single packet in this implementation. +TU_VERIFY_STATIC(USBTMCD_MAX_PACKET_SIZE >= 32u,"USBTMC dev EP packet size too small"); +TU_VERIFY_STATIC( + (sizeof(usbtmc_state.ep_bulk_in_buf) % USBTMCD_MAX_PACKET_SIZE) == 0, + "packet buffer must be a multiple of the packet size"); + +static bool handle_devMsgOutStart(uint8_t rhport, void *data, size_t len); +static bool handle_devMsgOut(uint8_t rhport, void *data, size_t len, size_t packetLen); + +static uint8_t termChar; +static uint8_t termCharRequested = false; + + +osal_mutex_def_t usbtmcLockBuffer; +static osal_mutex_t usbtmcLock; + +// Our own private lock, mostly for the state variable. +#define criticalEnter() do {osal_mutex_lock(usbtmcLock,OSAL_TIMEOUT_WAIT_FOREVER); } while (0) +#define criticalLeave() do {osal_mutex_unlock(usbtmcLock); } while (0) + +bool atomicChangeState(usbtmcd_state_enum expectedState, usbtmcd_state_enum newState) +{ + bool ret = true; + criticalEnter(); + usbtmcd_state_enum oldState = usbtmc_state.state; + if (oldState == expectedState) + { + usbtmc_state.state = newState; + } + else + { + ret = false; + } + criticalLeave(); + return ret; +} + +// called from app +// We keep a reference to the buffer, so it MUST not change until the app is +// notified that the transfer is complete. +// length of data is specified in the hdr. + +// We can't just send the whole thing at once because we need to concatanate the +// header with the data. +bool tud_usbtmc_transmit_dev_msg_data( + const void * data, size_t len, + bool endOfMessage, + bool usingTermChar) +{ + const unsigned int txBufLen = sizeof(usbtmc_state.ep_bulk_in_buf); + +#ifndef NDEBUG + TU_ASSERT(len > 0u); + TU_ASSERT(len <= usbtmc_state.transfer_size_remaining); + TU_ASSERT(usbtmc_state.transfer_size_sent == 0u); + if(usingTermChar) + { + TU_ASSERT(usbtmc_state.capabilities->bmDevCapabilities.canEndBulkInOnTermChar); + TU_ASSERT(termCharRequested); + TU_ASSERT(((uint8_t const*)data)[len-1u] == termChar); + } +#endif + + TU_VERIFY(usbtmc_state.state == STATE_TX_REQUESTED); + usbtmc_msg_dev_dep_msg_in_header_t *hdr = (usbtmc_msg_dev_dep_msg_in_header_t*)usbtmc_state.ep_bulk_in_buf; + tu_varclr(hdr); + hdr->header.MsgID = USBTMC_MSGID_DEV_DEP_MSG_IN; + hdr->header.bTag = usbtmc_state.lastBulkInTag; + hdr->header.bTagInverse = (uint8_t)~(usbtmc_state.lastBulkInTag); + hdr->TransferSize = len; + hdr->bmTransferAttributes.EOM = endOfMessage; + hdr->bmTransferAttributes.UsingTermChar = usingTermChar; + + // Copy in the header + const size_t headerLen = sizeof(*hdr); + const size_t dataLen = ((headerLen + hdr->TransferSize) <= txBufLen) ? + len : (txBufLen - headerLen); + const size_t packetLen = headerLen + dataLen; + + memcpy((uint8_t*)(usbtmc_state.ep_bulk_in_buf) + headerLen, data, dataLen); + usbtmc_state.transfer_size_remaining = len - dataLen; + usbtmc_state.transfer_size_sent = dataLen; + usbtmc_state.devInBuffer = (uint8_t const*) data + (dataLen); + + bool stateChanged = + atomicChangeState(STATE_TX_REQUESTED, (packetLen >= txBufLen) ? STATE_TX_INITIATED : STATE_TX_SHORTED); + TU_VERIFY(stateChanged); + TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf, (uint16_t)packetLen)); + return true; +} + +void usbtmcd_init_cb(void) +{ + usbtmc_state.capabilities = tud_usbtmc_get_capabilities_cb(); +#ifndef NDEBUG +# if CFG_TUD_USBTMC_ENABLE_488 + if (usbtmc_state.capabilities->bmIntfcCapabilities488.supportsTrigger) { + TU_ASSERT(&tud_usbtmc_msg_trigger_cb != NULL,); + } + // Per USB488 spec: table 8 + TU_ASSERT(!usbtmc_state.capabilities->bmIntfcCapabilities.listenOnly,); + TU_ASSERT(!usbtmc_state.capabilities->bmIntfcCapabilities.talkOnly,); +# endif + if (usbtmc_state.capabilities->bmIntfcCapabilities.supportsIndicatorPulse) { + TU_ASSERT(&tud_usbtmc_indicator_pulse_cb != NULL,); + } +#endif + + usbtmcLock = osal_mutex_create(&usbtmcLockBuffer); +} + +uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + (void)rhport; + + uint16_t drv_len; + uint8_t const * p_desc; + uint8_t found_endpoints = 0; + + TU_VERIFY(itf_desc->bInterfaceClass == TUD_USBTMC_APP_CLASS , 0); + TU_VERIFY(itf_desc->bInterfaceSubClass == TUD_USBTMC_APP_SUBCLASS, 0); + +#ifndef NDEBUG + // Only 2 or 3 endpoints are allowed for USBTMC. + TU_ASSERT((itf_desc->bNumEndpoints == 2) || (itf_desc->bNumEndpoints ==3), 0); +#endif + + TU_ASSERT(usbtmc_state.state == STATE_CLOSED, 0); + + // Interface + drv_len = 0u; + p_desc = (uint8_t const *) itf_desc; + + usbtmc_state.itf_id = itf_desc->bInterfaceNumber; + usbtmc_state.rhport = rhport; + + while (found_endpoints < itf_desc->bNumEndpoints && drv_len <= max_len) + { + if ( TUSB_DESC_ENDPOINT == p_desc[DESC_OFFSET_TYPE]) + { + tusb_desc_endpoint_t const *ep_desc = (tusb_desc_endpoint_t const *)p_desc; + switch(ep_desc->bmAttributes.xfer) { + case TUSB_XFER_BULK: + TU_ASSERT(tu_edpt_packet_size(ep_desc) == USBTMCD_MAX_PACKET_SIZE, 0); + if (tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN) + { + usbtmc_state.ep_bulk_in = ep_desc->bEndpointAddress; + } else { + usbtmc_state.ep_bulk_out = ep_desc->bEndpointAddress; + } + + break; + case TUSB_XFER_INTERRUPT: +#ifndef NDEBUG + TU_ASSERT(tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN, 0); + TU_ASSERT(usbtmc_state.ep_int_in == 0, 0); +#endif + usbtmc_state.ep_int_in = ep_desc->bEndpointAddress; + break; + default: + TU_ASSERT(false, 0); + } + TU_ASSERT( usbd_edpt_open(rhport, ep_desc), 0); + found_endpoints++; + } + + drv_len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + + // bulk endpoints are required, but interrupt IN is optional +#ifndef NDEBUG + TU_ASSERT(usbtmc_state.ep_bulk_in != 0, 0); + TU_ASSERT(usbtmc_state.ep_bulk_out != 0, 0); + if (itf_desc->bNumEndpoints == 2) + { + TU_ASSERT(usbtmc_state.ep_int_in == 0, 0); + } + else if (itf_desc->bNumEndpoints == 3) + { + TU_ASSERT(usbtmc_state.ep_int_in != 0, 0); + } +#if (CFG_TUD_USBTMC_ENABLE_488) + if(usbtmc_state.capabilities->bmIntfcCapabilities488.is488_2 || + usbtmc_state.capabilities->bmDevCapabilities488.SR1) + { + TU_ASSERT(usbtmc_state.ep_int_in != 0, 0); + } +#endif +#endif + atomicChangeState(STATE_CLOSED, STATE_NAK); + tud_usbtmc_open_cb(itf_desc->iInterface); + + return drv_len; +} +// Tell USBTMC class to set its bulk-in EP to ACK so that it can +// receive USBTMC commands. +// Returns false if it was already in an ACK state or is busy +// processing a command (such as a clear). Returns true if it was +// in the NAK state and successfully transitioned to the ACK wait +// state. +bool tud_usbtmc_start_bus_read() +{ + usbtmcd_state_enum oldState = usbtmc_state.state; + switch(oldState) + { + // These may transition to IDLE + case STATE_NAK: + case STATE_ABORTING_BULK_IN_ABORTED: + TU_VERIFY(atomicChangeState(oldState, STATE_IDLE)); + break; + // When receiving, let it remain receiving + case STATE_RCV: + break; + default: + TU_VERIFY(false); + } + TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_bulk_out, usbtmc_state.ep_bulk_out_buf, 64)); + return true; +} + +void usbtmcd_reset_cb(uint8_t rhport) +{ + (void)rhport; + usbtmc_capabilities_specific_t const * capabilities = tud_usbtmc_get_capabilities_cb(); + + criticalEnter(); + tu_varclr(&usbtmc_state); + usbtmc_state.capabilities = capabilities; + usbtmc_state.itf_id = 0xFFu; + criticalLeave(); +} + +static bool handle_devMsgOutStart(uint8_t rhport, void *data, size_t len) +{ + (void)rhport; + // return true upon failure, as we can assume error is being handled elsewhere. + TU_VERIFY(atomicChangeState(STATE_IDLE, STATE_RCV), true); + usbtmc_state.transfer_size_sent = 0u; + + // must be a header, should have been confirmed before calling here. + usbtmc_msg_request_dev_dep_out *msg = (usbtmc_msg_request_dev_dep_out*)data; + usbtmc_state.transfer_size_remaining = msg->TransferSize; + TU_VERIFY(tud_usbtmc_msgBulkOut_start_cb(msg)); + + TU_VERIFY(handle_devMsgOut(rhport, (uint8_t*)data + sizeof(*msg), len - sizeof(*msg), len)); + usbtmc_state.lastBulkOutTag = msg->header.bTag; + return true; +} + +static bool handle_devMsgOut(uint8_t rhport, void *data, size_t len, size_t packetLen) +{ + (void)rhport; + // return true upon failure, as we can assume error is being handled elsewhere. + TU_VERIFY(usbtmc_state.state == STATE_RCV,true); + + bool shortPacket = (packetLen < USBTMCD_MAX_PACKET_SIZE); + + // Packet is to be considered complete when we get enough data or at a short packet. + bool atEnd = false; + if(len >= usbtmc_state.transfer_size_remaining || shortPacket) + { + atEnd = true; + TU_VERIFY(atomicChangeState(STATE_RCV, STATE_NAK)); + } + + len = tu_min32(len, usbtmc_state.transfer_size_remaining); + + usbtmc_state.transfer_size_remaining -= len; + usbtmc_state.transfer_size_sent += len; + + // App may (should?) call the wait_for_bus() command at this point + if(!tud_usbtmc_msg_data_cb(data, len, atEnd)) + { + // TODO: Go to an error state upon failure other than just stalling the EP? + return false; + } + + + return true; +} + +static bool handle_devMsgIn(void *data, size_t len) +{ + TU_VERIFY(len == sizeof(usbtmc_msg_request_dev_dep_in)); + usbtmc_msg_request_dev_dep_in *msg = (usbtmc_msg_request_dev_dep_in*)data; + bool stateChanged = atomicChangeState(STATE_IDLE, STATE_TX_REQUESTED); + TU_VERIFY(stateChanged); + usbtmc_state.lastBulkInTag = msg->header.bTag; + usbtmc_state.transfer_size_remaining = msg->TransferSize; + usbtmc_state.transfer_size_sent = 0u; + + termCharRequested = msg->bmTransferAttributes.TermCharEnabled; + termChar = msg->TermChar; + + if(termCharRequested) + TU_VERIFY(usbtmc_state.capabilities->bmDevCapabilities.canEndBulkInOnTermChar); + + TU_VERIFY(tud_usbtmc_msgBulkIn_request_cb(msg)); + return true; +} + +bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + TU_VERIFY(result == XFER_RESULT_SUCCESS); + //uart_tx_str_sync("TMC XFER CB\r\n"); + if(usbtmc_state.state == STATE_CLEARING) { + return true; /* I think we can ignore everything here */ + } + + if(ep_addr == usbtmc_state.ep_bulk_out) + { + usbtmc_msg_generic_t *msg = NULL; + + switch(usbtmc_state.state) + { + case STATE_IDLE: + TU_VERIFY(xferred_bytes >= sizeof(usbtmc_msg_generic_t)); + msg = (usbtmc_msg_generic_t*)(usbtmc_state.ep_bulk_out_buf); + uint8_t invInvTag = (uint8_t)~(msg->header.bTagInverse); + TU_VERIFY(msg->header.bTag == invInvTag); + TU_VERIFY(msg->header.bTag != 0x00); + + switch(msg->header.MsgID) { + case USBTMC_MSGID_DEV_DEP_MSG_OUT: + if(!handle_devMsgOutStart(rhport, msg, xferred_bytes)) + { + usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out); + TU_VERIFY(false); + } + break; + + case USBTMC_MSGID_DEV_DEP_MSG_IN: + TU_VERIFY(handle_devMsgIn(msg, xferred_bytes)); + break; + +#if (CFG_TUD_USBTMC_ENABLE_488) + case USBTMC_MSGID_USB488_TRIGGER: + // Spec says we halt the EP if we didn't declare we support it. + TU_VERIFY(usbtmc_state.capabilities->bmIntfcCapabilities488.supportsTrigger); + TU_VERIFY(tud_usbtmc_msg_trigger_cb(msg)); + + break; +#endif + case USBTMC_MSGID_VENDOR_SPECIFIC_MSG_OUT: + case USBTMC_MSGID_VENDOR_SPECIFIC_IN: + default: + usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out); + TU_VERIFY(false); + return false; + } + return true; + + case STATE_RCV: + if(!handle_devMsgOut(rhport, usbtmc_state.ep_bulk_out_buf, xferred_bytes, xferred_bytes)) + { + usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out); + TU_VERIFY(false); + } + return true; + + case STATE_ABORTING_BULK_OUT: + TU_VERIFY(false); + return false; // Should be stalled by now, shouldn't have received a packet. + + case STATE_TX_REQUESTED: + case STATE_TX_INITIATED: + case STATE_ABORTING_BULK_IN: + case STATE_ABORTING_BULK_IN_SHORTED: + case STATE_ABORTING_BULK_IN_ABORTED: + default: + TU_VERIFY(false); + } + } + else if(ep_addr == usbtmc_state.ep_bulk_in) + { + switch(usbtmc_state.state) { + case STATE_TX_SHORTED: + TU_VERIFY(atomicChangeState(STATE_TX_SHORTED, STATE_NAK)); + TU_VERIFY(tud_usbtmc_msgBulkIn_complete_cb()); + break; + + case STATE_TX_INITIATED: + if(usbtmc_state.transfer_size_remaining >=sizeof(usbtmc_state.ep_bulk_in_buf)) + { + // FIXME! This removes const below! + TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, + (void*)(uintptr_t) usbtmc_state.devInBuffer, sizeof(usbtmc_state.ep_bulk_in_buf))); + usbtmc_state.devInBuffer += sizeof(usbtmc_state.ep_bulk_in_buf); + usbtmc_state.transfer_size_remaining -= sizeof(usbtmc_state.ep_bulk_in_buf); + usbtmc_state.transfer_size_sent += sizeof(usbtmc_state.ep_bulk_in_buf); + } + else // last packet + { + size_t packetLen = usbtmc_state.transfer_size_remaining; + memcpy(usbtmc_state.ep_bulk_in_buf, usbtmc_state.devInBuffer, usbtmc_state.transfer_size_remaining); + usbtmc_state.transfer_size_sent += sizeof(usbtmc_state.transfer_size_remaining); + usbtmc_state.transfer_size_remaining = 0; + usbtmc_state.devInBuffer = NULL; + TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf, (uint16_t)packetLen) ); + if(((packetLen % USBTMCD_MAX_PACKET_SIZE) != 0) || (packetLen == 0 )) + { + usbtmc_state.state = STATE_TX_SHORTED; + } + } + return true; + + case STATE_ABORTING_BULK_IN: + // need to send short packet (ZLP?) + TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf,(uint16_t)0u)); + usbtmc_state.state = STATE_ABORTING_BULK_IN_SHORTED; + return true; + + case STATE_ABORTING_BULK_IN_SHORTED: + /* Done. :)*/ + usbtmc_state.state = STATE_ABORTING_BULK_IN_ABORTED; + return true; + + default: + TU_ASSERT(false); + return false; + } + } + else if (ep_addr == usbtmc_state.ep_int_in) { + // Good? + return true; + } + return false; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + // nothing to do with DATA and ACK stage + if ( stage != CONTROL_STAGE_SETUP ) return true; + + uint8_t tmcStatusCode = USBTMC_STATUS_FAILED; +#if (CFG_TUD_USBTMC_ENABLE_488) + uint8_t bTag; +#endif + + if((request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) && + (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_ENDPOINT) && + (request->bRequest == TUSB_REQ_CLEAR_FEATURE) && + (request->wValue == TUSB_REQ_FEATURE_EDPT_HALT)) + { + uint32_t ep_addr = (request->wIndex); + + if(ep_addr == usbtmc_state.ep_bulk_out) + { + criticalEnter(); + usbtmc_state.state = STATE_NAK; // USBD core has placed EP in NAK state for us + criticalLeave(); + tud_usbtmc_bulkOut_clearFeature_cb(); + } + else if (ep_addr == usbtmc_state.ep_bulk_in) + { + tud_usbtmc_bulkIn_clearFeature_cb(); + } + else + { + return false; + } + return true; + } + + // Otherwise, we only handle class requests. + if(request->bmRequestType_bit.type != TUSB_REQ_TYPE_CLASS) + { + return false; + } + + // Verification that we own the interface is unneeded since it's been routed to us specifically. + + switch(request->bRequest) + { + // USBTMC required requests + case USBTMC_bREQUEST_INITIATE_ABORT_BULK_OUT: + { + usbtmc_initiate_abort_rsp_t rsp = { + .bTag = usbtmc_state.lastBulkOutTag, + }; + TU_VERIFY(request->bmRequestType == 0xA2); // in,class,interface + TU_VERIFY(request->wLength == sizeof(rsp)); + TU_VERIFY(request->wIndex == usbtmc_state.ep_bulk_out); + + // wValue is the requested bTag to abort + if(usbtmc_state.state != STATE_RCV) + { + rsp.USBTMC_status = USBTMC_STATUS_FAILED; + } + else if(usbtmc_state.lastBulkOutTag == (request->wValue & 0x7Fu)) + { + rsp.USBTMC_status = USBTMC_STATUS_TRANSFER_NOT_IN_PROGRESS; + } + else + { + rsp.USBTMC_status = USBTMC_STATUS_SUCCESS; + // Check if we've queued a short packet + criticalEnter(); + usbtmc_state.state = STATE_ABORTING_BULK_OUT; + criticalLeave(); + TU_VERIFY(tud_usbtmc_initiate_abort_bulk_out_cb(&(rsp.USBTMC_status))); + usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out); + } + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&rsp,sizeof(rsp))); + return true; + } + + case USBTMC_bREQUEST_CHECK_ABORT_BULK_OUT_STATUS: + { + usbtmc_check_abort_bulk_rsp_t rsp = { + .USBTMC_status = USBTMC_STATUS_SUCCESS, + .NBYTES_RXD_TXD = usbtmc_state.transfer_size_sent + }; + TU_VERIFY(request->bmRequestType == 0xA2); // in,class,EP + TU_VERIFY(request->wLength == sizeof(rsp)); + TU_VERIFY(request->wIndex == usbtmc_state.ep_bulk_out); + TU_VERIFY(tud_usbtmc_check_abort_bulk_out_cb(&rsp)); + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&rsp,sizeof(rsp))); + return true; + } + + case USBTMC_bREQUEST_INITIATE_ABORT_BULK_IN: + { + usbtmc_initiate_abort_rsp_t rsp = { + .bTag = usbtmc_state.lastBulkInTag, + }; + TU_VERIFY(request->bmRequestType == 0xA2); // in,class,interface + TU_VERIFY(request->wLength == sizeof(rsp)); + TU_VERIFY(request->wIndex == usbtmc_state.ep_bulk_in); + // wValue is the requested bTag to abort + if((usbtmc_state.state == STATE_TX_REQUESTED || usbtmc_state.state == STATE_TX_INITIATED) && + usbtmc_state.lastBulkInTag == (request->wValue & 0x7Fu)) + { + rsp.USBTMC_status = USBTMC_STATUS_SUCCESS; + usbtmc_state.transfer_size_remaining = 0u; + // Check if we've queued a short packet + criticalEnter(); + usbtmc_state.state = ((usbtmc_state.transfer_size_sent % USBTMCD_MAX_PACKET_SIZE) == 0) ? + STATE_ABORTING_BULK_IN : STATE_ABORTING_BULK_IN_SHORTED; + criticalLeave(); + if(usbtmc_state.transfer_size_sent == 0) + { + // Send short packet, nothing is in the buffer yet + TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf,(uint16_t)0u)); + usbtmc_state.state = STATE_ABORTING_BULK_IN_SHORTED; + } + TU_VERIFY(tud_usbtmc_initiate_abort_bulk_in_cb(&(rsp.USBTMC_status))); + } + else if((usbtmc_state.state == STATE_TX_REQUESTED || usbtmc_state.state == STATE_TX_INITIATED)) + { // FIXME: Unsure how to check if the OUT endpoint fifo is non-empty.... + rsp.USBTMC_status = USBTMC_STATUS_TRANSFER_NOT_IN_PROGRESS; + } + else + { + rsp.USBTMC_status = USBTMC_STATUS_FAILED; + } + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&rsp,sizeof(rsp))); + return true; + } + + case USBTMC_bREQUEST_CHECK_ABORT_BULK_IN_STATUS: + { + TU_VERIFY(request->bmRequestType == 0xA2); // in,class,EP + TU_VERIFY(request->wLength == 8u); + + usbtmc_check_abort_bulk_rsp_t rsp = + { + .USBTMC_status = USBTMC_STATUS_FAILED, + .bmAbortBulkIn = + { + .BulkInFifoBytes = (usbtmc_state.state != STATE_ABORTING_BULK_IN_ABORTED) + }, + .NBYTES_RXD_TXD = usbtmc_state.transfer_size_sent, + }; + TU_VERIFY(tud_usbtmc_check_abort_bulk_in_cb(&rsp)); + criticalEnter(); + switch(usbtmc_state.state) + { + case STATE_ABORTING_BULK_IN_ABORTED: + rsp.USBTMC_status = USBTMC_STATUS_SUCCESS; + usbtmc_state.state = STATE_IDLE; + break; + case STATE_ABORTING_BULK_IN: + case STATE_ABORTING_BULK_OUT: + rsp.USBTMC_status = USBTMC_STATUS_PENDING; + break; + default: + break; + } + criticalLeave(); + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&rsp,sizeof(rsp))); + + return true; + } + + case USBTMC_bREQUEST_INITIATE_CLEAR: + { + TU_VERIFY(request->bmRequestType == 0xA1); // in,class,interface + TU_VERIFY(request->wLength == sizeof(tmcStatusCode)); + // After receiving an INITIATE_CLEAR request, the device must Halt the Bulk-OUT endpoint, queue the + // control endpoint response shown in Table 31, and clear all input buffers and output buffers. + usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out); + usbtmc_state.transfer_size_remaining = 0; + criticalEnter(); + usbtmc_state.state = STATE_CLEARING; + criticalLeave(); + TU_VERIFY(tud_usbtmc_initiate_clear_cb(&tmcStatusCode)); + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&tmcStatusCode,sizeof(tmcStatusCode))); + return true; + } + + case USBTMC_bREQUEST_CHECK_CLEAR_STATUS: + { + TU_VERIFY(request->bmRequestType == 0xA1); // in,class,interface + usbtmc_get_clear_status_rsp_t clearStatusRsp = {0}; + TU_VERIFY(request->wLength == sizeof(clearStatusRsp)); + + if(usbd_edpt_busy(rhport, usbtmc_state.ep_bulk_in)) + { + // Stuff stuck in TX buffer? + clearStatusRsp.bmClear.BulkInFifoBytes = 1; + clearStatusRsp.USBTMC_status = USBTMC_STATUS_PENDING; + } + else + { + // Let app check if it's clear + TU_VERIFY(tud_usbtmc_check_clear_cb(&clearStatusRsp)); + } + if(clearStatusRsp.USBTMC_status == USBTMC_STATUS_SUCCESS) + { + criticalEnter(); + usbtmc_state.state = STATE_IDLE; + criticalLeave(); + } + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&clearStatusRsp,sizeof(clearStatusRsp))); + return true; + } + + case USBTMC_bREQUEST_GET_CAPABILITIES: + { + TU_VERIFY(request->bmRequestType == 0xA1); // in,class,interface + TU_VERIFY(request->wLength == sizeof(*(usbtmc_state.capabilities))); + TU_VERIFY(tud_control_xfer(rhport, request, (void*)(uintptr_t) usbtmc_state.capabilities, sizeof(*usbtmc_state.capabilities))); + return true; + } + // USBTMC Optional Requests + + case USBTMC_bREQUEST_INDICATOR_PULSE: // Optional + { + TU_VERIFY(request->bmRequestType == 0xA1); // in,class,interface + TU_VERIFY(request->wLength == sizeof(tmcStatusCode)); + TU_VERIFY(usbtmc_state.capabilities->bmIntfcCapabilities.supportsIndicatorPulse); + TU_VERIFY(tud_usbtmc_indicator_pulse_cb(request, &tmcStatusCode)); + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&tmcStatusCode, sizeof(tmcStatusCode))); + return true; + } +#if (CFG_TUD_USBTMC_ENABLE_488) + + // USB488 required requests + case USB488_bREQUEST_READ_STATUS_BYTE: + { + usbtmc_read_stb_rsp_488_t rsp; + TU_VERIFY(request->bmRequestType == 0xA1); // in,class,interface + TU_VERIFY(request->wLength == sizeof(rsp)); // in,class,interface + + bTag = request->wValue & 0x7F; + TU_VERIFY(request->bmRequestType == 0xA1); + TU_VERIFY((request->wValue & (~0x7F)) == 0u); // Other bits are required to be zero + TU_VERIFY(bTag >= 0x02 && bTag <= 127); + TU_VERIFY(request->wIndex == usbtmc_state.itf_id); + TU_VERIFY(request->wLength == 0x0003); + rsp.bTag = (uint8_t)bTag; + if(usbtmc_state.ep_int_in != 0) + { + rsp.USBTMC_status = USBTMC_STATUS_SUCCESS; + rsp.statusByte = 0x00; // Use interrupt endpoint, instead. + + usbtmc_read_stb_interrupt_488_t intMsg = + { + .bNotify1 = { + .one = 1, + .bTag = bTag & 0x7Fu, + }, + .StatusByte = tud_usbtmc_get_stb_cb(&(rsp.USBTMC_status)) + }; + usbd_edpt_xfer(rhport, usbtmc_state.ep_int_in, (void*)&intMsg, sizeof(intMsg)); + } + else + { + rsp.statusByte = tud_usbtmc_get_stb_cb(&(rsp.USBTMC_status)); + } + TU_VERIFY(tud_control_xfer(rhport, request, (void*)&rsp, sizeof(rsp))); + return true; + } + // USB488 optional requests + case USB488_bREQUEST_REN_CONTROL: + case USB488_bREQUEST_GO_TO_LOCAL: + case USB488_bREQUEST_LOCAL_LOCKOUT: + { + TU_VERIFY(request->bmRequestType == 0xA1); // in,class,interface + TU_VERIFY(false); + return false; + } +#endif + + default: + TU_VERIFY(false); + return false; + } + TU_VERIFY(false); +} + +#endif /* CFG_TUD_TSMC */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc_device.h new file mode 100644 index 000000000..0549a1569 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/usbtmc/usbtmc_device.h @@ -0,0 +1,116 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 N Conrad + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + + +#ifndef CLASS_USBTMC_USBTMC_DEVICE_H_ +#define CLASS_USBTMC_USBTMC_DEVICE_H_ + +#include "usbtmc.h" + +// Enable 488 mode by default +#if !defined(CFG_TUD_USBTMC_ENABLE_488) +#define CFG_TUD_USBTMC_ENABLE_488 (1) +#endif + +// USB spec says that full-speed must be 8,16,32, or 64. +// However, this driver implementation requires it to be >=32 +#define USBTMCD_MAX_PACKET_SIZE (64u) + +/*********************************************** + * Functions to be implemeted by the class implementation + */ + +// In order to proceed, app must call call tud_usbtmc_start_bus_read(rhport) during or soon after: +// * tud_usbtmc_open_cb +// * tud_usbtmc_msg_data_cb +// * tud_usbtmc_msgBulkIn_complete_cb +// * tud_usbtmc_msg_trigger_cb +// * (successful) tud_usbtmc_check_abort_bulk_out_cb +// * (successful) tud_usbtmc_check_abort_bulk_in_cb +// * (successful) tud_usmtmc_bulkOut_clearFeature_cb + +#if (CFG_TUD_USBTMC_ENABLE_488) +usbtmc_response_capabilities_488_t const * tud_usbtmc_get_capabilities_cb(void); +#else +usbtmc_response_capabilities_t const * tud_usbtmc_get_capabilities_cb(void); +#endif + +void tud_usbtmc_open_cb(uint8_t interface_id); + +bool tud_usbtmc_msgBulkOut_start_cb(usbtmc_msg_request_dev_dep_out const * msgHeader); +// transfer_complete does not imply that a message is complete. +bool tud_usbtmc_msg_data_cb( void *data, size_t len, bool transfer_complete); +void tud_usbtmc_bulkOut_clearFeature_cb(void); // Notice to clear and abort the pending BULK out transfer + +bool tud_usbtmc_msgBulkIn_request_cb(usbtmc_msg_request_dev_dep_in const * request); +bool tud_usbtmc_msgBulkIn_complete_cb(void); +void tud_usbtmc_bulkIn_clearFeature_cb(void); // Notice to clear and abort the pending BULK out transfer + +bool tud_usbtmc_initiate_abort_bulk_in_cb(uint8_t *tmcResult); +bool tud_usbtmc_initiate_abort_bulk_out_cb(uint8_t *tmcResult); +bool tud_usbtmc_initiate_clear_cb(uint8_t *tmcResult); + +bool tud_usbtmc_check_abort_bulk_in_cb(usbtmc_check_abort_bulk_rsp_t *rsp); +bool tud_usbtmc_check_abort_bulk_out_cb(usbtmc_check_abort_bulk_rsp_t *rsp); +bool tud_usbtmc_check_clear_cb(usbtmc_get_clear_status_rsp_t *rsp); + +// Indicator pulse should be 0.5 to 1.0 seconds long +TU_ATTR_WEAK bool tud_usbtmc_indicator_pulse_cb(tusb_control_request_t const * msg, uint8_t *tmcResult); + +#if (CFG_TUD_USBTMC_ENABLE_488) +uint8_t tud_usbtmc_get_stb_cb(uint8_t *tmcResult); +TU_ATTR_WEAK bool tud_usbtmc_msg_trigger_cb(usbtmc_msg_generic_t* msg); +//TU_ATTR_WEAK bool tud_usbtmc_app_go_to_local_cb(); +#endif + +/******************************************* + * Called from app + * + * We keep a reference to the buffer, so it MUST not change until the app is + * notified that the transfer is complete. + ******************************************/ + +bool tud_usbtmc_transmit_dev_msg_data( + const void * data, size_t len, + bool endOfMessage, bool usingTermChar); + +bool tud_usbtmc_start_bus_read(void); + + +/* "callbacks" from USB device core */ + +uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +void usbtmcd_reset_cb(uint8_t rhport); +bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); +bool usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +void usbtmcd_init_cb(void); + +/************************************************************ + * USBTMC Descriptor Templates + *************************************************************/ + + +#endif /* CLASS_USBTMC_USBTMC_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_device.c new file mode 100644 index 000000000..8a4ca1d2e --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_device.c @@ -0,0 +1,257 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_VENDOR) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "vendor_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef struct +{ + uint8_t itf_num; + uint8_t ep_in; + uint8_t ep_out; + + /*------------- From this point, data is not cleared by bus reset -------------*/ + tu_fifo_t rx_ff; + tu_fifo_t tx_ff; + + uint8_t rx_ff_buf[CFG_TUD_VENDOR_RX_BUFSIZE]; + uint8_t tx_ff_buf[CFG_TUD_VENDOR_TX_BUFSIZE]; + +#if CFG_FIFO_MUTEX + osal_mutex_def_t rx_ff_mutex; + osal_mutex_def_t tx_ff_mutex; +#endif + + // Endpoint Transfer buffer + CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_VENDOR_EPSIZE]; + CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_VENDOR_EPSIZE]; +} vendord_interface_t; + +CFG_TUSB_MEM_SECTION static vendord_interface_t _vendord_itf[CFG_TUD_VENDOR]; + +#define ITF_MEM_RESET_SIZE offsetof(vendord_interface_t, rx_ff) + + +bool tud_vendor_n_mounted (uint8_t itf) +{ + return _vendord_itf[itf].ep_in && _vendord_itf[itf].ep_out; +} + +uint32_t tud_vendor_n_available (uint8_t itf) +{ + return tu_fifo_count(&_vendord_itf[itf].rx_ff); +} + +bool tud_vendor_n_peek(uint8_t itf, uint8_t* u8) +{ + return tu_fifo_peek(&_vendord_itf[itf].rx_ff, u8); +} + +//--------------------------------------------------------------------+ +// Read API +//--------------------------------------------------------------------+ +static void _prep_out_transaction (vendord_interface_t* p_itf) +{ + // skip if previous transfer not complete + if ( usbd_edpt_busy(TUD_OPT_RHPORT, p_itf->ep_out) ) return; + + // Prepare for incoming data but only allow what we can store in the ring buffer. + uint16_t max_read = tu_fifo_remaining(&p_itf->rx_ff); + if ( max_read >= CFG_TUD_VENDOR_EPSIZE ) + { + usbd_edpt_xfer(TUD_OPT_RHPORT, p_itf->ep_out, p_itf->epout_buf, CFG_TUD_VENDOR_EPSIZE); + } +} + +uint32_t tud_vendor_n_read (uint8_t itf, void* buffer, uint32_t bufsize) +{ + vendord_interface_t* p_itf = &_vendord_itf[itf]; + uint32_t num_read = tu_fifo_read_n(&p_itf->rx_ff, buffer, bufsize); + _prep_out_transaction(p_itf); + return num_read; +} + +void tud_vendor_n_read_flush (uint8_t itf) +{ + vendord_interface_t* p_itf = &_vendord_itf[itf]; + tu_fifo_clear(&p_itf->rx_ff); + _prep_out_transaction(p_itf); +} + +//--------------------------------------------------------------------+ +// Write API +//--------------------------------------------------------------------+ +static bool maybe_transmit(vendord_interface_t* p_itf) +{ + // skip if previous transfer not complete + TU_VERIFY( !usbd_edpt_busy(TUD_OPT_RHPORT, p_itf->ep_in) ); + + uint16_t count = tu_fifo_read_n(&p_itf->tx_ff, p_itf->epin_buf, CFG_TUD_VENDOR_EPSIZE); + if (count > 0) + { + TU_ASSERT( usbd_edpt_xfer(TUD_OPT_RHPORT, p_itf->ep_in, p_itf->epin_buf, count) ); + } + return true; +} + +uint32_t tud_vendor_n_write (uint8_t itf, void const* buffer, uint32_t bufsize) +{ + vendord_interface_t* p_itf = &_vendord_itf[itf]; + uint16_t ret = tu_fifo_write_n(&p_itf->tx_ff, buffer, bufsize); + maybe_transmit(p_itf); + return ret; +} + +uint32_t tud_vendor_n_write_available (uint8_t itf) +{ + return tu_fifo_remaining(&_vendord_itf[itf].tx_ff); +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void vendord_init(void) +{ + tu_memclr(_vendord_itf, sizeof(_vendord_itf)); + + for(uint8_t i=0; irx_ff, p_itf->rx_ff_buf, CFG_TUD_VENDOR_RX_BUFSIZE, 1, false); + tu_fifo_config(&p_itf->tx_ff, p_itf->tx_ff_buf, CFG_TUD_VENDOR_TX_BUFSIZE, 1, false); + +#if CFG_FIFO_MUTEX + tu_fifo_config_mutex(&p_itf->rx_ff, NULL, osal_mutex_create(&p_itf->rx_ff_mutex)); + tu_fifo_config_mutex(&p_itf->tx_ff, osal_mutex_create(&p_itf->tx_ff_mutex), NULL); +#endif + } +} + +void vendord_reset(uint8_t rhport) +{ + (void) rhport; + + for(uint8_t i=0; irx_ff); + tu_fifo_clear(&p_itf->tx_ff); + } +} + +uint16_t vendord_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint16_t max_len) +{ + TU_VERIFY(TUSB_CLASS_VENDOR_SPECIFIC == desc_itf->bInterfaceClass, 0); + + uint8_t const * p_desc = tu_desc_next(desc_itf); + uint8_t const * desc_end = p_desc + max_len; + + // Find available interface + vendord_interface_t* p_vendor = NULL; + for(uint8_t i=0; iitf_num = desc_itf->bInterfaceNumber; + if (desc_itf->bNumEndpoints) + { + // skip non-endpoint descriptors + while ( (TUSB_DESC_ENDPOINT != tu_desc_type(p_desc)) && (p_desc < desc_end) ) + { + p_desc = tu_desc_next(p_desc); + } + + // Open endpoint pair with usbd helper + TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, desc_itf->bNumEndpoints, TUSB_XFER_BULK, &p_vendor->ep_out, &p_vendor->ep_in), 0); + + p_desc += desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t); + + // Prepare for incoming data + if ( p_vendor->ep_out ) + { + TU_ASSERT(usbd_edpt_xfer(rhport, p_vendor->ep_out, p_vendor->epout_buf, sizeof(p_vendor->epout_buf)), 0); + } + + if ( p_vendor->ep_in ) maybe_transmit(p_vendor); + } + + return (uintptr_t) p_desc - (uintptr_t) desc_itf; +} + +bool vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) rhport; + (void) result; + + uint8_t itf = 0; + vendord_interface_t* p_itf = _vendord_itf; + + for ( ; ; itf++, p_itf++) + { + if (itf >= TU_ARRAY_SIZE(_vendord_itf)) return false; + + if ( ( ep_addr == p_itf->ep_out ) || ( ep_addr == p_itf->ep_in ) ) break; + } + + if ( ep_addr == p_itf->ep_out ) + { + // Receive new data + tu_fifo_write_n(&p_itf->rx_ff, p_itf->epout_buf, xferred_bytes); + + // Invoked callback if any + if (tud_vendor_rx_cb) tud_vendor_rx_cb(itf); + + _prep_out_transaction(p_itf); + } + else if ( ep_addr == p_itf->ep_in ) + { + // Send complete, try to send more if possible + maybe_transmit(p_itf); + } + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_device.h new file mode 100644 index 000000000..d71c2a3e9 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_device.h @@ -0,0 +1,136 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_VENDOR_DEVICE_H_ +#define _TUSB_VENDOR_DEVICE_H_ + +#include "common/tusb_common.h" + +#ifndef CFG_TUD_VENDOR_EPSIZE +#define CFG_TUD_VENDOR_EPSIZE 64 +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Application API (Multiple Interfaces) +//--------------------------------------------------------------------+ +bool tud_vendor_n_mounted (uint8_t itf); + +uint32_t tud_vendor_n_available (uint8_t itf); +uint32_t tud_vendor_n_read (uint8_t itf, void* buffer, uint32_t bufsize); +bool tud_vendor_n_peek (uint8_t itf, uint8_t* ui8); +void tud_vendor_n_read_flush (uint8_t itf); + +uint32_t tud_vendor_n_write (uint8_t itf, void const* buffer, uint32_t bufsize); +uint32_t tud_vendor_n_write_available (uint8_t itf); + +static inline +uint32_t tud_vendor_n_write_str (uint8_t itf, char const* str); + +//--------------------------------------------------------------------+ +// Application API (Single Port) +//--------------------------------------------------------------------+ +static inline bool tud_vendor_mounted (void); +static inline uint32_t tud_vendor_available (void); +static inline uint32_t tud_vendor_read (void* buffer, uint32_t bufsize); +static inline bool tud_vendor_peek (uint8_t* ui8); +static inline void tud_vendor_read_flush (void); +static inline uint32_t tud_vendor_write (void const* buffer, uint32_t bufsize); +static inline uint32_t tud_vendor_write_str (char const* str); +static inline uint32_t tud_vendor_write_available (void); + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ + +// Invoked when received new data +TU_ATTR_WEAK void tud_vendor_rx_cb(uint8_t itf); + +//--------------------------------------------------------------------+ +// Inline Functions +//--------------------------------------------------------------------+ + +static inline uint32_t tud_vendor_n_write_str (uint8_t itf, char const* str) +{ + return tud_vendor_n_write(itf, str, strlen(str)); +} + +static inline bool tud_vendor_mounted (void) +{ + return tud_vendor_n_mounted(0); +} + +static inline uint32_t tud_vendor_available (void) +{ + return tud_vendor_n_available(0); +} + +static inline uint32_t tud_vendor_read (void* buffer, uint32_t bufsize) +{ + return tud_vendor_n_read(0, buffer, bufsize); +} + +static inline bool tud_vendor_peek (uint8_t* ui8) +{ + return tud_vendor_n_peek(0, ui8); +} + +static inline void tud_vendor_read_flush(void) +{ + tud_vendor_n_read_flush(0); +} + +static inline uint32_t tud_vendor_write (void const* buffer, uint32_t bufsize) +{ + return tud_vendor_n_write(0, buffer, bufsize); +} + +static inline uint32_t tud_vendor_write_str (char const* str) +{ + return tud_vendor_n_write_str(0, str); +} + +static inline uint32_t tud_vendor_write_available (void) +{ + return tud_vendor_n_write_available(0); +} + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void vendord_init(void); +void vendord_reset(uint8_t rhport); +uint16_t vendord_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_VENDOR_DEVICE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_host.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_host.c new file mode 100644 index 000000000..1e28e9af4 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_host.c @@ -0,0 +1,146 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_HOST_ENABLED && CFG_TUH_VENDOR) + +//--------------------------------------------------------------------+ +// INCLUDE +//--------------------------------------------------------------------+ +#include "host/usbh.h" +#include "vendor_host.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +custom_interface_info_t custom_interface[CFG_TUH_DEVICE_MAX]; + +static tusb_error_t cush_validate_paras(uint8_t dev_addr, uint16_t vendor_id, uint16_t product_id, void * p_buffer, uint16_t length) +{ + if ( !tusbh_custom_is_mounted(dev_addr, vendor_id, product_id) ) + { + return TUSB_ERROR_DEVICE_NOT_READY; + } + + TU_ASSERT( p_buffer != NULL && length != 0, TUSB_ERROR_INVALID_PARA); + + return TUSB_ERROR_NONE; +} +//--------------------------------------------------------------------+ +// APPLICATION API (need to check parameters) +//--------------------------------------------------------------------+ +tusb_error_t tusbh_custom_read(uint8_t dev_addr, uint16_t vendor_id, uint16_t product_id, void * p_buffer, uint16_t length) +{ + TU_ASSERT_ERR( cush_validate_paras(dev_addr, vendor_id, product_id, p_buffer, length) ); + + if ( !hcd_pipe_is_idle(custom_interface[dev_addr-1].pipe_in) ) + { + return TUSB_ERROR_INTERFACE_IS_BUSY; + } + + (void) usbh_edpt_xfer( custom_interface[dev_addr-1].pipe_in, p_buffer, length); + + return TUSB_ERROR_NONE; +} + +tusb_error_t tusbh_custom_write(uint8_t dev_addr, uint16_t vendor_id, uint16_t product_id, void const * p_data, uint16_t length) +{ + TU_ASSERT_ERR( cush_validate_paras(dev_addr, vendor_id, product_id, p_data, length) ); + + if ( !hcd_pipe_is_idle(custom_interface[dev_addr-1].pipe_out) ) + { + return TUSB_ERROR_INTERFACE_IS_BUSY; + } + + (void) usbh_edpt_xfer( custom_interface[dev_addr-1].pipe_out, p_data, length); + + return TUSB_ERROR_NONE; +} + +//--------------------------------------------------------------------+ +// USBH-CLASS API +//--------------------------------------------------------------------+ +void cush_init(void) +{ + tu_memclr(&custom_interface, sizeof(custom_interface_info_t) * CFG_TUH_DEVICE_MAX); +} + +tusb_error_t cush_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_interface_desc, uint16_t *p_length) +{ + // FIXME quick hack to test lpc1k custom class with 2 bulk endpoints + uint8_t const *p_desc = (uint8_t const *) p_interface_desc; + p_desc = tu_desc_next(p_desc); + + //------------- Bulk Endpoints Descriptor -------------// + for(uint32_t i=0; i<2; i++) + { + tusb_desc_endpoint_t const *p_endpoint = (tusb_desc_endpoint_t const *) p_desc; + TU_ASSERT(TUSB_DESC_ENDPOINT == p_endpoint->bDescriptorType, TUSB_ERROR_INVALID_PARA); + + pipe_handle_t * p_pipe_hdl = ( p_endpoint->bEndpointAddress & TUSB_DIR_IN_MASK ) ? + &custom_interface[dev_addr-1].pipe_in : &custom_interface[dev_addr-1].pipe_out; + *p_pipe_hdl = usbh_edpt_open(dev_addr, p_endpoint, TUSB_CLASS_VENDOR_SPECIFIC); + TU_ASSERT ( pipehandle_is_valid(*p_pipe_hdl), TUSB_ERROR_HCD_OPEN_PIPE_FAILED ); + + p_desc = tu_desc_next(p_desc); + } + + (*p_length) = sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t); + return TUSB_ERROR_NONE; +} + +void cush_isr(pipe_handle_t pipe_hdl, xfer_result_t event) +{ + +} + +void cush_close(uint8_t dev_addr) +{ + tusb_error_t err1, err2; + custom_interface_info_t * p_interface = &custom_interface[dev_addr-1]; + + // TODO re-consider to check pipe valid before calling pipe_close + if( pipehandle_is_valid( p_interface->pipe_in ) ) + { + err1 = hcd_pipe_close( p_interface->pipe_in ); + } + + if ( pipehandle_is_valid( p_interface->pipe_out ) ) + { + err2 = hcd_pipe_close( p_interface->pipe_out ); + } + + tu_memclr(p_interface, sizeof(custom_interface_info_t)); + + TU_ASSERT(err1 == TUSB_ERROR_NONE && err2 == TUSB_ERROR_NONE, (void) 0 ); +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_host.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_host.h new file mode 100644 index 000000000..07fa56c61 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/vendor/vendor_host.h @@ -0,0 +1,67 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_VENDOR_HOST_H_ +#define _TUSB_VENDOR_HOST_H_ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +typedef struct { + pipe_handle_t pipe_in; + pipe_handle_t pipe_out; +}custom_interface_info_t; + +//--------------------------------------------------------------------+ +// USBH-CLASS DRIVER API +//--------------------------------------------------------------------+ +static inline bool tusbh_custom_is_mounted(uint8_t dev_addr, uint16_t vendor_id, uint16_t product_id) +{ + (void) vendor_id; // TODO check this later + (void) product_id; +// return (tusbh_device_get_mounted_class_flag(dev_addr) & TU_BIT(TUSB_CLASS_MAPPED_INDEX_END-1) ) != 0; + return false; +} + +tusb_error_t tusbh_custom_read(uint8_t dev_addr, uint16_t vendor_id, uint16_t product_id, void * p_buffer, uint16_t length); +tusb_error_t tusbh_custom_write(uint8_t dev_addr, uint16_t vendor_id, uint16_t product_id, void const * p_data, uint16_t length); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void cush_init(void); +tusb_error_t cush_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_interface_desc, uint16_t *p_length); +void cush_isr(pipe_handle_t pipe_hdl, xfer_result_t event); +void cush_close(uint8_t dev_addr); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_VENDOR_HOST_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video.h new file mode 100644 index 000000000..844746546 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video.h @@ -0,0 +1,480 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021 Koji KITAYAMA + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef TUSB_VIDEO_H_ +#define TUSB_VIDEO_H_ + +#include "common/tusb_common.h" + +// Table 3-19 Color Matching Descriptor +typedef enum { + VIDEO_COLOR_PRIMARIES_UNDEFINED = 0x00, + VIDEO_COLOR_PRIMARIES_BT709, // sRGB (default) + VIDEO_COLOR_PRIMARIES_BT470_2M, + VIDEO_COLOR_PRIMARIES_BT470_2BG, + VIDEO_COLOR_PRIMARIES_SMPTE170M, + VIDEO_COLOR_PRIMARIES_SMPTE240M, +} video_color_primaries_t; + +// Table 3-19 Color Matching Descriptor +typedef enum { + VIDEO_COLOR_XFER_CH_UNDEFINED = 0x00, + VIDEO_COLOR_XFER_CH_BT709, // default + VIDEO_COLOR_XFER_CH_BT470_2M, + VIDEO_COLOR_XFER_CH_BT470_2BG, + VIDEO_COLOR_XFER_CH_SMPTE170M, + VIDEO_COLOR_XFER_CH_SMPTE240M, + VIDEO_COLOR_XFER_CH_LINEAR, + VIDEO_COLOR_XFER_CH_SRGB, +} video_color_transfer_characteristics_t; + +// Table 3-19 Color Matching Descriptor +typedef enum { + VIDEO_COLOR_COEF_UNDEFINED = 0x00, + VIDEO_COLOR_COEF_BT709, + VIDEO_COLOR_COEF_FCC, + VIDEO_COLOR_COEF_BT470_2BG, + VIDEO_COLOR_COEF_SMPTE170M, // BT.601 default + VIDEO_COLOR_COEF_SMPTE240M, +} video_color_matrix_coefficients_t; + +/* 4.2.1.2 Request Error Code Control */ +typedef enum { + VIDEO_ERROR_NONE = 0, /* The request succeeded. */ + VIDEO_ERROR_NOT_READY, + VIDEO_ERROR_WRONG_STATE, + VIDEO_ERROR_POWER, + VIDEO_ERROR_OUT_OF_RANGE, + VIDEO_ERROR_INVALID_UNIT, + VIDEO_ERROR_INVALID_CONTROL, + VIDEO_ERROR_INVALID_REQUEST, + VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE, + VIDEO_ERROR_UNKNOWN = 0xFF, +} video_error_code_t; + +/* A.2 Interface Subclass */ +typedef enum { + VIDEO_SUBCLASS_UNDEFINED = 0x00, + VIDEO_SUBCLASS_CONTROL, + VIDEO_SUBCLASS_STREAMING, + VIDEO_SUBCLASS_INTERFACE_COLLECTION, +} video_subclass_type_t; + +/* A.3 Interface Protocol */ +typedef enum { + VIDEO_ITF_PROTOCOL_UNDEFINED = 0x00, + VIDEO_ITF_PROTOCOL_15, +} video_interface_protocol_code_t; + +/* A.5 Class-Specific VideoControl Interface Descriptor Subtypes */ +typedef enum { + VIDEO_CS_ITF_VC_UNDEFINED = 0x00, + VIDEO_CS_ITF_VC_HEADER, + VIDEO_CS_ITF_VC_INPUT_TERMINAL, + VIDEO_CS_ITF_VC_OUTPUT_TERMINAL, + VIDEO_CS_ITF_VC_SELECTOR_UNIT, + VIDEO_CS_ITF_VC_PROCESSING_UNIT, + VIDEO_CS_ITF_VC_EXTENSION_UNIT, + VIDEO_CS_ITF_VC_ENCODING_UNIT, + VIDEO_CS_ITF_VC_MAX, +} video_cs_vc_interface_subtype_t; + +/* A.6 Class-Specific VideoStreaming Interface Descriptor Subtypes */ +typedef enum { + VIDEO_CS_ITF_VS_UNDEFINED = 0x00, + VIDEO_CS_ITF_VS_INPUT_HEADER = 0x01, + VIDEO_CS_ITF_VS_OUTPUT_HEADER = 0x02, + VIDEO_CS_ITF_VS_STILL_IMAGE_FRAME = 0x03, + VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED = 0x04, + VIDEO_CS_ITF_VS_FRAME_UNCOMPRESSED = 0x05, + VIDEO_CS_ITF_VS_FORMAT_MJPEG = 0x06, + VIDEO_CS_ITF_VS_FRAME_MJPEG = 0x07, + VIDEO_CS_ITF_VS_FORMAT_MPEG2TS = 0x0A, + VIDEO_CS_ITF_VS_FORMAT_DV = 0x0C, + VIDEO_CS_ITF_VS_COLORFORMAT = 0x0D, + VIDEO_CS_ITF_VS_FORMAT_FRAME_BASED = 0x10, + VIDEO_CS_ITF_VS_FRAME_FRAME_BASED = 0x11, + VIDEO_CS_ITF_VS_FORMAT_STREAM_BASED = 0x12, + VIDEO_CS_ITF_VS_FORMAT_H264 = 0x13, + VIDEO_CS_ITF_VS_FRAME_H264 = 0x14, + VIDEO_CS_ITF_VS_FORMAT_H264_SIMULCAST = 0x15, + VIDEO_CS_ITF_VS_FORMAT_VP8 = 0x16, + VIDEO_CS_ITF_VS_FRAME_VP8 = 0x17, + VIDEO_CS_ITF_VS_FORMAT_VP8_SIMULCAST = 0x18, +} video_cs_vs_interface_subtype_t; + +/* A.7. Class-Specific Endpoint Descriptor Subtypes */ +typedef enum { + VIDEO_CS_EP_UNDEFINED = 0x00, + VIDEO_CS_EP_GENERAL, + VIDEO_CS_EP_ENDPOINT, + VIDEO_CS_EP_INTERRUPT +} video_cs_ep_subtype_t; + +/* A.8 Class-Specific Request Codes */ +typedef enum { + VIDEO_REQUEST_UNDEFINED = 0x00, + VIDEO_REQUEST_SET_CUR = 0x01, + VIDEO_REQUEST_SET_CUR_ALL = 0x11, + VIDEO_REQUEST_GET_CUR = 0x81, + VIDEO_REQUEST_GET_MIN = 0x82, + VIDEO_REQUEST_GET_MAX = 0x83, + VIDEO_REQUEST_GET_RES = 0x84, + VIDEO_REQUEST_GET_LEN = 0x85, + VIDEO_REQUEST_GET_INFO = 0x86, + VIDEO_REQUEST_GET_DEF = 0x87, + VIDEO_REQUEST_GET_CUR_ALL = 0x91, + VIDEO_REQUEST_GET_MIN_ALL = 0x92, + VIDEO_REQUEST_GET_MAX_ALL = 0x93, + VIDEO_REQUEST_GET_RES_ALL = 0x94, + VIDEO_REQUEST_GET_DEF_ALL = 0x97 +} video_control_request_t; + +/* A.9.1 VideoControl Interface Control Selectors */ +typedef enum { + VIDEO_VC_CTL_UNDEFINED = 0x00, + VIDEO_VC_CTL_VIDEO_POWER_MODE, + VIDEO_VC_CTL_REQUEST_ERROR_CODE, +} video_interface_control_selector_t; + +/* A.9.8 VideoStreaming Interface Control Selectors */ +typedef enum { + VIDEO_VS_CTL_UNDEFINED = 0x00, + VIDEO_VS_CTL_PROBE, + VIDEO_VS_CTL_COMMIT, + VIDEO_VS_CTL_STILL_PROBE, + VIDEO_VS_CTL_STILL_COMMIT, + VIDEO_VS_CTL_STILL_IMAGE_TRIGGER, + VIDEO_VS_CTL_STREAM_ERROR_CODE, + VIDEO_VS_CTL_GENERATE_KEY_FRAME, + VIDEO_VS_CTL_UPDATE_FRAME_SEGMENT, + VIDEO_VS_CTL_SYNCH_DELAY_CONTROL, +} video_interface_streaming_selector_t; + +/* B. Terminal Types */ +typedef enum { + // Terminal + VIDEO_TT_VENDOR_SPECIFIC = 0x0100, + VIDEO_TT_STREAMING = 0x0101, + + // Input + VIDEO_ITT_VENDOR_SPECIFIC = 0x0200, + VIDEO_ITT_CAMERA = 0x0201, + VIDEO_ITT_MEDIA_TRANSPORT_INPUT = 0x0202, + + // Output + VIDEO_OTT_VENDOR_SPECIFIC = 0x0300, + VIDEO_OTT_DISPLAY = 0x0301, + VIDEO_OTT_MEDIA_TRANSPORT_OUTPUT = 0x0302, + + // External + VIDEO_ETT_VENDOR_SPEIFIC = 0x0400, + VIDEO_ETT_COMPOSITE_CONNECTOR = 0x0401, + VIDEO_ETT_SVIDEO_CONNECTOR = 0x0402, + VIDEO_ETT_COMPONENT_CONNECTOR = 0x0403, +} video_terminal_type_t; + +//--------------------------------------------------------------------+ +// Descriptors +//--------------------------------------------------------------------+ + +/* 2.3.4.2 */ +typedef struct TU_ATTR_PACKED { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint16_t bcdUVC; + uint16_t wTotalLength; + uint32_t dwClockFrequency; + uint8_t bInCollection; + uint8_t baInterfaceNr[]; +} tusb_desc_cs_video_ctl_itf_hdr_t; + +/* 2.4.3.3 */ +typedef struct TU_ATTR_PACKED { + uint8_t bHeaderLength; + union { + uint8_t bmHeaderInfo; + struct { + uint8_t FrameID: 1; + uint8_t EndOfFrame: 1; + uint8_t PresentationTime: 1; + uint8_t SourceClockReference: 1; + uint8_t PayloadSpecific: 1; + uint8_t StillImage: 1; + uint8_t Error: 1; + uint8_t EndOfHeader: 1; + }; + }; +} tusb_video_payload_header_t; + +/* 3.9.2.1 */ +typedef struct TU_ATTR_PACKED { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bNumFormats; + uint16_t wTotalLength; + uint8_t bEndpointAddress; + uint8_t bmInfo; + uint8_t bTerminalLink; + uint8_t bStillCaptureMethod; + uint8_t bTriggerSupport; + uint8_t bTriggerUsage; + uint8_t bControlSize; + uint8_t bmaControls[]; +} tusb_desc_cs_video_stm_itf_in_hdr_t; + +/* 3.9.2.2 */ +typedef struct TU_ATTR_PACKED { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bNumFormats; + uint16_t wTotalLength; + uint8_t bEndpointAddress; + uint8_t bTerminalLink; + uint8_t bControlSize; + uint8_t bmaControls[]; +} tusb_desc_cs_video_stm_itf_out_hdr_t; + +typedef struct TU_ATTR_PACKED { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bNumFormats; + uint16_t wTotalLength; + uint8_t bEndpointAddress; + union { + struct { + uint8_t bmInfo; + uint8_t bTerminalLink; + uint8_t bStillCaptureMethod; + uint8_t bTriggerSupport; + uint8_t bTriggerUsage; + uint8_t bControlSize; + uint8_t bmaControls[]; + } input; + struct { + uint8_t bEndpointAddress; + uint8_t bTerminalLink; + uint8_t bControlSize; + uint8_t bmaControls[]; + } output; + }; +} tusb_desc_cs_video_stm_itf_hdr_t; + +typedef struct TU_ATTR_PACKED { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bFormatIndex; + uint8_t bNumFrameDescriptors; + uint8_t guidFormat[16]; + uint8_t bBitsPerPixel; + uint8_t bDefaultFrameIndex; + uint8_t bAspectRatioX; + uint8_t bAspectRatioY; + uint8_t bmInterlaceFlags; + uint8_t bCopyProtect; +} tusb_desc_cs_video_fmt_uncompressed_t; + +typedef struct TU_ATTR_PACKED { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubType; + uint8_t bFrameIndex; + uint8_t bmCapabilities; + uint16_t wWidth; + uint16_t wHeight; + uint32_t dwMinBitRate; + uint32_t dwMaxBitRate; + uint32_t dwMaxVideoFrameBufferSize; /* deprecated */ + uint32_t dwDefaultFrameInterval; + uint8_t bFrameIntervalType; + uint32_t dwFrameInterval[]; +} tusb_desc_cs_video_frm_uncompressed_t; + +//--------------------------------------------------------------------+ +// Requests +//--------------------------------------------------------------------+ + +/* 4.3.1.1 */ +typedef struct TU_ATTR_PACKED { + union { + uint8_t bmHint; + struct TU_ATTR_PACKED { + uint16_t dwFrameInterval: 1; + uint16_t wKeyFrameRatel : 1; + uint16_t wPFrameRate : 1; + uint16_t wCompQuality : 1; + uint16_t wCompWindowSize: 1; + uint16_t : 0; + } Hint; + }; + uint8_t bFormatIndex; + uint8_t bFrameIndex; + uint32_t dwFrameInterval; + uint16_t wKeyFrameRate; + uint16_t wPFrameRate; + uint16_t wCompQuality; + uint16_t wCompWindowSize; + uint16_t wDelay; + uint32_t dwMaxVideoFrameSize; + uint32_t dwMaxPayloadTransferSize; + uint32_t dwClockFrequency; + union { + uint8_t bmFramingInfo; + struct TU_ATTR_PACKED { + uint8_t FrameID : 1; + uint8_t EndOfFrame: 1; + uint8_t EndOfSlice: 1; + uint8_t : 0; + } FramingInfo; + }; + uint8_t bPreferedVersion; + uint8_t bMinVersion; + uint8_t bMaxVersion; + uint8_t bUsage; + uint8_t bBitDepthLuma; + uint8_t bmSettings; + uint8_t bMaxNumberOfRefFramesPlus1; + uint16_t bmRateControlModes; + uint64_t bmLayoutPerStream; +} video_probe_and_commit_control_t; + +TU_VERIFY_STATIC( sizeof(video_probe_and_commit_control_t) == 48, "size is not correct"); + +#define TUD_VIDEO_DESC_IAD_LEN 8 +#define TUD_VIDEO_DESC_STD_VC_LEN 9 +#define TUD_VIDEO_DESC_CS_VC_LEN 12 +#define TUD_VIDEO_DESC_INPUT_TERM_LEN 8 +#define TUD_VIDEO_DESC_OUTPUT_TERM_LEN 9 +#define TUD_VIDEO_DESC_CAMERA_TERM_LEN 18 +#define TUD_VIDEO_DESC_STD_VS_LEN 9 +#define TUD_VIDEO_DESC_CS_VS_IN_LEN 13 +#define TUD_VIDEO_DESC_CS_VS_OUT_LEN 9 +#define TUD_VIDEO_DESC_CS_VS_FMT_UNCOMPR_LEN 27 +#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_CONT_LEN 38 +#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_DISC_LEN 26 +#define TUD_VIDEO_DESC_CS_VS_COLOR_MATCHING_LEN 6 + +/* 2.2 compression formats */ +#define TUD_VIDEO_GUID_YUY2 0x59,0x55,0x59,0x32,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71 +#define TUD_VIDEO_GUID_NV12 0x4E,0x56,0x31,0x32,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71 +#define TUD_VIDEO_GUID_M420 0x4D,0x34,0x32,0x30,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71 +#define TUD_VIDEO_GUID_I420 0x49,0x34,0x32,0x30,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71 + +#define TUD_VIDEO_DESC_IAD(_firstitfs, _nitfs, _stridx) \ + TUD_VIDEO_DESC_IAD_LEN, TUSB_DESC_INTERFACE_ASSOCIATION, \ + _firstitfs, _nitfs, TUSB_CLASS_VIDEO, VIDEO_SUBCLASS_INTERFACE_COLLECTION, \ + VIDEO_ITF_PROTOCOL_UNDEFINED, _stridx + +#define TUD_VIDEO_DESC_STD_VC(_itfnum, _nEPs, _stridx) \ + TUD_VIDEO_DESC_STD_VC_LEN, TUSB_DESC_INTERFACE, _itfnum, /* fixed to zero */ 0x00, \ + _nEPs, TUSB_CLASS_VIDEO, VIDEO_SUBCLASS_CONTROL, VIDEO_ITF_PROTOCOL_15, _stridx + +/* 3.7.2 */ +#define TUD_VIDEO_DESC_CS_VC(_bcdUVC, _totallen, _clkfreq, ...) \ + TUD_VIDEO_DESC_CS_VC_LEN + (TU_ARGS_NUM(__VA_ARGS__)), TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VC_HEADER, \ + U16_TO_U8S_LE(_bcdUVC), U16_TO_U8S_LE((_totallen) + TUD_VIDEO_DESC_CS_VC_LEN + (TU_ARGS_NUM(__VA_ARGS__))), \ + U32_TO_U8S_LE(_clkfreq), TU_ARGS_NUM(__VA_ARGS__), __VA_ARGS__ + +/* 3.7.2.1 */ +#define TUD_VIDEO_DESC_INPUT_TERM(_tid, _tt, _at, _stridx) \ + TUD_VIDEO_DESC_INPUT_TERM_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VC_INPUT_TERMINAL, \ + _tid, U16_TO_U8S_LE(_tt), _at, _stridx + +/* 3.7.2.2 */ +#define TUD_VIDEO_DESC_OUTPUT_TERM(_tid, _tt, _at, _srcid, _stridx) \ + TUD_VIDEO_DESC_OUTPUT_TERM_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VC_OUTPUT_TERMINAL, \ + _tid, U16_TO_U8S_LE(_tt), _at, _srcid, _stridx + +/* 3.7.2.3 */ +#define TUD_VIDEO_DESC_CAMERA_TERM(_tid, _at, _stridx, _focal_min, _focal_max, _focal, _ctls) \ + TUD_VIDEO_DESC_CAMERA_TERM_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VC_INPUT_TERMINAL, \ + _tid, U16_TO_U8S_LE(VIDEO_ITT_CAMERA), _at, _stridx, \ + U16_TO_U8S_LE(_focal_min), U16_TO_U8S_LE(_focal_max), U16_TO_U8S_LE(_focal), 3, \ + TU_U32_BYTE0(_ctls), TU_U32_BYTE1(_ctls), TU_U32_BYTE2(_ctls) + +/* 3.9.1 */ +#define TUD_VIDEO_DESC_STD_VS(_itfnum, _alt, _epn, _stridx) \ + TUD_VIDEO_DESC_STD_VS_LEN, TUSB_DESC_INTERFACE, _itfnum, _alt, \ + _epn, TUSB_CLASS_VIDEO, VIDEO_SUBCLASS_STREAMING, VIDEO_ITF_PROTOCOL_15, _stridx + +/* 3.9.2.1 */ +#define TUD_VIDEO_DESC_CS_VS_INPUT(_numfmt, _totallen, _ep, _inf, _termlnk, _sticaptmeth, _trgspt, _trgusg, ...) \ + TUD_VIDEO_DESC_CS_VS_IN_LEN + (_numfmt) * (TU_ARGS_NUM(__VA_ARGS__)), TUSB_DESC_CS_INTERFACE, \ + VIDEO_CS_ITF_VS_INPUT_HEADER, _numfmt, \ + U16_TO_U8S_LE((_totallen) + TUD_VIDEO_DESC_CS_VS_IN_LEN + (_numfmt) * (TU_ARGS_NUM(__VA_ARGS__))), \ + _ep, _inf, _termlnk, _sticaptmeth, _trgspt, _trgusg, (TU_ARGS_NUM(__VA_ARGS__)), __VA_ARGS__ + +/* 3.9.2.2 */ +#define TUD_VIDEO_DESC_CS_VS_OUTPUT(_numfmt, _totallen, _ep, _inf, _termlnk, ...) \ + TUD_VIDEO_DESC_CS_VS_OUT_LEN + (_numfmt) * (TU_ARGS_NUM(__VA_ARGS__)), TUSB_DESC_CS_INTERFACE, \ + VIDEO_CS_ITF_VS_OUTPUT_HEADER, _numfmt, \ + U16_TO_U8S_LE((_totallen) + TUD_VIDEO_DESC_CS_VS_OUT_LEN + (_numfmt) * (TU_ARGS_NUM(__VA_ARGS__))), \ + _ep, _inf, _termlnk, (TU_ARGS_NUM(__VA_ARGS__)), __VA_ARGS__ + +/* Uncompressed 3.1.1 */ +#define TUD_VIDEO_GUID(_g0,_g1,_g2,_g3,_g4,_g5,_g6,_g7,_g8,_g9,_g10,_g11,_g12,_g13,_g14,_g15) _g0,_g1,_g2,_g3,_g4,_g5,_g6,_g7,_g8,_g9,_g10,_g11,_g12,_g13,_g14,_g15 + +#define TUD_VIDEO_DESC_CS_VS_FMT_UNCOMPR(_fmtidx, _numfrmdesc, \ + _guid, _bitsperpix, _frmidx, _asrx, _asry, _interlace, _cp) \ + TUD_VIDEO_DESC_CS_VS_FMT_UNCOMPR_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED, \ + _fmtidx, _numfrmdesc, TUD_VIDEO_GUID(_guid), \ + _bitsperpix, _frmidx, _asrx, _asry, _interlace, _cp + +/* Uncompressed 3.1.2 Table 3-3 */ +#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_CONT(_frmidx, _cap, _width, _height, _minbr, _maxbr, _maxfrmbufsz, _frminterval, _minfrminterval, _maxfrminterval, _frmintervalstep) \ + TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_CONT_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FRAME_UNCOMPRESSED, \ + _frmidx, _cap, U16_TO_U8S_LE(_width), U16_TO_U8S_LE(_height), U32_TO_U8S_LE(_minbr), U32_TO_U8S_LE(_maxbr), \ + U32_TO_U8S_LE(_maxfrmbufsz), U32_TO_U8S_LE(_frminterval), 0, \ + U32_TO_U8S_LE(_minfrminterval), U32_TO_U8S_LE(_maxfrminterval), U32_TO_U8S_LE(_frmintervalstep) + +/* Uncompressed 3.1.2 Table 3-4 */ +#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_DISC(_frmidx, _cap, _width, _height, _minbr, _maxbr, _maxfrmbufsz, _frminterval, ...) \ + TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_DISC_LEN + (TU_ARGS_NUM(__VA_ARGS__)) * 4, \ + TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FRAME_UNCOMPRESSED, \ + _frmidx, _cap, U16_TO_U8S_LE(_width), U16_TO_U8S_LE(_height), U32_TO_U8S_LE(_minbr), U32_TO_U8S_LE(_maxbr), \ + U32_TO_U8S_LE(_maxfrmbufsz), U32_TO_U8S_LE(_frminterval), (TU_ARGS_NUM(__VA_ARGS__)), __VA_ARGS__ + +/* 3.9.2.6 */ +#define TUD_VIDEO_DESC_CS_VS_COLOR_MATCHING(_color, _trns, _mat) \ + TUD_VIDEO_DESC_CS_VS_COLOR_MATCHING_LEN, \ + TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_COLORFORMAT, \ + _color, _trns, _mat + +/* 3.10.1.1 */ +#define TUD_VIDEO_DESC_EP_ISO(_ep, _epsize, _ep_interval) \ + 7, TUSB_DESC_ENDPOINT, _ep, TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS,\ + U16_TO_U8S_LE(_epsize), _ep_interval + +/* 3.10.1.2 */ +#define TUD_VIDEO_DESC_EP_BULK(_ep, _epsize, _ep_interval) \ + 7, TUSB_DESC_ENDPOINT, _ep, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), _ep_interval + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video_device.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video_device.c new file mode 100644 index 000000000..eeb068197 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video_device.c @@ -0,0 +1,1149 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021 Koji KITAYAMA + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_VIDEO && CFG_TUD_VIDEO_STREAMING) + +#include "device/usbd.h" +#include "device/usbd_pvt.h" + +#include "video_device.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef struct { + tusb_desc_interface_t std; + tusb_desc_cs_video_ctl_itf_hdr_t ctl; +} tusb_desc_vc_itf_t; + +typedef struct { + tusb_desc_interface_t std; + tusb_desc_cs_video_stm_itf_hdr_t stm; +} tusb_desc_vs_itf_t; + +typedef union { + tusb_desc_cs_video_ctl_itf_hdr_t ctl; + tusb_desc_cs_video_stm_itf_hdr_t stm; +} tusb_desc_video_itf_hdr_t; + +typedef struct TU_ATTR_PACKED { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + uint8_t bEntityId; +} tusb_desc_cs_video_entity_itf_t; + +/* video streaming interface */ +typedef struct TU_ATTR_PACKED { + uint8_t index_vc; /* index of bound video control interface */ + uint8_t index_vs; /* index from the video control interface */ + struct { + uint16_t beg; /* Offset of the begging of video streaming interface descriptor */ + uint16_t end; /* Offset of the end of video streaming interface descriptor */ + uint16_t cur; /* Offset of the current settings */ + uint16_t ep[2]; /* Offset of endpoint descriptors. 0: streaming, 1: still capture */ + } desc; + uint8_t *buffer; /* frame buffer. assume linear buffer. no support for stride access */ + uint32_t bufsize; /* frame buffer size */ + uint32_t offset; /* offset for the next payload transfer */ + uint32_t max_payload_transfer_size; + uint8_t error_code;/* error code */ + /*------------- From this point, data is not cleared by bus reset -------------*/ + CFG_TUSB_MEM_ALIGN uint8_t ep_buf[CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE]; /* EP transfer buffer for streaming */ +} videod_streaming_interface_t; + +/* video control interface */ +typedef struct TU_ATTR_PACKED { + void const *beg; /* The head of the first video control interface descriptor */ + uint16_t len; /* Byte length of the descriptors */ + uint16_t cur; /* offset for current video control interface */ + uint8_t stm[CFG_TUD_VIDEO_STREAMING]; /* Indices of streaming interface */ + uint8_t error_code; /* error code */ + uint8_t power_mode; + + /*------------- From this point, data is not cleared by bus reset -------------*/ + // CFG_TUSB_MEM_ALIGN uint8_t ctl_buf[64]; /* EP transfer buffer for interrupt transfer */ + +} videod_interface_t; + +#define ITF_STM_MEM_RESET_SIZE offsetof(videod_streaming_interface_t, ep_buf) + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ +CFG_TUSB_MEM_SECTION static videod_interface_t _videod_itf[CFG_TUD_VIDEO]; +CFG_TUSB_MEM_SECTION static videod_streaming_interface_t _videod_streaming_itf[CFG_TUD_VIDEO_STREAMING]; + +static uint8_t const _cap_get = 0x1u; /* support for GET */ +static uint8_t const _cap_get_set = 0x3u; /* support for GET and SET */ + +/** Get interface number from the interface descriptor + * + * @param[in] desc interface descriptor + * + * @return bInterfaceNumber */ +static inline uint8_t _desc_itfnum(void const *desc) +{ + return ((uint8_t const*)desc)[2]; +} + +/** Get endpoint address from the endpoint descriptor + * + * @param[in] desc endpoint descriptor + * + * @return bEndpointAddress */ +static inline uint8_t _desc_ep_addr(void const *desc) +{ + return ((uint8_t const*)desc)[2]; +} + +/** Get instance of streaming interface + * + * @param[in] ctl_idx instance number of video control + * @param[in] stm_idx index number of streaming interface + * + * @return instance */ +static videod_streaming_interface_t* _get_instance_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx) +{ + videod_interface_t *ctl = &_videod_itf[ctl_idx]; + if (!ctl->beg) return NULL; + videod_streaming_interface_t *stm = &_videod_streaming_itf[ctl->stm[stm_idx]]; + if (!stm->desc.beg) return NULL; + return stm; +} + +static tusb_desc_vc_itf_t const* _get_desc_vc(videod_interface_t const *self) +{ + return (tusb_desc_vc_itf_t const *)(self->beg + self->cur); +} + +static tusb_desc_vs_itf_t const* _get_desc_vs(videod_streaming_interface_t const *self) +{ + if (!self->desc.cur) return NULL; + void const *desc = _videod_itf[self->index_vc].beg; + return (tusb_desc_vs_itf_t const*)(desc + self->desc.cur); +} + +/** Find the first descriptor of a given type + * + * @param[in] beg The head of descriptor byte array. + * @param[in] end The tail of descriptor byte array. + * @param[in] desc_type The target descriptor type. + * + * @return The pointer for interface descriptor. + * @retval end did not found interface descriptor */ +static void const* _find_desc(void const *beg, void const *end, uint_fast8_t desc_type) +{ + void const *cur = beg; + while ((cur < end) && (desc_type != tu_desc_type(cur))) { + cur = tu_desc_next(cur); + } + return cur; +} + +/** Find the first descriptor specified by the arguments + * + * @param[in] beg The head of descriptor byte array. + * @param[in] end The tail of descriptor byte array. + * @param[in] desc_type The target descriptor type + * @param[in] element_0 The target element following the desc_type + * @param[in] element_1 The target element following the element_0 + * + * @return The pointer for interface descriptor. + * @retval end did not found interface descriptor */ +static void const* _find_desc_3(void const *beg, void const *end, + uint_fast8_t desc_type, + uint_fast8_t element_0, + uint_fast8_t element_1) +{ + for (void const *cur = beg; cur < end; cur = _find_desc(cur, end, desc_type)) { + uint8_t const *p = (uint8_t const *)cur; + if ((p[2] == element_0) && (p[3] == element_1)) { + return cur; + } + cur = tu_desc_next(cur); + } + return end; +} + +/** Return the next interface descriptor which has another interface number. + * + * @param[in] beg The head of descriptor byte array. + * @param[in] end The tail of descriptor byte array. + * + * @return The pointer for interface descriptor. + * @retval end did not found interface descriptor */ +static void const* _next_desc_itf(void const *beg, void const *end) +{ + void const *cur = beg; + uint_fast8_t itfnum = ((tusb_desc_interface_t const*)cur)->bInterfaceNumber; + while ((cur < end) && + (itfnum == ((tusb_desc_interface_t const*)cur)->bInterfaceNumber)) { + cur = _find_desc(tu_desc_next(cur), end, TUSB_DESC_INTERFACE); + } + return cur; +} + +/** Find the first interface descriptor with the specified interface number and alternate setting number. + * + * @param[in] beg The head of descriptor byte array. + * @param[in] end The tail of descriptor byte array. + * @param[in] itfnum The target interface number. + * @param[in] altnum The target alternate setting number. + * + * @return The pointer for interface descriptor. + * @retval end did not found interface descriptor */ +static inline void const* _find_desc_itf(void const *beg, void const *end, uint_fast8_t itfnum, uint_fast8_t altnum) +{ + return _find_desc_3(beg, end, TUSB_DESC_INTERFACE, itfnum, altnum); +} + +/** Find the first endpoint descriptor belonging to the current interface descriptor. + * + * The search range is from `beg` to `end` or the next interface descriptor. + * + * @param[in] beg The head of descriptor byte array. + * @param[in] end The tail of descriptor byte array. + * + * @return The pointer for endpoint descriptor. + * @retval end did not found endpoint descriptor */ +static void const* _find_desc_ep(void const *beg, void const *end) +{ + for (void const *cur = beg; cur < end; cur = tu_desc_next(cur)) { + uint_fast8_t desc_type = tu_desc_type(cur); + if (TUSB_DESC_ENDPOINT == desc_type) return cur; + if (TUSB_DESC_INTERFACE == desc_type) break; + } + return end; +} + +/** Find the first entity descriptor with the entity ID + * specified by the argument belonging to the current video control descriptor. + * + * @param[in] desc The video control interface descriptor. + * @param[in] entityid The target entity id. + * + * @return The pointer for interface descriptor. + * @retval end did not found interface descriptor */ +static void const* _find_desc_entity(void const *desc, uint_fast8_t entityid) +{ + tusb_desc_vc_itf_t const *vc = (tusb_desc_vc_itf_t const*)desc; + void const *beg = vc; + void const *end = beg + vc->std.bLength + vc->ctl.wTotalLength; + for (void const *cur = beg; cur < end; cur = _find_desc(cur, end, TUSB_DESC_CS_INTERFACE)) { + tusb_desc_cs_video_entity_itf_t const *itf = (tusb_desc_cs_video_entity_itf_t const *)cur; + if ((VIDEO_CS_ITF_VC_INPUT_TERMINAL <= itf->bDescriptorSubtype + && itf->bDescriptorSubtype < VIDEO_CS_ITF_VC_MAX) + && itf->bEntityId == entityid) { + return itf; + } + cur = tu_desc_next(cur); + } + return end; +} + +/** Return the end of the video streaming descriptor. */ +static inline void const* _end_of_streaming_descriptor(void const *desc) +{ + tusb_desc_vs_itf_t const *vs = (tusb_desc_vs_itf_t const *)desc; + return desc + vs->std.bLength + vs->stm.wTotalLength; +} + +/** Find the first format descriptor with the specified format number. */ +static inline tusb_desc_cs_video_fmt_uncompressed_t const *_find_desc_format(void const *beg, void const *end, uint_fast8_t fmtnum) +{ + return (tusb_desc_cs_video_fmt_uncompressed_t const*) + _find_desc_3(beg, end, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED, fmtnum); +} + +/** Find the first frame descriptor with the specified format number. */ +static inline tusb_desc_cs_video_frm_uncompressed_t const *_find_desc_frame(void const *beg, void const *end, uint_fast8_t frmnum) +{ + return (tusb_desc_cs_video_frm_uncompressed_t const*) + _find_desc_3(beg, end, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FRAME_UNCOMPRESSED, frmnum); +} + +/** Set uniquely determined values to variables that have not been set + * + * @param[in,out] param Target */ +static bool _update_streaming_parameters(videod_streaming_interface_t const *stm, + video_probe_and_commit_control_t *param) +{ + tusb_desc_vs_itf_t const *vs = _get_desc_vs(stm); + uint_fast8_t fmtnum = param->bFormatIndex; + TU_ASSERT(fmtnum <= vs->stm.bNumFormats); + if (!fmtnum) { + if (1 < vs->stm.bNumFormats) return true; /* Need to negotiate all variables. */ + fmtnum = 1; + param->bFormatIndex = 1; + } + + /* Set the parameters determined by the format */ + param->wKeyFrameRate = 1; + param->wPFrameRate = 0; + param->wCompQuality = 1; /* 1 to 10000 */ + param->wCompWindowSize = 1; /* GOP size? */ + param->wDelay = 0; /* milliseconds */ + param->dwClockFrequency = 27000000; /* same as MPEG-2 system time clock */ + param->bmFramingInfo = 0x3; /* enables FrameID and EndOfFrame */ + param->bPreferedVersion = 1; + param->bMinVersion = 1; + param->bMaxVersion = 1; + param->bUsage = 0; + param->bBitDepthLuma = 8; + + void const *end = _end_of_streaming_descriptor(vs); + tusb_desc_cs_video_fmt_uncompressed_t const *fmt = _find_desc_format(tu_desc_next(vs), end, fmtnum); + TU_ASSERT(fmt != end); + uint_fast8_t frmnum = param->bFrameIndex; + TU_ASSERT(frmnum <= fmt->bNumFrameDescriptors); + if (!frmnum) { + if (1 < fmt->bNumFrameDescriptors) return true; + frmnum = 1; + param->bFrameIndex = 1; + } + tusb_desc_cs_video_frm_uncompressed_t const *frm = _find_desc_frame(tu_desc_next(fmt), end, frmnum); + TU_ASSERT(frm != end); + + /* Set the parameters determined by the frame */ + uint_fast32_t frame_size = param->dwMaxVideoFrameSize; + if (!frame_size) { + frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * fmt->bBitsPerPixel / 8; + param->dwMaxVideoFrameSize = frame_size; + } + + uint_fast32_t interval = param->dwFrameInterval; + if (!interval) { + if ((1 < frm->bFrameIntervalType) || + ((0 == frm->bFrameIntervalType) && (frm->dwFrameInterval[1] != frm->dwFrameInterval[0]))) { + return true; + } + interval = frm->dwFrameInterval[0]; + param->dwFrameInterval = interval; + } + uint_fast32_t interval_ms = interval / 10000; + TU_ASSERT(interval_ms); + uint_fast32_t payload_size = (frame_size + interval_ms - 1) / interval_ms + 2; + param->dwMaxPayloadTransferSize = payload_size; + return true; +} + +/** Set the minimum, maximum, default values or resolutions to variables which need to negotiate with the host + * + * @param[in] request GET_MAX, GET_MIN, GET_RES or GET_DEF + * @param[in,out] param Target + */ +static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *stm, uint_fast8_t request, + video_probe_and_commit_control_t *param) +{ + uint_fast8_t const fmtnum = param->bFormatIndex; + if (!fmtnum) { + switch (request) { + case VIDEO_REQUEST_GET_MAX: + param->bFormatIndex = _get_desc_vs(stm)->stm.bNumFormats; + break; + case VIDEO_REQUEST_GET_MIN: + case VIDEO_REQUEST_GET_DEF: + param->bFormatIndex = 1; + break; + default: return false; + } + /* Set the parameters determined by the format */ + param->wKeyFrameRate = 1; + param->wPFrameRate = 0; + param->wCompQuality = 1; /* 1 to 10000 */ + param->wCompWindowSize = 1; /* GOP size? */ + param->wDelay = 0; /* milliseconds */ + param->dwClockFrequency = 27000000; /* same as MPEG-2 system time clock */ + param->bmFramingInfo = 0x3; /* enables FrameID and EndOfFrame */ + param->bPreferedVersion = 1; + param->bMinVersion = 1; + param->bMaxVersion = 1; + param->bUsage = 0; + param->bBitDepthLuma = 8; + return true; + } + + uint_fast8_t frmnum = param->bFrameIndex; + if (!frmnum) { + tusb_desc_vs_itf_t const *vs = _get_desc_vs(stm); + void const *end = _end_of_streaming_descriptor(vs); + tusb_desc_cs_video_fmt_uncompressed_t const *fmt = _find_desc_format(tu_desc_next(vs), end, fmtnum); + switch (request) { + case VIDEO_REQUEST_GET_MAX: + frmnum = fmt->bNumFrameDescriptors; + break; + case VIDEO_REQUEST_GET_MIN: + frmnum = 1; + break; + case VIDEO_REQUEST_GET_DEF: + frmnum = fmt->bDefaultFrameIndex; + break; + default: return false; + } + param->bFrameIndex = frmnum; + /* Set the parameters determined by the frame */ + tusb_desc_cs_video_frm_uncompressed_t const *frm = _find_desc_frame(tu_desc_next(fmt), end, frmnum); + param->dwMaxVideoFrameSize = frm->wWidth * frm->wHeight * fmt->bBitsPerPixel / 8; + return true; + } + + if (!param->dwFrameInterval) { + tusb_desc_vs_itf_t const *vs = _get_desc_vs(stm); + void const *end = _end_of_streaming_descriptor(vs); + tusb_desc_cs_video_fmt_uncompressed_t const *fmt = _find_desc_format(tu_desc_next(vs), end, fmtnum); + tusb_desc_cs_video_frm_uncompressed_t const *frm = _find_desc_frame(tu_desc_next(fmt), end, frmnum); + + uint_fast32_t interval, interval_ms; + switch (request) { + case VIDEO_REQUEST_GET_MAX: + { + uint_fast32_t min_interval, max_interval; + uint_fast8_t num_intervals = frm->bFrameIntervalType; + max_interval = num_intervals ? frm->dwFrameInterval[num_intervals - 1]: frm->dwFrameInterval[1]; + min_interval = frm->dwFrameInterval[0]; + interval = max_interval; + interval_ms = min_interval / 10000; + } + break; + case VIDEO_REQUEST_GET_MIN: + { + uint_fast32_t min_interval, max_interval; + uint_fast8_t num_intervals = frm->bFrameIntervalType; + max_interval = num_intervals ? frm->dwFrameInterval[num_intervals - 1]: frm->dwFrameInterval[1]; + min_interval = frm->dwFrameInterval[0]; + interval = min_interval; + interval_ms = max_interval / 10000; + } + break; + case VIDEO_REQUEST_GET_DEF: + interval = frm->dwDefaultFrameInterval; + interval_ms = interval / 10000; + break; + case VIDEO_REQUEST_GET_RES: + { + uint_fast8_t num_intervals = frm->bFrameIntervalType; + if (num_intervals) { + interval = 0; + } else { + interval = frm->dwFrameInterval[2]; + interval_ms = interval / 10000; + } + } + break; + default: return false; + } + param->dwFrameInterval = interval; + if (!interval) { + param->dwMaxPayloadTransferSize = 0; + } else { + uint_fast32_t frame_size = param->dwMaxVideoFrameSize; + if (!interval_ms) { + param->dwMaxPayloadTransferSize = frame_size + 2; + } else { + param->dwMaxPayloadTransferSize = (frame_size + interval_ms - 1) / interval_ms + 2; + } + } + return true; + } + return true; +} + +/** Close current video control interface. + * + * @param[in,out] self Video control interface context. + * @param[in] altnum The target alternate setting number. */ +static bool _close_vc_itf(uint8_t rhport, videod_interface_t *self) +{ + tusb_desc_vc_itf_t const *vc = _get_desc_vc(self); + /* The next descriptor after the class-specific VC interface header descriptor. */ + void const *cur = (void const*)vc + vc->std.bLength + vc->ctl.bLength; + /* The end of the video control interface descriptor. */ + void const *end = (void const*)vc + vc->std.bLength + vc->ctl.wTotalLength; + if (vc->std.bNumEndpoints) { + /* Find the notification endpoint descriptor. */ + cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT); + TU_ASSERT(cur < end); + tusb_desc_endpoint_t const *notif = (tusb_desc_endpoint_t const *)cur; + usbd_edpt_close(rhport, notif->bEndpointAddress); + } + self->cur = 0; + return true; +} + +/** Set the alternate setting to own video control interface. + * + * @param[in,out] self Video control interface context. + * @param[in] altnum The target alternate setting number. */ +static bool _open_vc_itf(uint8_t rhport, videod_interface_t *self, uint_fast8_t altnum) +{ + TU_LOG2(" open VC %d\n", altnum); + void const *beg = self->beg; + void const *end = beg + self->len; + /* The first descriptor is a video control interface descriptor. */ + void const *cur = _find_desc_itf(beg, end, _desc_itfnum(beg), altnum); + TU_LOG2(" cur %d\n", cur - beg); + TU_VERIFY(cur < end); + + tusb_desc_vc_itf_t const *vc = (tusb_desc_vc_itf_t const *)cur; + TU_LOG2(" bInCollection %d\n", vc->ctl.bInCollection); + /* Support for up to 2 streaming interfaces only. */ + TU_ASSERT(vc->ctl.bInCollection <= CFG_TUD_VIDEO_STREAMING); + + /* Update to point the end of the video control interface descriptor. */ + end = cur + vc->std.bLength + vc->ctl.wTotalLength; + /* Advance to the next descriptor after the class-specific VC interface header descriptor. */ + cur += vc->std.bLength + vc->ctl.bLength; + TU_LOG2(" bNumEndpoints %d\n", vc->std.bNumEndpoints); + /* Open the notification endpoint if it exist. */ + if (vc->std.bNumEndpoints) { + /* Support for 1 endpoint only. */ + TU_VERIFY(1 == vc->std.bNumEndpoints); + /* Find the notification endpoint descriptor. */ + cur = _find_desc(cur, end, TUSB_DESC_ENDPOINT); + TU_VERIFY(cur < end); + tusb_desc_endpoint_t const *notif = (tusb_desc_endpoint_t const *)cur; + /* Open the notification endpoint */ + TU_ASSERT(usbd_edpt_open(rhport, notif)); + } + self->cur = (void const*)vc - beg; + return true; +} + +/** Set the alternate setting to own video streaming interface. + * + * @param[in,out] stm Streaming interface context. + * @param[in] altnum The target alternate setting number. */ +static bool _open_vs_itf(uint8_t rhport, videod_streaming_interface_t *stm, uint_fast8_t altnum) +{ + uint_fast8_t i; + TU_LOG2(" reopen VS %d\n", altnum); + void const *desc = _videod_itf[stm->index_vc].beg; + + /* Close endpoints of previous settings. */ + for (i = 0; i < TU_ARRAY_SIZE(stm->desc.ep); ++i) { + uint_fast16_t ofs_ep = stm->desc.ep[i]; + if (!ofs_ep) break; + uint_fast8_t ep_adr = _desc_ep_addr(desc + ofs_ep); + usbd_edpt_close(rhport, ep_adr); + stm->desc.ep[i] = 0; + TU_LOG2(" close EP%02x\n", ep_adr); + } + /* clear transfer management information */ + stm->buffer = NULL; + stm->bufsize = 0; + stm->offset = 0; + + /* Find a alternate interface */ + void const *beg = desc + stm->desc.beg; + void const *end = desc + stm->desc.end; + void const *cur = _find_desc_itf(beg, end, _desc_itfnum(beg), altnum); + TU_VERIFY(cur < end); + uint_fast8_t numeps = ((tusb_desc_interface_t const *)cur)->bNumEndpoints; + TU_ASSERT(numeps <= TU_ARRAY_SIZE(stm->desc.ep)); + stm->desc.cur = cur - desc; /* Save the offset of the new settings */ + if (!altnum) { + /* initialize streaming settings */ + stm->max_payload_transfer_size = 0; + video_probe_and_commit_control_t *param = + (video_probe_and_commit_control_t *)&stm->ep_buf; + tu_memclr(param, sizeof(*param)); + return _update_streaming_parameters(stm, param); + } + /* Open endpoints of the new settings. */ + for (i = 0, cur = tu_desc_next(cur); i < numeps; ++i, cur = tu_desc_next(cur)) { + cur = _find_desc_ep(cur, end); + TU_ASSERT(cur < end); + tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const*)cur; + if (!stm->max_payload_transfer_size) { + video_probe_and_commit_control_t const *param = (video_probe_and_commit_control_t const*)&stm->ep_buf; + uint_fast32_t max_size = param->dwMaxPayloadTransferSize; + if ((TUSB_XFER_ISOCHRONOUS == ep->bmAttributes.xfer) && + (tu_edpt_packet_size(ep) < max_size)) + { + /* FS must be less than or equal to max packet size */ + return false; + } + /* Set the negotiated value */ + stm->max_payload_transfer_size = max_size; + } + TU_ASSERT(usbd_edpt_open(rhport, ep)); + stm->desc.ep[i] = cur - desc; + TU_LOG2(" open EP%02x\n", _desc_ep_addr(cur)); + } + /* initialize payload header */ + tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm->ep_buf; + hdr->bHeaderLength = sizeof(*hdr); + hdr->bmHeaderInfo = 0; + + return true; +} + +/** Prepare the next packet payload. */ +static uint_fast16_t _prepare_in_payload(videod_streaming_interface_t *stm) +{ + uint_fast16_t remaining = stm->bufsize - stm->offset; + uint_fast16_t hdr_len = stm->ep_buf[0]; + uint_fast16_t pkt_len = stm->max_payload_transfer_size; + if (hdr_len + remaining < pkt_len) { + pkt_len = hdr_len + remaining; + } + uint_fast16_t data_len = pkt_len - hdr_len; + memcpy(&stm->ep_buf[hdr_len], stm->buffer + stm->offset, data_len); + stm->offset += data_len; + remaining -= data_len; + if (!remaining) { + tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm->ep_buf; + hdr->EndOfFrame = 1; + } + return hdr_len + data_len; +} + +/** Handle a standard request to the video control interface. */ +static int handle_video_ctl_std_req(uint8_t rhport, uint8_t stage, + tusb_control_request_t const *request, + uint_fast8_t ctl_idx) +{ + switch (request->bRequest) { + case TUSB_REQ_GET_INTERFACE: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN); + tusb_desc_vc_itf_t const *vc = _get_desc_vc(&_videod_itf[ctl_idx]); + TU_VERIFY(vc, VIDEO_ERROR_UNKNOWN); + + uint8_t alt_num = vc->std.bAlternateSetting; + + TU_VERIFY(tud_control_xfer(rhport, request, &alt_num, sizeof(alt_num)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case TUSB_REQ_SET_INTERFACE: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(0 == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(_close_vc_itf(rhport, &_videod_itf[ctl_idx]), VIDEO_ERROR_UNKNOWN); + TU_VERIFY(_open_vc_itf(rhport, &_videod_itf[ctl_idx], request->wValue), VIDEO_ERROR_UNKNOWN); + tud_control_status(rhport, request); + } + return VIDEO_ERROR_NONE; + + default: /* Unknown/Unsupported request */ + TU_BREAKPOINT(); + return VIDEO_ERROR_INVALID_REQUEST; + } +} + +static int handle_video_ctl_cs_req(uint8_t rhport, uint8_t stage, + tusb_control_request_t const *request, + uint_fast8_t ctl_idx) +{ + videod_interface_t *self = &_videod_itf[ctl_idx]; + + /* 4.2.1 Interface Control Request */ + switch (TU_U16_HIGH(request->wValue)) { + case VIDEO_VC_CTL_VIDEO_POWER_MODE: + switch (request->bRequest) { + case VIDEO_REQUEST_SET_CUR: + if (stage == CONTROL_STAGE_SETUP) { + TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, &self->power_mode, sizeof(self->power_mode)), VIDEO_ERROR_UNKNOWN); + } else if (stage == CONTROL_STAGE_ACK) { + if (tud_video_power_mode_cb) return tud_video_power_mode_cb(ctl_idx, self->power_mode); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_CUR: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, &self->power_mode, sizeof(self->power_mode)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_INFO: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get_set, sizeof(_cap_get_set)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + default: break; + } + break; + + case VIDEO_VC_CTL_REQUEST_ERROR_CODE: + switch (request->bRequest) { + case VIDEO_REQUEST_GET_CUR: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(tud_control_xfer(rhport, request, &self->error_code, sizeof(uint8_t)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_INFO: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get, sizeof(_cap_get)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + default: break; + } + break; + + default: break; + } + + /* Unknown/Unsupported request */ + TU_BREAKPOINT(); + return VIDEO_ERROR_INVALID_REQUEST; +} + +static int handle_video_ctl_req(uint8_t rhport, uint8_t stage, + tusb_control_request_t const *request, + uint_fast8_t ctl_idx) +{ + uint_fast8_t entity_id; + switch (request->bmRequestType_bit.type) { + case TUSB_REQ_TYPE_STANDARD: + return handle_video_ctl_std_req(rhport, stage, request, ctl_idx); + + case TUSB_REQ_TYPE_CLASS: + entity_id = TU_U16_HIGH(request->wIndex); + if (!entity_id) { + return handle_video_ctl_cs_req(rhport, stage, request, ctl_idx); + } else { + TU_VERIFY(_find_desc_entity(_get_desc_vc(&_videod_itf[ctl_idx]), entity_id), VIDEO_ERROR_INVALID_REQUEST); + return VIDEO_ERROR_NONE; + } + + default: + return VIDEO_ERROR_INVALID_REQUEST; + } +} + +static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage, + tusb_control_request_t const *request, + uint_fast8_t stm_idx) +{ + videod_streaming_interface_t *self = &_videod_streaming_itf[stm_idx]; + switch (request->bRequest) { + case TUSB_REQ_GET_INTERFACE: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN); + tusb_desc_vs_itf_t const *vs = _get_desc_vs(self); + TU_VERIFY(vs, VIDEO_ERROR_UNKNOWN); + uint8_t alt_num = vs->std.bAlternateSetting; + + TU_VERIFY(tud_control_xfer(rhport, request, &alt_num, sizeof(alt_num)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case TUSB_REQ_SET_INTERFACE: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(_open_vs_itf(rhport, self, request->wValue), VIDEO_ERROR_UNKNOWN); + tud_control_status(rhport, request); + } + return VIDEO_ERROR_NONE; + + default: /* Unknown/Unsupported request */ + TU_BREAKPOINT(); + return VIDEO_ERROR_INVALID_REQUEST; + } +} + +static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage, + tusb_control_request_t const *request, + uint_fast8_t stm_idx) +{ + (void)rhport; + videod_streaming_interface_t *self = &_videod_streaming_itf[stm_idx]; + + /* 4.2.1 Interface Control Request */ + switch (TU_U16_HIGH(request->wValue)) { + case VIDEO_VS_CTL_STREAM_ERROR_CODE: + switch (request->bRequest) { + case VIDEO_REQUEST_GET_CUR: + if (stage == CONTROL_STAGE_SETUP) + { + /* TODO */ + TU_VERIFY(tud_control_xfer(rhport, request, &self->error_code, sizeof(uint8_t)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_INFO: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get, sizeof(_cap_get)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + default: break; + } + break; + + case VIDEO_VS_CTL_PROBE: + switch (request->bRequest) { + case VIDEO_REQUEST_SET_CUR: + if (stage == CONTROL_STAGE_SETUP) { + TU_VERIFY(sizeof(video_probe_and_commit_control_t) == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), + VIDEO_ERROR_UNKNOWN); + } else if (stage == CONTROL_STAGE_ACK) { + TU_VERIFY(_update_streaming_parameters(self, (video_probe_and_commit_control_t*)self->ep_buf), + VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_CUR: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_MIN: + case VIDEO_REQUEST_GET_MAX: + case VIDEO_REQUEST_GET_RES: + case VIDEO_REQUEST_GET_DEF: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN); + video_probe_and_commit_control_t tmp; + tmp = *(video_probe_and_commit_control_t*)&self->ep_buf; + TU_VERIFY(_negotiate_streaming_parameters(self, request->bRequest, &tmp), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE); + TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_LEN: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(2 == request->wLength, VIDEO_ERROR_UNKNOWN); + uint16_t len = sizeof(video_probe_and_commit_control_t); + TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)&len, sizeof(len)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_INFO: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get_set, sizeof(_cap_get_set)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + default: break; + } + break; + + case VIDEO_VS_CTL_COMMIT: + switch (request->bRequest) { + case VIDEO_REQUEST_SET_CUR: + if (stage == CONTROL_STAGE_SETUP) { + TU_VERIFY(sizeof(video_probe_and_commit_control_t) == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN); + } else if (stage == CONTROL_STAGE_ACK) { + TU_VERIFY(_update_streaming_parameters(self, (video_probe_and_commit_control_t*)self->ep_buf), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE); + if (tud_video_commit_cb) { + return tud_video_commit_cb(self->index_vc, self->index_vs, (video_probe_and_commit_control_t*)self->ep_buf); + } + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_CUR: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_LEN: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(2 == request->wLength, VIDEO_ERROR_UNKNOWN); + uint16_t len = sizeof(video_probe_and_commit_control_t); + TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)&len, sizeof(len)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + case VIDEO_REQUEST_GET_INFO: + if (stage == CONTROL_STAGE_SETUP) + { + TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN); + TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get_set, sizeof(_cap_get_set)), VIDEO_ERROR_UNKNOWN); + } + return VIDEO_ERROR_NONE; + + default: break; + } + break; + + case VIDEO_VS_CTL_STILL_PROBE: + case VIDEO_VS_CTL_STILL_COMMIT: + case VIDEO_VS_CTL_STILL_IMAGE_TRIGGER: + case VIDEO_VS_CTL_GENERATE_KEY_FRAME: + case VIDEO_VS_CTL_UPDATE_FRAME_SEGMENT: + case VIDEO_VS_CTL_SYNCH_DELAY_CONTROL: + /* TODO */ + break; + + default: break; + } + + /* Unknown/Unsupported request */ + TU_BREAKPOINT(); + return VIDEO_ERROR_INVALID_REQUEST; +} + +static int handle_video_stm_req(uint8_t rhport, uint8_t stage, + tusb_control_request_t const *request, + uint_fast8_t stm_idx) +{ + switch (request->bmRequestType_bit.type) { + case TUSB_REQ_TYPE_STANDARD: + return handle_video_stm_std_req(rhport, stage, request, stm_idx); + + case TUSB_REQ_TYPE_CLASS: + if (TU_U16_HIGH(request->wIndex)) return VIDEO_ERROR_INVALID_REQUEST; + return handle_video_stm_cs_req(rhport, stage, request, stm_idx); + + default: return VIDEO_ERROR_INVALID_REQUEST; + } + return VIDEO_ERROR_UNKNOWN; +} + +//--------------------------------------------------------------------+ +// APPLICATION API +//--------------------------------------------------------------------+ + +bool tud_video_n_connected(uint_fast8_t ctl_idx) +{ + TU_ASSERT(ctl_idx < CFG_TUD_VIDEO); + videod_streaming_interface_t *stm = _get_instance_streaming(ctl_idx, 0); + if (stm) return true; + return false; +} + +bool tud_video_n_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx) +{ + TU_ASSERT(ctl_idx < CFG_TUD_VIDEO); + TU_ASSERT(stm_idx < CFG_TUD_VIDEO_STREAMING); + videod_streaming_interface_t *stm = _get_instance_streaming(ctl_idx, stm_idx); + if (!stm || !stm->desc.ep[0]) return false; + return true; +} + +bool tud_video_n_frame_xfer(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, void *buffer, size_t bufsize) +{ + TU_ASSERT(ctl_idx < CFG_TUD_VIDEO); + TU_ASSERT(stm_idx < CFG_TUD_VIDEO_STREAMING); + if (!buffer || !bufsize) return false; + videod_streaming_interface_t *stm = _get_instance_streaming(ctl_idx, stm_idx); + if (!stm || !stm->desc.ep[0] || stm->buffer) return false; + + /* Find EP address */ + void const *desc = _videod_itf[stm->index_vc].beg; + uint_fast8_t ep_addr = 0; + for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO_STREAMING; ++i) { + uint_fast16_t ofs_ep = stm->desc.ep[i]; + if (!ofs_ep) continue; + ep_addr = _desc_ep_addr(desc + ofs_ep); + break; + } + if (!ep_addr) return false; + + TU_VERIFY( usbd_edpt_claim(0, ep_addr)); + /* update the packet header */ + tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm->ep_buf; + hdr->FrameID ^= 1; + hdr->EndOfFrame = 0; + /* update the packet data */ + stm->buffer = (uint8_t*)buffer; + stm->bufsize = bufsize; + uint_fast16_t pkt_len = _prepare_in_payload(stm); + TU_ASSERT( usbd_edpt_xfer(0, ep_addr, stm->ep_buf, pkt_len), 0); + return true; +} + +//--------------------------------------------------------------------+ +// USBD Driver API +//--------------------------------------------------------------------+ +void videod_init(void) +{ + for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO; ++i) { + videod_interface_t* ctl = &_videod_itf[i]; + tu_memclr(ctl, sizeof(*ctl)); + } + for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO_STREAMING; ++i) { + videod_streaming_interface_t *stm = &_videod_streaming_itf[i]; + tu_memclr(stm, ITF_STM_MEM_RESET_SIZE); + } +} + +void videod_reset(uint8_t rhport) +{ + (void) rhport; + for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO; ++i) { + videod_interface_t* ctl = &_videod_itf[i]; + tu_memclr(ctl, sizeof(*ctl)); + } + for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO_STREAMING; ++i) { + videod_streaming_interface_t *stm = &_videod_streaming_itf[i]; + tu_memclr(stm, ITF_STM_MEM_RESET_SIZE); + } +} + +uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) +{ + TU_VERIFY((TUSB_CLASS_VIDEO == itf_desc->bInterfaceClass) && + (VIDEO_SUBCLASS_CONTROL == itf_desc->bInterfaceSubClass) && + (VIDEO_ITF_PROTOCOL_15 == itf_desc->bInterfaceProtocol), 0); + + /* Find available interface */ + videod_interface_t *self = NULL; + uint_fast8_t ctl_idx; + for (ctl_idx = 0; ctl_idx < CFG_TUD_VIDEO; ++ctl_idx) { + if (_videod_itf[ctl_idx].beg) continue; + self = &_videod_itf[ctl_idx]; + break; + } + TU_ASSERT(ctl_idx < CFG_TUD_VIDEO, 0); + + void const *end = (void const*)itf_desc + max_len; + self->beg = itf_desc; + self->len = max_len; + /*------------- Video Control Interface -------------*/ + TU_VERIFY(_open_vc_itf(rhport, self, 0), 0); + tusb_desc_vc_itf_t const *vc = _get_desc_vc(self); + uint_fast8_t bInCollection = vc->ctl.bInCollection; + /* Find the end of the video interface descriptor */ + void const *cur = _next_desc_itf(itf_desc, end); + for (uint_fast8_t stm_idx = 0; stm_idx < bInCollection; ++stm_idx) { + videod_streaming_interface_t *stm = NULL; + /* find free streaming interface handle */ + for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO_STREAMING; ++i) { + if (_videod_streaming_itf[i].desc.beg) continue; + stm = &_videod_streaming_itf[i]; + self->stm[stm_idx] = i; + break; + } + TU_ASSERT(stm, 0); + stm->index_vc = ctl_idx; + stm->index_vs = stm_idx; + stm->desc.beg = (uintptr_t)cur - (uintptr_t)itf_desc; + cur = _next_desc_itf(cur, end); + stm->desc.end = (uintptr_t)cur - (uintptr_t)itf_desc; + } + self->len = (uintptr_t)cur - (uintptr_t)itf_desc; + return (uintptr_t)cur - (uintptr_t)itf_desc; +} + +// Invoked when a control transfer occurred on an interface of this class +// Driver response accordingly to the request and the transfer stage (setup/data/ack) +// return false to stall control endpoint (e.g unsupported request) +bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) +{ + int err; + TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE); + uint_fast8_t itfnum = tu_u16_low(request->wIndex); + + /* Identify which control interface to use */ + uint_fast8_t itf; + for (itf = 0; itf < CFG_TUD_VIDEO; ++itf) { + void const *desc = _videod_itf[itf].beg; + if (!desc) continue; + if (itfnum == _desc_itfnum(desc)) break; + } + + if (itf < CFG_TUD_VIDEO) { + err = handle_video_ctl_req(rhport, stage, request, itf); + _videod_itf[itf].error_code = (uint8_t)err; + if (err) return false; + return true; + } + + /* Identify which streaming interface to use */ + for (itf = 0; itf < CFG_TUD_VIDEO_STREAMING; ++itf) { + videod_streaming_interface_t *stm = &_videod_streaming_itf[itf]; + if (!stm->desc.beg) continue; + void const *desc = _videod_itf[stm->index_vc].beg; + if (itfnum == _desc_itfnum(desc + stm->desc.beg)) break; + } + + if (itf < CFG_TUD_VIDEO_STREAMING) { + err = handle_video_stm_req(rhport, stage, request, itf); + _videod_streaming_itf[itf].error_code = (uint8_t)err; + if (err) return false; + return true; + } + return false; +} + +bool videod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void)result; (void)xferred_bytes; + + /* find streaming handle */ + uint_fast8_t itf; + videod_interface_t *ctl; + videod_streaming_interface_t *stm; + for (itf = 0; itf < CFG_TUD_VIDEO_STREAMING; ++itf) { + stm = &_videod_streaming_itf[itf]; + uint_fast16_t const ep_ofs = stm->desc.ep[0]; + if (!ep_ofs) continue; + ctl = &_videod_itf[stm->index_vc]; + void const *desc = ctl->beg; + if (ep_addr == _desc_ep_addr(desc + ep_ofs)) break; + } + + TU_ASSERT(itf < CFG_TUD_VIDEO_STREAMING); + if (stm->offset < stm->bufsize) { + /* Claim the endpoint */ + TU_VERIFY( usbd_edpt_claim(rhport, ep_addr), 0); + uint_fast16_t pkt_len = _prepare_in_payload(stm); + TU_ASSERT( usbd_edpt_xfer(rhport, ep_addr, stm->ep_buf, pkt_len), 0); + } else { + stm->buffer = NULL; + stm->bufsize = 0; + stm->offset = 0; + if (tud_video_frame_xfer_complete_cb) { + tud_video_frame_xfer_complete_cb(stm->index_vc, stm->index_vs); + } + } + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video_device.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video_device.h new file mode 100644 index 000000000..ee2fcb9d5 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/class/video/video_device.h @@ -0,0 +1,97 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * Copyright (c) 2021 Koji KITAYAMA + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef TUSB_VIDEO_DEVICE_H_ +#define TUSB_VIDEO_DEVICE_H_ + +#include "common/tusb_common.h" +#include "video.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Application API (Multiple Ports) +// CFG_TUD_VIDEO > 1 +//--------------------------------------------------------------------+ + +/** Return true if streaming + * + * @param[in] ctl_idx Destination control interface index + * @param[in] stm_idx Destination streaming interface index */ +bool tud_video_n_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx); + +/** Transfer a frame + * + * @param[in] ctl_idx Destination control interface index + * @param[in] stm_idx Destination streaming interface index + * @param[in] buffer Frame buffer. The caller must not use this buffer until the operation is completed. + * @param[in] bufsize Byte size of the frame buffer */ +bool tud_video_n_frame_xfer(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, void *buffer, size_t bufsize); + +/*------------- Optional callbacks -------------*/ +/** Invoked when compeletion of a frame transfer + * + * @param[in] ctl_idx Destination control interface index + * @param[in] stm_idx Destination streaming interface index */ +TU_ATTR_WEAK void tud_video_frame_xfer_complete_cb(uint_fast8_t ctl_idx, uint_fast8_t stm_idx); + +//--------------------------------------------------------------------+ +// Application Callback API (weak is optional) +//--------------------------------------------------------------------+ + +/** Invoked when SET_POWER_MODE request received + * + * @param[in] ctl_idx Destination control interface index + * @param[in] stm_idx Destination streaming interface index + * @return video_error_code_t */ +TU_ATTR_WEAK int tud_video_power_mode_cb(uint_fast8_t ctl_idx, uint8_t power_mod); + +/** Invoked when VS_COMMIT_CONTROL(SET_CUR) request received + * + * @param[in] ctl_idx Destination control interface index + * @param[in] stm_idx Destination streaming interface index + * @param[in] parameters Video streaming parameters + * @return video_error_code_t */ +TU_ATTR_WEAK int tud_video_commit_cb(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, + video_probe_and_commit_control_t const *parameters); + +//--------------------------------------------------------------------+ +// INTERNAL USBD-CLASS DRIVER API +//--------------------------------------------------------------------+ +void videod_init (void); +void videod_reset (uint8_t rhport); +uint16_t videod_open (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len); +bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); +bool videod_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); + +#ifdef __cplusplus + } +#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_common.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_common.h new file mode 100644 index 000000000..9b9e2b007 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_common.h @@ -0,0 +1,406 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_COMMON_H_ +#define _TUSB_COMMON_H_ + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Macros Helper +//--------------------------------------------------------------------+ +#define TU_ARRAY_SIZE(_arr) ( sizeof(_arr) / sizeof(_arr[0]) ) +#define TU_MIN(_x, _y) ( ( (_x) < (_y) ) ? (_x) : (_y) ) +#define TU_MAX(_x, _y) ( ( (_x) > (_y) ) ? (_x) : (_y) ) + +#define TU_U16_HIGH(_u16) ((uint8_t) (((_u16) >> 8) & 0x00ff)) +#define TU_U16_LOW(_u16) ((uint8_t) ((_u16) & 0x00ff)) +#define U16_TO_U8S_BE(_u16) TU_U16_HIGH(_u16), TU_U16_LOW(_u16) +#define U16_TO_U8S_LE(_u16) TU_U16_LOW(_u16), TU_U16_HIGH(_u16) + +#define TU_U32_BYTE3(_u32) ((uint8_t) ((((uint32_t) _u32) >> 24) & 0x000000ff)) // MSB +#define TU_U32_BYTE2(_u32) ((uint8_t) ((((uint32_t) _u32) >> 16) & 0x000000ff)) +#define TU_U32_BYTE1(_u32) ((uint8_t) ((((uint32_t) _u32) >> 8) & 0x000000ff)) +#define TU_U32_BYTE0(_u32) ((uint8_t) (((uint32_t) _u32) & 0x000000ff)) // LSB + +#define U32_TO_U8S_BE(_u32) TU_U32_BYTE3(_u32), TU_U32_BYTE2(_u32), TU_U32_BYTE1(_u32), TU_U32_BYTE0(_u32) +#define U32_TO_U8S_LE(_u32) TU_U32_BYTE0(_u32), TU_U32_BYTE1(_u32), TU_U32_BYTE2(_u32), TU_U32_BYTE3(_u32) + +#define TU_BIT(n) (1UL << (n)) +#define TU_GENMASK(h, l) ( (UINT32_MAX << (l)) & (UINT32_MAX >> (31 - (h))) ) + +//--------------------------------------------------------------------+ +// Includes +//--------------------------------------------------------------------+ + +// Standard Headers +#include +#include +#include +#include +#include + +// Tinyusb Common Headers +#include "tusb_option.h" +#include "tusb_compiler.h" +#include "tusb_verify.h" +#include "tusb_types.h" + +#include "tusb_error.h" // TODO remove +#include "tusb_timeout.h" // TODO remove + +//--------------------------------------------------------------------+ +// Internal Helper used by Host and Device Stack +//--------------------------------------------------------------------+ + +// Check if endpoint descriptor is valid per USB specs +bool tu_edpt_validate(tusb_desc_endpoint_t const * desc_ep, tusb_speed_t speed); + +// Bind all endpoint of a interface descriptor to class driver +void tu_edpt_bind_driver(uint8_t ep2drv[][2], tusb_desc_interface_t const* p_desc, uint16_t desc_len, uint8_t driver_id); + +// Calculate total length of n interfaces (depending on IAD) +uint16_t tu_desc_get_interface_total_len(tusb_desc_interface_t const* desc_itf, uint8_t itf_count, uint16_t max_len); + +//--------------------------------------------------------------------+ +// Internal Inline Functions +//--------------------------------------------------------------------+ + +//------------- Mem -------------// +#define tu_memclr(buffer, size) memset((buffer), 0, (size)) +#define tu_varclr(_var) tu_memclr(_var, sizeof(*(_var))) + +//------------- Bytes -------------// +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_u32(uint8_t b3, uint8_t b2, uint8_t b1, uint8_t b0) +{ + return ( ((uint32_t) b3) << 24) | ( ((uint32_t) b2) << 16) | ( ((uint32_t) b1) << 8) | b0; +} + +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_u16(uint8_t high, uint8_t low) +{ + return (uint16_t) ((((uint16_t) high) << 8) | low); +} + +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u32_byte3(uint32_t ui32) { return TU_U32_BYTE3(ui32); } +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u32_byte2(uint32_t ui32) { return TU_U32_BYTE2(ui32); } +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u32_byte1(uint32_t ui32) { return TU_U32_BYTE1(ui32); } +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u32_byte0(uint32_t ui32) { return TU_U32_BYTE0(ui32); } + +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_u32_high16(uint32_t ui32) { return (uint16_t) (ui32 >> 16); } +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_u32_low16 (uint32_t ui32) { return (uint16_t) (ui32 & 0x0000ffffu); } + +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u16_high(uint16_t ui16) { return TU_U16_HIGH(ui16); } +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u16_low (uint16_t ui16) { return TU_U16_LOW(ui16); } + +//------------- Bits -------------// +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_bit_set (uint32_t value, uint8_t pos) { return value | TU_BIT(pos); } +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_bit_clear(uint32_t value, uint8_t pos) { return value & (~TU_BIT(pos)); } +TU_ATTR_ALWAYS_INLINE static inline bool tu_bit_test (uint32_t value, uint8_t pos) { return (value & TU_BIT(pos)) ? true : false; } + +//------------- Min -------------// +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_min8 (uint8_t x, uint8_t y ) { return (x < y) ? x : y; } +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_min16 (uint16_t x, uint16_t y) { return (x < y) ? x : y; } +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_min32 (uint32_t x, uint32_t y) { return (x < y) ? x : y; } + +//------------- Max -------------// +TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_max8 (uint8_t x, uint8_t y ) { return (x > y) ? x : y; } +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_max16 (uint16_t x, uint16_t y) { return (x > y) ? x : y; } +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_max32 (uint32_t x, uint32_t y) { return (x > y) ? x : y; } + +//------------- Align -------------// +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_align(uint32_t value, uint32_t alignment) +{ + return value & ((uint32_t) ~(alignment-1)); +} + +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_align16 (uint32_t value) { return (value & 0xFFFFFFF0UL); } +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_align32 (uint32_t value) { return (value & 0xFFFFFFE0UL); } +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_align4k (uint32_t value) { return (value & 0xFFFFF000UL); } +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_offset4k(uint32_t value) { return (value & 0xFFFUL); } + +//------------- Mathematics -------------// +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_div_ceil(uint32_t v, uint32_t d) { return (v + d -1)/d; } + +/// inclusive range checking TODO remove +TU_ATTR_ALWAYS_INLINE static inline bool tu_within(uint32_t lower, uint32_t value, uint32_t upper) +{ + return (lower <= value) && (value <= upper); +} + +// log2 of a value is its MSB's position +// TODO use clz TODO remove +static inline uint8_t tu_log2(uint32_t value) +{ + uint8_t result = 0; + while (value >>= 1) { result++; } + return result; +} + +//------------- Unaligned Access -------------// +#if TUP_ARCH_STRICT_ALIGN + +// Rely on compiler to generate correct code for unaligned access +typedef struct { uint16_t val; } TU_ATTR_PACKED tu_unaligned_uint16_t; +typedef struct { uint32_t val; } TU_ATTR_PACKED tu_unaligned_uint32_t; + +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_unaligned_read32(const void* mem) +{ + tu_unaligned_uint32_t const* ua32 = (tu_unaligned_uint32_t const*) mem; + return ua32->val; +} + +TU_ATTR_ALWAYS_INLINE static inline void tu_unaligned_write32(void* mem, uint32_t value) +{ + tu_unaligned_uint32_t* ua32 = (tu_unaligned_uint32_t*) mem; + ua32->val = value; +} + +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_unaligned_read16(const void* mem) +{ + tu_unaligned_uint16_t const* ua16 = (tu_unaligned_uint16_t const*) mem; + return ua16->val; +} + +TU_ATTR_ALWAYS_INLINE static inline void tu_unaligned_write16(void* mem, uint16_t value) +{ + tu_unaligned_uint16_t* ua16 = (tu_unaligned_uint16_t*) mem; + ua16->val = value; +} + +#elif TUP_MCU_STRICT_ALIGN + +// MCU such as LPC_IP3511 Highspeed cannot access unaligned memory on USB_RAM although it is ARM M4. +// We have to manually pick up bytes since tu_unaligned_uint32_t will still generate unaligned code +// NOTE: volatile cast to memory to prevent compiler to optimize and generate unaligned code +// TODO Big Endian may need minor changes +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_unaligned_read32(const void* mem) +{ + volatile uint8_t const* buf8 = (uint8_t const*) mem; + return tu_u32(buf8[3], buf8[2], buf8[1], buf8[0]); +} + +TU_ATTR_ALWAYS_INLINE static inline void tu_unaligned_write32(void* mem, uint32_t value) +{ + volatile uint8_t* buf8 = (uint8_t*) mem; + buf8[0] = tu_u32_byte0(value); + buf8[1] = tu_u32_byte1(value); + buf8[2] = tu_u32_byte2(value); + buf8[3] = tu_u32_byte3(value); +} + +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_unaligned_read16(const void* mem) +{ + volatile uint8_t const* buf8 = (uint8_t const*) mem; + return tu_u16(buf8[1], buf8[0]); +} + +TU_ATTR_ALWAYS_INLINE static inline void tu_unaligned_write16(void* mem, uint16_t value) +{ + volatile uint8_t* buf8 = (uint8_t*) mem; + buf8[0] = tu_u16_low(value); + buf8[1] = tu_u16_high(value); +} + + +#else + +// MCU that could access unaligned memory natively +TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_unaligned_read32 (const void* mem) { return *((uint32_t const *) mem); } +TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_unaligned_read16 (const void* mem) { return *((uint16_t const *) mem); } + +TU_ATTR_ALWAYS_INLINE static inline void tu_unaligned_write32 (void* mem, uint32_t value ) { *((uint32_t*) mem) = value; } +TU_ATTR_ALWAYS_INLINE static inline void tu_unaligned_write16 (void* mem, uint16_t value ) { *((uint16_t*) mem) = value; } + +#endif + +// To be removed +//------------- Binary constant -------------// +#if defined(__GNUC__) && !defined(__CC_ARM) + +#define TU_BIN8(x) ((uint8_t) (0b##x)) +#define TU_BIN16(b1, b2) ((uint16_t) (0b##b1##b2)) +#define TU_BIN32(b1, b2, b3, b4) ((uint32_t) (0b##b1##b2##b3##b4)) + +#else + +// internal macro of B8, B16, B32 +#define _B8__(x) (((x&0x0000000FUL)?1:0) \ + +((x&0x000000F0UL)?2:0) \ + +((x&0x00000F00UL)?4:0) \ + +((x&0x0000F000UL)?8:0) \ + +((x&0x000F0000UL)?16:0) \ + +((x&0x00F00000UL)?32:0) \ + +((x&0x0F000000UL)?64:0) \ + +((x&0xF0000000UL)?128:0)) + +#define TU_BIN8(d) ((uint8_t) _B8__(0x##d##UL)) +#define TU_BIN16(dmsb,dlsb) (((uint16_t)TU_BIN8(dmsb)<<8) + TU_BIN8(dlsb)) +#define TU_BIN32(dmsb,db2,db3,dlsb) \ + (((uint32_t)TU_BIN8(dmsb)<<24) \ + + ((uint32_t)TU_BIN8(db2)<<16) \ + + ((uint32_t)TU_BIN8(db3)<<8) \ + + TU_BIN8(dlsb)) +#endif + +//--------------------------------------------------------------------+ +// Debug Function +//--------------------------------------------------------------------+ + +// CFG_TUSB_DEBUG for debugging +// 0 : no debug +// 1 : print error +// 2 : print warning +// 3 : print info +#if CFG_TUSB_DEBUG + +void tu_print_mem(void const *buf, uint32_t count, uint8_t indent); + +#ifdef CFG_TUSB_DEBUG_PRINTF + extern int CFG_TUSB_DEBUG_PRINTF(const char *format, ...); + #define tu_printf CFG_TUSB_DEBUG_PRINTF +#else + #define tu_printf printf +#endif + +static inline +void tu_print_var(uint8_t const* buf, uint32_t bufsize) +{ + for(uint32_t i=0; i= 2 + #define TU_LOG2 TU_LOG1 + #define TU_LOG2_MEM TU_LOG1_MEM + #define TU_LOG2_VAR TU_LOG1_VAR + #define TU_LOG2_INT TU_LOG1_INT + #define TU_LOG2_HEX TU_LOG1_HEX +#endif + +// Log Level 3: Info +#if CFG_TUSB_DEBUG >= 3 + #define TU_LOG3 TU_LOG1 + #define TU_LOG3_MEM TU_LOG1_MEM + #define TU_LOG3_VAR TU_LOG1_VAR + #define TU_LOG3_INT TU_LOG1_INT + #define TU_LOG3_HEX TU_LOG1_HEX +#endif + +typedef struct +{ + uint32_t key; + const char* data; +} tu_lookup_entry_t; + +typedef struct +{ + uint16_t count; + tu_lookup_entry_t const* items; +} tu_lookup_table_t; + +static inline const char* tu_lookup_find(tu_lookup_table_t const* p_table, uint32_t key) +{ + static char not_found[11]; + + for(uint16_t i=0; icount; i++) + { + if (p_table->items[i].key == key) return p_table->items[i].data; + } + + // not found return the key value in hex + sprintf(not_found, "0x%08lX", (unsigned long) key); + + return not_found; +} + +#endif // CFG_TUSB_DEBUG + +#ifndef TU_LOG +#define TU_LOG(n, ...) +#define TU_LOG_MEM(n, ...) +#define TU_LOG_VAR(n, ...) +#define TU_LOG_INT(n, ...) +#define TU_LOG_HEX(n, ...) +#define TU_LOG_LOCATION() +#define TU_LOG_FAILED() +#endif + +// TODO replace all TU_LOGn with TU_LOG(n) + +#define TU_LOG0(...) +#define TU_LOG0_MEM(...) +#define TU_LOG0_VAR(...) +#define TU_LOG0_INT(...) +#define TU_LOG0_HEX(...) + + +#ifndef TU_LOG1 + #define TU_LOG1(...) + #define TU_LOG1_MEM(...) + #define TU_LOG1_VAR(...) + #define TU_LOG1_INT(...) + #define TU_LOG1_HEX(...) +#endif + +#ifndef TU_LOG2 + #define TU_LOG2(...) + #define TU_LOG2_MEM(...) + #define TU_LOG2_VAR(...) + #define TU_LOG2_INT(...) + #define TU_LOG2_HEX(...) +#endif + +#ifndef TU_LOG3 + #define TU_LOG3(...) + #define TU_LOG3_MEM(...) + #define TU_LOG3_VAR(...) + #define TU_LOG3_INT(...) + #define TU_LOG3_HEX(...) +#endif + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_COMMON_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_compiler.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_compiler.h new file mode 100644 index 000000000..38e6a16d0 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_compiler.h @@ -0,0 +1,260 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup Group_Common + * \defgroup Group_Compiler Compiler + * \brief Group_Compiler brief + * @{ */ + +#ifndef _TUSB_COMPILER_H_ +#define _TUSB_COMPILER_H_ + +#define TU_TOKEN(x) x +#define TU_STRING(x) #x ///< stringify without expand +#define TU_XSTRING(x) TU_STRING(x) ///< expand then stringify + +#define TU_STRCAT(a, b) a##b ///< concat without expand +#define TU_STRCAT3(a, b, c) a##b##c ///< concat without expand + +#define TU_XSTRCAT(a, b) TU_STRCAT(a, b) ///< expand then concat +#define TU_XSTRCAT3(a, b, c) TU_STRCAT3(a, b, c) ///< expand then concat 3 tokens + +#define TU_INCLUDE_PATH(_dir,_file) TU_XSTRING( TU_TOKEN(_dir)TU_TOKEN(_file) ) + +#if defined __COUNTER__ && __COUNTER__ != __COUNTER__ + #define _TU_COUNTER_ __COUNTER__ +#else + #define _TU_COUNTER_ __LINE__ +#endif + +// Compile-time Assert +#if defined (__STDC_VERSION__) && __STDC_VERSION__ >= 201112L + #define TU_VERIFY_STATIC _Static_assert +#elif defined (__cplusplus) && __cplusplus >= 201103L + #define TU_VERIFY_STATIC static_assert +#elif defined(__CCRX__) + #define TU_VERIFY_STATIC(const_expr, _mess) typedef char TU_XSTRCAT(Line, __LINE__)[(const_expr) ? 1 : 0]; +#else + #define TU_VERIFY_STATIC(const_expr, _mess) enum { TU_XSTRCAT(_verify_static_, _TU_COUNTER_) = 1/(!!(const_expr)) } +#endif + +// for declaration of reserved field, make use of _TU_COUNTER_ +#define TU_RESERVED TU_XSTRCAT(reserved, _TU_COUNTER_) + +#define TU_LITTLE_ENDIAN (0x12u) +#define TU_BIG_ENDIAN (0x21u) + +/*------------------------------------------------------------------*/ +/* Count number of arguments of __VA_ARGS__ + * - reference https://stackoverflow.com/questions/2124339/c-preprocessor-va-args-number-of-arguments + * - _GET_NTH_ARG() takes args >= N (64) but only expand to Nth one (64th) + * - _RSEQ_N() is reverse sequential to N to add padding to have + * Nth position is the same as the number of arguments + * - ##__VA_ARGS__ is used to deal with 0 paramerter (swallows comma) + *------------------------------------------------------------------*/ +#define TU_ARGS_NUM(...) _TU_NARG(_0, ##__VA_ARGS__,_RSEQ_N()) + +#define _TU_NARG(...) _GET_NTH_ARG(__VA_ARGS__) +#define _GET_NTH_ARG( \ + _1, _2, _3, _4, _5, _6, _7, _8, _9,_10, \ + _11,_12,_13,_14,_15,_16,_17,_18,_19,_20, \ + _21,_22,_23,_24,_25,_26,_27,_28,_29,_30, \ + _31,_32,_33,_34,_35,_36,_37,_38,_39,_40, \ + _41,_42,_43,_44,_45,_46,_47,_48,_49,_50, \ + _51,_52,_53,_54,_55,_56,_57,_58,_59,_60, \ + _61,_62,_63,N,...) N +#define _RSEQ_N() \ + 62,61,60, \ + 59,58,57,56,55,54,53,52,51,50, \ + 49,48,47,46,45,44,43,42,41,40, \ + 39,38,37,36,35,34,33,32,31,30, \ + 29,28,27,26,25,24,23,22,21,20, \ + 19,18,17,16,15,14,13,12,11,10, \ + 9,8,7,6,5,4,3,2,1,0 + +// Apply an macro X to each of the arguments with an separated of choice +#define TU_ARGS_APPLY(_X, _s, ...) TU_XSTRCAT(_TU_ARGS_APPLY_, TU_ARGS_NUM(__VA_ARGS__))(_X, _s, __VA_ARGS__) + +#define _TU_ARGS_APPLY_1(_X, _s, _a1) _X(_a1) +#define _TU_ARGS_APPLY_2(_X, _s, _a1, _a2) _X(_a1) _s _X(_a2) +#define _TU_ARGS_APPLY_3(_X, _s, _a1, _a2, _a3) _X(_a1) _s _TU_ARGS_APPLY_2(_X, _s, _a2, _a3) +#define _TU_ARGS_APPLY_4(_X, _s, _a1, _a2, _a3, _a4) _X(_a1) _s _TU_ARGS_APPLY_3(_X, _s, _a2, _a3, _a4) +#define _TU_ARGS_APPLY_5(_X, _s, _a1, _a2, _a3, _a4, _a5) _X(_a1) _s _TU_ARGS_APPLY_4(_X, _s, _a2, _a3, _a4, _a5) +#define _TU_ARGS_APPLY_6(_X, _s, _a1, _a2, _a3, _a4, _a5, _a6) _X(_a1) _s _TU_ARGS_APPLY_5(_X, _s, _a2, _a3, _a4, _a5, _a6) +#define _TU_ARGS_APPLY_7(_X, _s, _a1, _a2, _a3, _a4, _a5, _a6, _a7) _X(_a1) _s _TU_ARGS_APPLY_6(_X, _s, _a2, _a3, _a4, _a5, _a6, _a7) +#define _TU_ARGS_APPLY_8(_X, _s, _a1, _a2, _a3, _a4, _a5, _a6, _a7, _a8) _X(_a1) _s _TU_ARGS_APPLY_7(_X, _s, _a2, _a3, _a4, _a5, _a6, _a7, _a8) + +//--------------------------------------------------------------------+ +// Compiler porting with Attribute and Endian +//--------------------------------------------------------------------+ + +// TODO refactor since __attribute__ is supported across many compiler +#if defined(__GNUC__) + #define TU_ATTR_ALIGNED(Bytes) __attribute__ ((aligned(Bytes))) + #define TU_ATTR_SECTION(sec_name) __attribute__ ((section(#sec_name))) + #define TU_ATTR_PACKED __attribute__ ((packed)) + #define TU_ATTR_WEAK __attribute__ ((weak)) + #define TU_ATTR_ALWAYS_INLINE __attribute__ ((always_inline)) + #define TU_ATTR_DEPRECATED(mess) __attribute__ ((deprecated(mess))) // warn if function with this attribute is used + #define TU_ATTR_UNUSED __attribute__ ((unused)) // Function/Variable is meant to be possibly unused + #define TU_ATTR_USED __attribute__ ((used)) // Function/Variable is meant to be used + + #define TU_ATTR_PACKED_BEGIN + #define TU_ATTR_PACKED_END + #define TU_ATTR_BIT_FIELD_ORDER_BEGIN + #define TU_ATTR_BIT_FIELD_ORDER_END + + // Endian conversion use well-known host to network (big endian) naming + #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + #define TU_BYTE_ORDER TU_LITTLE_ENDIAN + #else + #define TU_BYTE_ORDER TU_BIG_ENDIAN + #endif + + #define TU_BSWAP16(u16) (__builtin_bswap16(u16)) + #define TU_BSWAP32(u32) (__builtin_bswap32(u32)) + + #ifndef __ARMCC_VERSION + // List of obsolete callback function that is renamed and should not be defined. + // Put it here since only gcc support this pragma + #pragma GCC poison tud_vendor_control_request_cb + #endif + +#elif defined(__TI_COMPILER_VERSION__) + #define TU_ATTR_ALIGNED(Bytes) __attribute__ ((aligned(Bytes))) + #define TU_ATTR_SECTION(sec_name) __attribute__ ((section(#sec_name))) + #define TU_ATTR_PACKED __attribute__ ((packed)) + #define TU_ATTR_WEAK __attribute__ ((weak)) + #define TU_ATTR_ALWAYS_INLINE __attribute__ ((always_inline)) + #define TU_ATTR_DEPRECATED(mess) __attribute__ ((deprecated(mess))) // warn if function with this attribute is used + #define TU_ATTR_UNUSED __attribute__ ((unused)) // Function/Variable is meant to be possibly unused + #define TU_ATTR_USED __attribute__ ((used)) + + #define TU_ATTR_PACKED_BEGIN + #define TU_ATTR_PACKED_END + #define TU_ATTR_BIT_FIELD_ORDER_BEGIN + #define TU_ATTR_BIT_FIELD_ORDER_END + + // __BYTE_ORDER is defined in the TI ARM compiler, but not MSP430 (which is little endian) + #if ((__BYTE_ORDER__) == (__ORDER_LITTLE_ENDIAN__)) || defined(__MSP430__) + #define TU_BYTE_ORDER TU_LITTLE_ENDIAN + #else + #define TU_BYTE_ORDER TU_BIG_ENDIAN + #endif + + #define TU_BSWAP16(u16) (__builtin_bswap16(u16)) + #define TU_BSWAP32(u32) (__builtin_bswap32(u32)) + +#elif defined(__ICCARM__) + #include + #define TU_ATTR_ALIGNED(Bytes) __attribute__ ((aligned(Bytes))) + #define TU_ATTR_SECTION(sec_name) __attribute__ ((section(#sec_name))) + #define TU_ATTR_PACKED __attribute__ ((packed)) + #define TU_ATTR_WEAK __attribute__ ((weak)) + #define TU_ATTR_ALWAYS_INLINE __attribute__ ((always_inline)) + #define TU_ATTR_DEPRECATED(mess) __attribute__ ((deprecated(mess))) // warn if function with this attribute is used + #define TU_ATTR_UNUSED __attribute__ ((unused)) // Function/Variable is meant to be possibly unused + #define TU_ATTR_USED __attribute__ ((used)) // Function/Variable is meant to be used + + #define TU_ATTR_PACKED_BEGIN + #define TU_ATTR_PACKED_END + #define TU_ATTR_BIT_FIELD_ORDER_BEGIN + #define TU_ATTR_BIT_FIELD_ORDER_END + + // Endian conversion use well-known host to network (big endian) naming + #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ + #define TU_BYTE_ORDER TU_LITTLE_ENDIAN + #else + #define TU_BYTE_ORDER TU_BIG_ENDIAN + #endif + + #define TU_BSWAP16(u16) (__iar_builtin_REV16(u16)) + #define TU_BSWAP32(u32) (__iar_builtin_REV(u32)) + +#elif defined(__CCRX__) + #define TU_ATTR_ALIGNED(Bytes) + #define TU_ATTR_SECTION(sec_name) + #define TU_ATTR_PACKED + #define TU_ATTR_WEAK + #define TU_ATTR_ALWAYS_INLINE + #define TU_ATTR_DEPRECATED(mess) + #define TU_ATTR_UNUSED + #define TU_ATTR_USED + + #define TU_ATTR_PACKED_BEGIN _Pragma("pack") + #define TU_ATTR_PACKED_END _Pragma("packoption") + #define TU_ATTR_BIT_FIELD_ORDER_BEGIN _Pragma("bit_order right") + #define TU_ATTR_BIT_FIELD_ORDER_END _Pragma("bit_order") + + // Endian conversion use well-known host to network (big endian) naming + #if defined(__LIT) + #define TU_BYTE_ORDER TU_LITTLE_ENDIAN + #else + #define TU_BYTE_ORDER TU_BIG_ENDIAN + #endif + + #define TU_BSWAP16(u16) ((unsigned short)_builtin_revw((unsigned long)u16)) + #define TU_BSWAP32(u32) (_builtin_revl(u32)) + +#else + #error "Compiler attribute porting is required" +#endif + +#if (TU_BYTE_ORDER == TU_LITTLE_ENDIAN) + + #define tu_htons(u16) (TU_BSWAP16(u16)) + #define tu_ntohs(u16) (TU_BSWAP16(u16)) + + #define tu_htonl(u32) (TU_BSWAP32(u32)) + #define tu_ntohl(u32) (TU_BSWAP32(u32)) + + #define tu_htole16(u16) (u16) + #define tu_le16toh(u16) (u16) + + #define tu_htole32(u32) (u32) + #define tu_le32toh(u32) (u32) + +#elif (TU_BYTE_ORDER == TU_BIG_ENDIAN) + + #define tu_htons(u16) (u16) + #define tu_ntohs(u16) (u16) + + #define tu_htonl(u32) (u32) + #define tu_ntohl(u32) (u32) + + #define tu_htole16(u16) (TU_BSWAP16(u16)) + #define tu_le16toh(u16) (TU_BSWAP16(u16)) + + #define tu_htole32(u32) (TU_BSWAP32(u32)) + #define tu_le32toh(u32) (TU_BSWAP32(u32)) + +#else + #error Byte order is undefined +#endif + +#endif /* _TUSB_COMPILER_H_ */ + +/// @} diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_error.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_error.h new file mode 100644 index 000000000..d7ad8c318 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_error.h @@ -0,0 +1,75 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup Group_Common + * \defgroup Group_Error Error Codes + * @{ */ + +#ifndef _TUSB_ERRORS_H_ +#define _TUSB_ERRORS_H_ + +#include "tusb_option.h" + +#ifdef __cplusplus + extern "C" { +#endif + +#define ERROR_ENUM(x) x, +#define ERROR_STRING(x) #x, + +#define ERROR_TABLE(ENTRY) \ + ENTRY(TUSB_ERROR_NONE )\ + ENTRY(TUSB_ERROR_INVALID_PARA )\ + ENTRY(TUSB_ERROR_DEVICE_NOT_READY )\ + ENTRY(TUSB_ERROR_INTERFACE_IS_BUSY )\ + ENTRY(TUSB_ERROR_HCD_OPEN_PIPE_FAILED )\ + ENTRY(TUSB_ERROR_OSAL_TIMEOUT )\ + ENTRY(TUSB_ERROR_CDCH_DEVICE_NOT_MOUNTED )\ + ENTRY(TUSB_ERROR_MSCH_DEVICE_NOT_MOUNTED )\ + ENTRY(TUSB_ERROR_NOT_SUPPORTED )\ + ENTRY(TUSB_ERROR_NOT_ENOUGH_MEMORY )\ + ENTRY(TUSB_ERROR_FAILED )\ + +/// \brief Error Code returned +/// TODO obsolete and to be remove +typedef enum +{ + ERROR_TABLE(ERROR_ENUM) + TUSB_ERROR_COUNT +}tusb_error_t; + +#if CFG_TUSB_DEBUG +/// Enum to String for debugging purposes. Only available if \ref CFG_TUSB_DEBUG > 0 +extern char const* const tusb_strerr[TUSB_ERROR_COUNT]; +#endif + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_ERRORS_H_ */ + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_fifo.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_fifo.c new file mode 100644 index 000000000..183c9c6fc --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_fifo.c @@ -0,0 +1,1007 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * Copyright (c) 2020 Reinhard Panhuber - rework to unmasked pointers + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "osal/osal.h" +#include "tusb_fifo.h" + +// Supress IAR warning +// Warning[Pa082]: undefined behavior: the order of volatile accesses is undefined in this statement +#if defined(__ICCARM__) +#pragma diag_suppress = Pa082 +#endif + +// implement mutex lock and unlock +#if CFG_FIFO_MUTEX + +static inline void _ff_lock(tu_fifo_mutex_t mutex) +{ + if (mutex) osal_mutex_lock(mutex, OSAL_TIMEOUT_WAIT_FOREVER); +} + +static inline void _ff_unlock(tu_fifo_mutex_t mutex) +{ + if (mutex) osal_mutex_unlock(mutex); +} + +#else + +#define _ff_lock(_mutex) +#define _ff_unlock(_mutex) + +#endif + +/** \enum tu_fifo_copy_mode_t + * \brief Write modes intended to allow special read and write functions to be able to + * copy data to and from USB hardware FIFOs as needed for e.g. STM32s and others + */ +typedef enum +{ + TU_FIFO_COPY_INC, ///< Copy from/to an increasing source/destination address - default mode + TU_FIFO_COPY_CST_FULL_WORDS, ///< Copy from/to a constant source/destination address - required for e.g. STM32 to write into USB hardware FIFO +} tu_fifo_copy_mode_t; + +bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_size, bool overwritable) +{ + if (depth > 0x8000) return false; // Maximum depth is 2^15 items + + _ff_lock(f->mutex_wr); + _ff_lock(f->mutex_rd); + + f->buffer = (uint8_t*) buffer; + f->depth = depth; + f->item_size = item_size; + f->overwritable = overwritable; + + // Limit index space to 2*depth - this allows for a fast "modulo" calculation + // but limits the maximum depth to 2^16/2 = 2^15 and buffer overflows are detectable + // only if overflow happens once (important for unsupervised DMA applications) + f->max_pointer_idx = 2*depth - 1; + f->non_used_index_space = UINT16_MAX - f->max_pointer_idx; + + f->rd_idx = f->wr_idx = 0; + + _ff_unlock(f->mutex_wr); + _ff_unlock(f->mutex_rd); + + return true; +} + +// Static functions are intended to work on local variables +static inline uint16_t _ff_mod(uint16_t idx, uint16_t depth) +{ + while ( idx >= depth) idx -= depth; + return idx; +} + +// Intended to be used to read from hardware USB FIFO in e.g. STM32 where all data is read from a constant address +// Code adapted from dcd_synopsis.c +// TODO generalize with configurable 1 byte or 4 byte each read +static void _ff_push_const_addr(uint8_t * ff_buf, const void * app_buf, uint16_t len) +{ + volatile const uint32_t * rx_fifo = (volatile const uint32_t *) app_buf; + + // Reading full available 32 bit words from const app address + uint16_t full_words = len >> 2; + while(full_words--) + { + tu_unaligned_write32(ff_buf, *rx_fifo); + ff_buf += 4; + } + + // Read the remaining 1-3 bytes from const app address + uint8_t const bytes_rem = len & 0x03; + if ( bytes_rem ) + { + uint32_t tmp32 = *rx_fifo; + memcpy(ff_buf, &tmp32, bytes_rem); + } +} + +// Intended to be used to write to hardware USB FIFO in e.g. STM32 +// where all data is written to a constant address in full word copies +static void _ff_pull_const_addr(void * app_buf, const uint8_t * ff_buf, uint16_t len) +{ + volatile uint32_t * tx_fifo = (volatile uint32_t *) app_buf; + + // Pushing full available 32 bit words to const app address + uint16_t full_words = len >> 2; + while(full_words--) + { + *tx_fifo = tu_unaligned_read32(ff_buf); + ff_buf += 4; + } + + // Write the remaining 1-3 bytes into const app address + uint8_t const bytes_rem = len & 0x03; + if ( bytes_rem ) + { + uint32_t tmp32 = 0; + memcpy(&tmp32, ff_buf, bytes_rem); + + *tx_fifo = tmp32; + } +} + +// send one item to FIFO WITHOUT updating write pointer +static inline void _ff_push(tu_fifo_t* f, void const * app_buf, uint16_t rel) +{ + memcpy(f->buffer + (rel * f->item_size), app_buf, f->item_size); +} + +// send n items to FIFO WITHOUT updating write pointer +static void _ff_push_n(tu_fifo_t* f, void const * app_buf, uint16_t n, uint16_t rel, tu_fifo_copy_mode_t copy_mode) +{ + uint16_t const nLin = f->depth - rel; + uint16_t const nWrap = n - nLin; + + uint16_t nLin_bytes = nLin * f->item_size; + uint16_t nWrap_bytes = nWrap * f->item_size; + + // current buffer of fifo + uint8_t* ff_buf = f->buffer + (rel * f->item_size); + + switch (copy_mode) + { + case TU_FIFO_COPY_INC: + if(n <= nLin) + { + // Linear only + memcpy(ff_buf, app_buf, n*f->item_size); + } + else + { + // Wrap around + + // Write data to linear part of buffer + memcpy(ff_buf, app_buf, nLin_bytes); + + // Write data wrapped around + memcpy(f->buffer, ((uint8_t const*) app_buf) + nLin_bytes, nWrap_bytes); + } + break; + + case TU_FIFO_COPY_CST_FULL_WORDS: + // Intended for hardware buffers from which it can be read word by word only + if(n <= nLin) + { + // Linear only + _ff_push_const_addr(ff_buf, app_buf, n*f->item_size); + } + else + { + // Wrap around case + + // Write full words to linear part of buffer + uint16_t nLin_4n_bytes = nLin_bytes & 0xFFFC; + _ff_push_const_addr(ff_buf, app_buf, nLin_4n_bytes); + ff_buf += nLin_4n_bytes; + + // There could be odd 1-3 bytes before the wrap-around boundary + volatile const uint32_t * rx_fifo = (volatile const uint32_t *) app_buf; + uint8_t rem = nLin_bytes & 0x03; + if (rem > 0) + { + uint8_t remrem = tu_min16(nWrap_bytes, 4-rem); + nWrap_bytes -= remrem; + + uint32_t tmp32 = *rx_fifo; + uint8_t * src_u8 = ((uint8_t *) &tmp32); + + // Write 1-3 bytes before wrapped boundary + while(rem--) *ff_buf++ = *src_u8++; + + // Read more bytes to beginning to complete a word + ff_buf = f->buffer; + while(remrem--) *ff_buf++ = *src_u8++; + } + else + { + ff_buf = f->buffer; // wrap around to beginning + } + + // Write data wrapped part + if (nWrap_bytes > 0) _ff_push_const_addr(ff_buf, app_buf, nWrap_bytes); + } + break; + } +} + +// get one item from FIFO WITHOUT updating read pointer +static inline void _ff_pull(tu_fifo_t* f, void * app_buf, uint16_t rel) +{ + memcpy(app_buf, f->buffer + (rel * f->item_size), f->item_size); +} + +// get n items from FIFO WITHOUT updating read pointer +static void _ff_pull_n(tu_fifo_t* f, void* app_buf, uint16_t n, uint16_t rel, tu_fifo_copy_mode_t copy_mode) +{ + uint16_t const nLin = f->depth - rel; + uint16_t const nWrap = n - nLin; // only used if wrapped + + uint16_t nLin_bytes = nLin * f->item_size; + uint16_t nWrap_bytes = nWrap * f->item_size; + + // current buffer of fifo + uint8_t* ff_buf = f->buffer + (rel * f->item_size); + + switch (copy_mode) + { + case TU_FIFO_COPY_INC: + if ( n <= nLin ) + { + // Linear only + memcpy(app_buf, ff_buf, n*f->item_size); + } + else + { + // Wrap around + + // Read data from linear part of buffer + memcpy(app_buf, ff_buf, nLin_bytes); + + // Read data wrapped part + memcpy((uint8_t*) app_buf + nLin_bytes, f->buffer, nWrap_bytes); + } + break; + + case TU_FIFO_COPY_CST_FULL_WORDS: + if ( n <= nLin ) + { + // Linear only + _ff_pull_const_addr(app_buf, ff_buf, n*f->item_size); + } + else + { + // Wrap around case + + // Read full words from linear part of buffer + uint16_t nLin_4n_bytes = nLin_bytes & 0xFFFC; + _ff_pull_const_addr(app_buf, ff_buf, nLin_4n_bytes); + ff_buf += nLin_4n_bytes; + + // There could be odd 1-3 bytes before the wrap-around boundary + volatile uint32_t * tx_fifo = (volatile uint32_t *) app_buf; + uint8_t rem = nLin_bytes & 0x03; + if (rem > 0) + { + uint8_t remrem = tu_min16(nWrap_bytes, 4-rem); + nWrap_bytes -= remrem; + + uint32_t tmp32=0; + uint8_t * dst_u8 = (uint8_t *)&tmp32; + + // Read 1-3 bytes before wrapped boundary + while(rem--) *dst_u8++ = *ff_buf++; + + // Read more bytes from beginning to complete a word + ff_buf = f->buffer; + while(remrem--) *dst_u8++ = *ff_buf++; + + *tx_fifo = tmp32; + } + else + { + ff_buf = f->buffer; // wrap around to beginning + } + + // Read data wrapped part + if (nWrap_bytes > 0) _ff_pull_const_addr(app_buf, ff_buf, nWrap_bytes); + } + break; + + default: break; + } +} + +// Advance an absolute pointer +static uint16_t advance_pointer(tu_fifo_t* f, uint16_t p, uint16_t offset) +{ + // We limit the index space of p such that a correct wrap around happens + // Check for a wrap around or if we are in unused index space - This has to be checked first!! + // We are exploiting the wrap around to the correct index + if ((p > (uint16_t)(p + offset)) || ((uint16_t)(p + offset) > f->max_pointer_idx)) + { + p = (p + offset) + f->non_used_index_space; + } + else + { + p += offset; + } + return p; +} + +// Backward an absolute pointer +static uint16_t backward_pointer(tu_fifo_t* f, uint16_t p, uint16_t offset) +{ + // We limit the index space of p such that a correct wrap around happens + // Check for a wrap around or if we are in unused index space - This has to be checked first!! + // We are exploiting the wrap around to the correct index + if ((p < (uint16_t)(p - offset)) || ((uint16_t)(p - offset) > f->max_pointer_idx)) + { + p = (p - offset) - f->non_used_index_space; + } + else + { + p -= offset; + } + return p; +} + +// get relative from absolute pointer +static uint16_t get_relative_pointer(tu_fifo_t* f, uint16_t p) +{ + return _ff_mod(p, f->depth); +} + +// Works on local copies of w and r - return only the difference and as such can be used to determine an overflow +static inline uint16_t _tu_fifo_count(tu_fifo_t* f, uint16_t wAbs, uint16_t rAbs) +{ + uint16_t cnt = wAbs-rAbs; + + // In case we have non-power of two depth we need a further modification + if (rAbs > wAbs) cnt -= f->non_used_index_space; + + return cnt; +} + +// Works on local copies of w and r +static inline bool _tu_fifo_empty(uint16_t wAbs, uint16_t rAbs) +{ + return wAbs == rAbs; +} + +// Works on local copies of w and r +static inline bool _tu_fifo_full(tu_fifo_t* f, uint16_t wAbs, uint16_t rAbs) +{ + return (_tu_fifo_count(f, wAbs, rAbs) == f->depth); +} + +// Works on local copies of w and r +// BE AWARE - THIS FUNCTION MIGHT NOT GIVE A CORRECT ANSWERE IN CASE WRITE POINTER "OVERFLOWS" +// Only one overflow is allowed for this function to work e.g. if depth = 100, you must not +// write more than 2*depth-1 items in one rush without updating write pointer. Otherwise +// write pointer wraps and you pointer states are messed up. This can only happen if you +// use DMAs, write functions do not allow such an error. +static inline bool _tu_fifo_overflowed(tu_fifo_t* f, uint16_t wAbs, uint16_t rAbs) +{ + return (_tu_fifo_count(f, wAbs, rAbs) > f->depth); +} + +// Works on local copies of w +// For more details see _tu_fifo_overflow()! +static inline void _tu_fifo_correct_read_pointer(tu_fifo_t* f, uint16_t wAbs) +{ + f->rd_idx = backward_pointer(f, wAbs, f->depth); +} + +// Works on local copies of w and r +// Must be protected by mutexes since in case of an overflow read pointer gets modified +static bool _tu_fifo_peek(tu_fifo_t* f, void * p_buffer, uint16_t wAbs, uint16_t rAbs) +{ + uint16_t cnt = _tu_fifo_count(f, wAbs, rAbs); + + // Check overflow and correct if required + if (cnt > f->depth) + { + _tu_fifo_correct_read_pointer(f, wAbs); + cnt = f->depth; + } + + // Skip beginning of buffer + if (cnt == 0) return false; + + uint16_t rRel = get_relative_pointer(f, rAbs); + + // Peek data + _ff_pull(f, p_buffer, rRel); + + return true; +} + +// Works on local copies of w and r +// Must be protected by mutexes since in case of an overflow read pointer gets modified +static uint16_t _tu_fifo_peek_n(tu_fifo_t* f, void * p_buffer, uint16_t n, uint16_t wAbs, uint16_t rAbs, tu_fifo_copy_mode_t copy_mode) +{ + uint16_t cnt = _tu_fifo_count(f, wAbs, rAbs); + + // Check overflow and correct if required + if (cnt > f->depth) + { + _tu_fifo_correct_read_pointer(f, wAbs); + rAbs = f->rd_idx; + cnt = f->depth; + } + + // Skip beginning of buffer + if (cnt == 0) return 0; + + // Check if we can read something at and after offset - if too less is available we read what remains + if (cnt < n) n = cnt; + + uint16_t rRel = get_relative_pointer(f, rAbs); + + // Peek data + _ff_pull_n(f, p_buffer, n, rRel, copy_mode); + + return n; +} + +// Works on local copies of w and r +static inline uint16_t _tu_fifo_remaining(tu_fifo_t* f, uint16_t wAbs, uint16_t rAbs) +{ + return f->depth - _tu_fifo_count(f, wAbs, rAbs); +} + +static uint16_t _tu_fifo_write_n(tu_fifo_t* f, const void * data, uint16_t n, tu_fifo_copy_mode_t copy_mode) +{ + if ( n == 0 ) return 0; + + _ff_lock(f->mutex_wr); + + uint16_t w = f->wr_idx, r = f->rd_idx; + uint8_t const* buf8 = (uint8_t const*) data; + + if (!f->overwritable) + { + // Not overwritable limit up to full + n = tu_min16(n, _tu_fifo_remaining(f, w, r)); + } + else if (n >= f->depth) + { + // Only copy last part + buf8 = buf8 + (n - f->depth) * f->item_size; + n = f->depth; + + // We start writing at the read pointer's position since we fill the complete + // buffer and we do not want to modify the read pointer within a write function! + // This would end up in a race condition with read functions! + w = r; + } + + uint16_t wRel = get_relative_pointer(f, w); + + // Write data + _ff_push_n(f, buf8, n, wRel, copy_mode); + + // Advance pointer + f->wr_idx = advance_pointer(f, w, n); + + _ff_unlock(f->mutex_wr); + + return n; +} + +static uint16_t _tu_fifo_read_n(tu_fifo_t* f, void * buffer, uint16_t n, tu_fifo_copy_mode_t copy_mode) +{ + _ff_lock(f->mutex_rd); + + // Peek the data + // f->rd_idx might get modified in case of an overflow so we can not use a local variable + n = _tu_fifo_peek_n(f, buffer, n, f->wr_idx, f->rd_idx, copy_mode); + + // Advance read pointer + f->rd_idx = advance_pointer(f, f->rd_idx, n); + + _ff_unlock(f->mutex_rd); + return n; +} + +/******************************************************************************/ +/*! + @brief Get number of items in FIFO. + + As this function only reads the read and write pointers once, this function is + reentrant and thus thread and ISR save without any mutexes. In case an + overflow occurred, this function return f.depth at maximum. Overflows are + checked and corrected for in the read functions! + + @param[in] f + Pointer to the FIFO buffer to manipulate + + @returns Number of items in FIFO + */ +/******************************************************************************/ +uint16_t tu_fifo_count(tu_fifo_t* f) +{ + return tu_min16(_tu_fifo_count(f, f->wr_idx, f->rd_idx), f->depth); +} + +/******************************************************************************/ +/*! + @brief Check if FIFO is empty. + + As this function only reads the read and write pointers once, this function is + reentrant and thus thread and ISR save without any mutexes. + + @param[in] f + Pointer to the FIFO buffer to manipulate + + @returns Number of items in FIFO + */ +/******************************************************************************/ +bool tu_fifo_empty(tu_fifo_t* f) +{ + return _tu_fifo_empty(f->wr_idx, f->rd_idx); +} + +/******************************************************************************/ +/*! + @brief Check if FIFO is full. + + As this function only reads the read and write pointers once, this function is + reentrant and thus thread and ISR save without any mutexes. + + @param[in] f + Pointer to the FIFO buffer to manipulate + + @returns Number of items in FIFO + */ +/******************************************************************************/ +bool tu_fifo_full(tu_fifo_t* f) +{ + return _tu_fifo_full(f, f->wr_idx, f->rd_idx); +} + +/******************************************************************************/ +/*! + @brief Get remaining space in FIFO. + + As this function only reads the read and write pointers once, this function is + reentrant and thus thread and ISR save without any mutexes. + + @param[in] f + Pointer to the FIFO buffer to manipulate + + @returns Number of items in FIFO + */ +/******************************************************************************/ +uint16_t tu_fifo_remaining(tu_fifo_t* f) +{ + return _tu_fifo_remaining(f, f->wr_idx, f->rd_idx); +} + +/******************************************************************************/ +/*! + @brief Check if overflow happened. + + BE AWARE - THIS FUNCTION MIGHT NOT GIVE A CORRECT ANSWERE IN CASE WRITE POINTER "OVERFLOWS" + Only one overflow is allowed for this function to work e.g. if depth = 100, you must not + write more than 2*depth-1 items in one rush without updating write pointer. Otherwise + write pointer wraps and your pointer states are messed up. This can only happen if you + use DMAs, write functions do not allow such an error. Avoid such nasty things! + + All reading functions (read, peek) check for overflows and correct read pointer on their own such + that latest items are read. + If required (e.g. for DMA use) you can also correct the read pointer by + tu_fifo_correct_read_pointer(). + + @param[in] f + Pointer to the FIFO buffer to manipulate + + @returns True if overflow happened + */ +/******************************************************************************/ +bool tu_fifo_overflowed(tu_fifo_t* f) +{ + return _tu_fifo_overflowed(f, f->wr_idx, f->rd_idx); +} + +// Only use in case tu_fifo_overflow() returned true! +void tu_fifo_correct_read_pointer(tu_fifo_t* f) +{ + _ff_lock(f->mutex_rd); + _tu_fifo_correct_read_pointer(f, f->wr_idx); + _ff_unlock(f->mutex_rd); +} + +/******************************************************************************/ +/*! + @brief Read one element out of the buffer. + + This function will return the element located at the array index of the + read pointer, and then increment the read pointer index. + This function checks for an overflow and corrects read pointer if required. + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] buffer + Pointer to the place holder for data read from the buffer + + @returns TRUE if the queue is not empty + */ +/******************************************************************************/ +bool tu_fifo_read(tu_fifo_t* f, void * buffer) +{ + _ff_lock(f->mutex_rd); + + // Peek the data + // f->rd_idx might get modified in case of an overflow so we can not use a local variable + bool ret = _tu_fifo_peek(f, buffer, f->wr_idx, f->rd_idx); + + // Advance pointer + f->rd_idx = advance_pointer(f, f->rd_idx, ret); + + _ff_unlock(f->mutex_rd); + return ret; +} + +/******************************************************************************/ +/*! + @brief This function will read n elements from the array index specified by + the read pointer and increment the read index. + This function checks for an overflow and corrects read pointer if required. + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] buffer + The pointer to data location + @param[in] n + Number of element that buffer can afford + + @returns number of items read from the FIFO + */ +/******************************************************************************/ +uint16_t tu_fifo_read_n(tu_fifo_t* f, void * buffer, uint16_t n) +{ + return _tu_fifo_read_n(f, buffer, n, TU_FIFO_COPY_INC); +} + +uint16_t tu_fifo_read_n_const_addr_full_words(tu_fifo_t* f, void * buffer, uint16_t n) +{ + return _tu_fifo_read_n(f, buffer, n, TU_FIFO_COPY_CST_FULL_WORDS); +} + +/******************************************************************************/ +/*! + @brief Read one item without removing it from the FIFO. + This function checks for an overflow and corrects read pointer if required. + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] offset + Position to read from in the FIFO buffer with respect to read pointer + @param[in] p_buffer + Pointer to the place holder for data read from the buffer + + @returns TRUE if the queue is not empty + */ +/******************************************************************************/ +bool tu_fifo_peek(tu_fifo_t* f, void * p_buffer) +{ + _ff_lock(f->mutex_rd); + bool ret = _tu_fifo_peek(f, p_buffer, f->wr_idx, f->rd_idx); + _ff_unlock(f->mutex_rd); + return ret; +} + +/******************************************************************************/ +/*! + @brief Read n items without removing it from the FIFO + This function checks for an overflow and corrects read pointer if required. + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] p_buffer + Pointer to the place holder for data read from the buffer + @param[in] n + Number of items to peek + + @returns Number of bytes written to p_buffer + */ +/******************************************************************************/ +uint16_t tu_fifo_peek_n(tu_fifo_t* f, void * p_buffer, uint16_t n) +{ + _ff_lock(f->mutex_rd); + uint16_t ret = _tu_fifo_peek_n(f, p_buffer, n, f->wr_idx, f->rd_idx, TU_FIFO_COPY_INC); + _ff_unlock(f->mutex_rd); + return ret; +} + +/******************************************************************************/ +/*! + @brief Write one element into the buffer. + + This function will write one element into the array index specified by + the write pointer and increment the write index. + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] data + The byte to add to the FIFO + + @returns TRUE if the data was written to the FIFO (overwrittable + FIFO will always return TRUE) + */ +/******************************************************************************/ +bool tu_fifo_write(tu_fifo_t* f, const void * data) +{ + _ff_lock(f->mutex_wr); + + bool ret; + uint16_t const w = f->wr_idx; + + if ( _tu_fifo_full(f, w, f->rd_idx) && !f->overwritable ) + { + ret = false; + }else + { + uint16_t wRel = get_relative_pointer(f, w); + + // Write data + _ff_push(f, data, wRel); + + // Advance pointer + f->wr_idx = advance_pointer(f, w, 1); + + ret = true; + } + + _ff_unlock(f->mutex_wr); + + return ret; +} + +/******************************************************************************/ +/*! + @brief This function will write n elements into the array index specified by + the write pointer and increment the write index. + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] data + The pointer to data to add to the FIFO + @param[in] count + Number of element + @return Number of written elements + */ +/******************************************************************************/ +uint16_t tu_fifo_write_n(tu_fifo_t* f, const void * data, uint16_t n) +{ + return _tu_fifo_write_n(f, data, n, TU_FIFO_COPY_INC); +} + +/******************************************************************************/ +/*! + @brief This function will write n elements into the array index specified by + the write pointer and increment the write index. The source address will + not be incremented which is useful for reading from registers. + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] data + The pointer to data to add to the FIFO + @param[in] count + Number of element + @return Number of written elements + */ +/******************************************************************************/ +uint16_t tu_fifo_write_n_const_addr_full_words(tu_fifo_t* f, const void * data, uint16_t n) +{ + return _tu_fifo_write_n(f, data, n, TU_FIFO_COPY_CST_FULL_WORDS); +} + +/******************************************************************************/ +/*! + @brief Clear the fifo read and write pointers + + @param[in] f + Pointer to the FIFO buffer to manipulate + */ +/******************************************************************************/ +bool tu_fifo_clear(tu_fifo_t *f) +{ + _ff_lock(f->mutex_wr); + _ff_lock(f->mutex_rd); + + f->rd_idx = f->wr_idx = 0; + f->max_pointer_idx = 2*f->depth-1; + f->non_used_index_space = UINT16_MAX - f->max_pointer_idx; + + _ff_unlock(f->mutex_wr); + _ff_unlock(f->mutex_rd); + return true; +} + +/******************************************************************************/ +/*! + @brief Change the fifo mode to overwritable or not overwritable + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] overwritable + Overwritable mode the fifo is set to + */ +/******************************************************************************/ +bool tu_fifo_set_overwritable(tu_fifo_t *f, bool overwritable) +{ + _ff_lock(f->mutex_wr); + _ff_lock(f->mutex_rd); + + f->overwritable = overwritable; + + _ff_unlock(f->mutex_wr); + _ff_unlock(f->mutex_rd); + + return true; +} + +/******************************************************************************/ +/*! + @brief Advance write pointer - intended to be used in combination with DMA. + It is possible to fill the FIFO by use of a DMA in circular mode. Within + DMA ISRs you may update the write pointer to be able to read from the FIFO. + As long as the DMA is the only process writing into the FIFO this is safe + to use. + + USE WITH CARE - WE DO NOT CONDUCT SAFTY CHECKS HERE! + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] n + Number of items the write pointer moves forward + */ +/******************************************************************************/ +void tu_fifo_advance_write_pointer(tu_fifo_t *f, uint16_t n) +{ + f->wr_idx = advance_pointer(f, f->wr_idx, n); +} + +/******************************************************************************/ +/*! + @brief Advance read pointer - intended to be used in combination with DMA. + It is possible to read from the FIFO by use of a DMA in linear mode. Within + DMA ISRs you may update the read pointer to be able to again write into the + FIFO. As long as the DMA is the only process reading from the FIFO this is + safe to use. + + USE WITH CARE - WE DO NOT CONDUCT SAFTY CHECKS HERE! + + @param[in] f + Pointer to the FIFO buffer to manipulate + @param[in] n + Number of items the read pointer moves forward + */ +/******************************************************************************/ +void tu_fifo_advance_read_pointer(tu_fifo_t *f, uint16_t n) +{ + f->rd_idx = advance_pointer(f, f->rd_idx, n); +} + +/******************************************************************************/ +/*! + @brief Get read info + + Returns the length and pointer from which bytes can be read in a linear manner. + This is of major interest for DMA transmissions. If returned length is zero the + corresponding pointer is invalid. + The read pointer does NOT get advanced, use tu_fifo_advance_read_pointer() to + do so! + @param[in] f + Pointer to FIFO + @param[out] *info + Pointer to struct which holds the desired infos + */ +/******************************************************************************/ +void tu_fifo_get_read_info(tu_fifo_t *f, tu_fifo_buffer_info_t *info) +{ + // Operate on temporary values in case they change in between + uint16_t w = f->wr_idx, r = f->rd_idx; + + uint16_t cnt = _tu_fifo_count(f, w, r); + + // Check overflow and correct if required - may happen in case a DMA wrote too fast + if (cnt > f->depth) + { + _ff_lock(f->mutex_rd); + _tu_fifo_correct_read_pointer(f, w); + _ff_unlock(f->mutex_rd); + r = f->rd_idx; + cnt = f->depth; + } + + // Check if fifo is empty + if (cnt == 0) + { + info->len_lin = 0; + info->len_wrap = 0; + info->ptr_lin = NULL; + info->ptr_wrap = NULL; + return; + } + + // Get relative pointers + w = get_relative_pointer(f, w); + r = get_relative_pointer(f, r); + + // Copy pointer to buffer to start reading from + info->ptr_lin = &f->buffer[r]; + + // Check if there is a wrap around necessary + if (w > r) { + // Non wrapping case + info->len_lin = cnt; + info->len_wrap = 0; + info->ptr_wrap = NULL; + } + else + { + info->len_lin = f->depth - r; // Also the case if FIFO was full + info->len_wrap = cnt - info->len_lin; + info->ptr_wrap = f->buffer; + } +} + +/******************************************************************************/ +/*! + @brief Get linear write info + + Returns the length and pointer to which bytes can be written into FIFO in a linear manner. + This is of major interest for DMA transmissions not using circular mode. If a returned length is zero the + corresponding pointer is invalid. The returned lengths summed up are the currently free space in the FIFO. + The write pointer does NOT get advanced, use tu_fifo_advance_write_pointer() to do so! + TAKE CARE TO NOT OVERFLOW THE BUFFER MORE THAN TWO TIMES THE FIFO DEPTH - IT CAN NOT RECOVERE OTHERWISE! + @param[in] f + Pointer to FIFO + @param[out] *info + Pointer to struct which holds the desired infos + */ +/******************************************************************************/ +void tu_fifo_get_write_info(tu_fifo_t *f, tu_fifo_buffer_info_t *info) +{ + uint16_t w = f->wr_idx, r = f->rd_idx; + uint16_t free = _tu_fifo_remaining(f, w, r); + + if (free == 0) + { + info->len_lin = 0; + info->len_wrap = 0; + info->ptr_lin = NULL; + info->ptr_wrap = NULL; + return; + } + + // Get relative pointers + w = get_relative_pointer(f, w); + r = get_relative_pointer(f, r); + + // Copy pointer to buffer to start writing to + info->ptr_lin = &f->buffer[w]; + + if (w < r) + { + // Non wrapping case + info->len_lin = r-w; + info->len_wrap = 0; + info->ptr_wrap = NULL; + } + else + { + info->len_lin = f->depth - w; + info->len_wrap = free - info->len_lin; // Remaining length - n already was limited to free or FIFO depth + info->ptr_wrap = f->buffer; // Always start of buffer + } +} diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_fifo.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_fifo.h new file mode 100644 index 000000000..18db289a1 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_fifo.h @@ -0,0 +1,151 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * Copyright (c) 2020 Reinhard Panhuber - rework to unmasked pointers + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_FIFO_H_ +#define _TUSB_FIFO_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// Due to the use of unmasked pointers, this FIFO does not suffer from loosing +// one item slice. Furthermore, write and read operations are completely +// decoupled as write and read functions do not modify a common state. Henceforth, +// writing or reading from the FIFO within an ISR is safe as long as no other +// process (thread or ISR) interferes. +// Also, this FIFO is ready to be used in combination with a DMA as the write and +// read pointers can be updated from within a DMA ISR. Overflows are detectable +// within a certain number (see tu_fifo_overflow()). + +#include "common/tusb_common.h" + +// mutex is only needed for RTOS +// for OS None, we don't get preempted +#define CFG_FIFO_MUTEX (CFG_TUSB_OS != OPT_OS_NONE) + +#if CFG_FIFO_MUTEX +#include "osal/osal.h" +#define tu_fifo_mutex_t osal_mutex_t +#endif + +typedef struct +{ + uint8_t* buffer ; ///< buffer pointer + uint16_t depth ; ///< max items + uint16_t item_size ; ///< size of each item + bool overwritable ; + + uint16_t non_used_index_space ; ///< required for non-power-of-two buffer length + uint16_t max_pointer_idx ; ///< maximum absolute pointer index + + volatile uint16_t wr_idx ; ///< write pointer + volatile uint16_t rd_idx ; ///< read pointer + +#if CFG_FIFO_MUTEX + tu_fifo_mutex_t mutex_wr; + tu_fifo_mutex_t mutex_rd; +#endif + +} tu_fifo_t; + +typedef struct +{ + uint16_t len_lin ; ///< linear length in item size + uint16_t len_wrap ; ///< wrapped length in item size + void * ptr_lin ; ///< linear part start pointer + void * ptr_wrap ; ///< wrapped part start pointer +} tu_fifo_buffer_info_t; + +#define TU_FIFO_INIT(_buffer, _depth, _type, _overwritable) \ +{ \ + .buffer = _buffer, \ + .depth = _depth, \ + .item_size = sizeof(_type), \ + .overwritable = _overwritable, \ + .non_used_index_space = UINT16_MAX - (2*(_depth)-1), \ + .max_pointer_idx = 2*(_depth)-1, \ +} + +#define TU_FIFO_DEF(_name, _depth, _type, _overwritable) \ + uint8_t _name##_buf[_depth*sizeof(_type)]; \ + tu_fifo_t _name = TU_FIFO_INIT(_name##_buf, _depth, _type, _overwritable) + + +bool tu_fifo_set_overwritable(tu_fifo_t *f, bool overwritable); +bool tu_fifo_clear(tu_fifo_t *f); +bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_size, bool overwritable); + +#if CFG_FIFO_MUTEX +TU_ATTR_ALWAYS_INLINE static inline +void tu_fifo_config_mutex(tu_fifo_t *f, tu_fifo_mutex_t write_mutex_hdl, tu_fifo_mutex_t read_mutex_hdl) +{ + f->mutex_wr = write_mutex_hdl; + f->mutex_rd = read_mutex_hdl; +} +#endif + +bool tu_fifo_write (tu_fifo_t* f, void const * p_data); +uint16_t tu_fifo_write_n (tu_fifo_t* f, void const * p_data, uint16_t n); +uint16_t tu_fifo_write_n_const_addr_full_words (tu_fifo_t* f, const void * data, uint16_t n); + +bool tu_fifo_read (tu_fifo_t* f, void * p_buffer); +uint16_t tu_fifo_read_n (tu_fifo_t* f, void * p_buffer, uint16_t n); +uint16_t tu_fifo_read_n_const_addr_full_words (tu_fifo_t* f, void * buffer, uint16_t n); + +bool tu_fifo_peek (tu_fifo_t* f, void * p_buffer); +uint16_t tu_fifo_peek_n (tu_fifo_t* f, void * p_buffer, uint16_t n); + +uint16_t tu_fifo_count (tu_fifo_t* f); +uint16_t tu_fifo_remaining (tu_fifo_t* f); +bool tu_fifo_empty (tu_fifo_t* f); +bool tu_fifo_full (tu_fifo_t* f); +bool tu_fifo_overflowed (tu_fifo_t* f); +void tu_fifo_correct_read_pointer (tu_fifo_t* f); + +TU_ATTR_ALWAYS_INLINE static inline +uint16_t tu_fifo_depth(tu_fifo_t* f) +{ + return f->depth; +} + +// Pointer modifications intended to be used in combinations with DMAs. +// USE WITH CARE - NO SAFTY CHECKS CONDUCTED HERE! NOT MUTEX PROTECTED! +void tu_fifo_advance_write_pointer(tu_fifo_t *f, uint16_t n); +void tu_fifo_advance_read_pointer (tu_fifo_t *f, uint16_t n); + +// If you want to read/write from/to the FIFO by use of a DMA, you may need to conduct two copies +// to handle a possible wrapping part. These functions deliver a pointer to start +// reading/writing from/to and a valid linear length along which no wrap occurs. +void tu_fifo_get_read_info (tu_fifo_t *f, tu_fifo_buffer_info_t *info); +void tu_fifo_get_write_info(tu_fifo_t *f, tu_fifo_buffer_info_t *info); + + +#ifdef __cplusplus +} +#endif + +#endif /* _TUSB_FIFO_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_timeout.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_timeout.h new file mode 100644 index 000000000..ce53955f0 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_timeout.h @@ -0,0 +1,80 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup Group_Common Common Files + * \defgroup Group_TimeoutTimer timeout timer + * @{ */ + +#ifndef _TUSB_TIMEOUT_H_ +#define _TUSB_TIMEOUT_H_ + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + uint32_t start; + uint32_t interval; +}tu_timeout_t; + +#if 0 + +extern uint32_t tusb_hal_millis(void); + +static inline void tu_timeout_set(tu_timeout_t* tt, uint32_t msec) +{ + tt->interval = msec; + tt->start = tusb_hal_millis(); +} + +static inline bool tu_timeout_expired(tu_timeout_t* tt) +{ + return ( tusb_hal_millis() - tt->start ) >= tt->interval; +} + +// For used with periodic event to prevent drift +static inline void tu_timeout_reset(tu_timeout_t* tt) +{ + tt->start += tt->interval; +} + +static inline void tu_timeout_restart(tu_timeout_t* tt) +{ + tt->start = tusb_hal_millis(); +} + +#endif + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_TIMEOUT_H_ */ + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_types.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_types.h new file mode 100644 index 000000000..5b26f5aec --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_types.h @@ -0,0 +1,546 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup group_usb_definitions + * \defgroup USBDef_Type USB Types + * @{ */ + +#ifndef _TUSB_TYPES_H_ +#define _TUSB_TYPES_H_ + +#include +#include +#include "tusb_compiler.h" + +#ifdef __cplusplus + extern "C" { +#endif + +/*------------------------------------------------------------------*/ +/* CONSTANTS + *------------------------------------------------------------------*/ + +/// defined base on EHCI specs value for Endpoint Speed +typedef enum +{ + TUSB_SPEED_FULL = 0, + TUSB_SPEED_LOW = 1, + TUSB_SPEED_HIGH = 2, + TUSB_SPEED_INVALID = 0xff, +}tusb_speed_t; + +/// defined base on USB Specs Endpoint's bmAttributes +typedef enum +{ + TUSB_XFER_CONTROL = 0 , + TUSB_XFER_ISOCHRONOUS , + TUSB_XFER_BULK , + TUSB_XFER_INTERRUPT +}tusb_xfer_type_t; + +typedef enum +{ + TUSB_DIR_OUT = 0, + TUSB_DIR_IN = 1, + + TUSB_DIR_IN_MASK = 0x80 +}tusb_dir_t; + +/// Isochronous End Point Attributes +typedef enum +{ + TUSB_ISO_EP_ATT_NO_SYNC = 0x00, + TUSB_ISO_EP_ATT_ASYNCHRONOUS = 0x04, + TUSB_ISO_EP_ATT_ADAPTIVE = 0x08, + TUSB_ISO_EP_ATT_SYNCHRONOUS = 0x0C, + TUSB_ISO_EP_ATT_DATA = 0x00, ///< Data End Point + TUSB_ISO_EP_ATT_EXPLICIT_FB = 0x10, ///< Feedback End Point + TUSB_ISO_EP_ATT_IMPLICIT_FB = 0x20, ///< Data endpoint that also serves as an implicit feedback +}tusb_iso_ep_attribute_t; + +/// USB Descriptor Types +typedef enum +{ + TUSB_DESC_DEVICE = 0x01, + TUSB_DESC_CONFIGURATION = 0x02, + TUSB_DESC_STRING = 0x03, + TUSB_DESC_INTERFACE = 0x04, + TUSB_DESC_ENDPOINT = 0x05, + TUSB_DESC_DEVICE_QUALIFIER = 0x06, + TUSB_DESC_OTHER_SPEED_CONFIG = 0x07, + TUSB_DESC_INTERFACE_POWER = 0x08, + TUSB_DESC_OTG = 0x09, + TUSB_DESC_DEBUG = 0x0A, + TUSB_DESC_INTERFACE_ASSOCIATION = 0x0B, + + TUSB_DESC_BOS = 0x0F, + TUSB_DESC_DEVICE_CAPABILITY = 0x10, + + TUSB_DESC_FUNCTIONAL = 0x21, + + // Class Specific Descriptor + TUSB_DESC_CS_DEVICE = 0x21, + TUSB_DESC_CS_CONFIGURATION = 0x22, + TUSB_DESC_CS_STRING = 0x23, + TUSB_DESC_CS_INTERFACE = 0x24, + TUSB_DESC_CS_ENDPOINT = 0x25, + + TUSB_DESC_SUPERSPEED_ENDPOINT_COMPANION = 0x30, + TUSB_DESC_SUPERSPEED_ISO_ENDPOINT_COMPANION = 0x31 +}tusb_desc_type_t; + +typedef enum +{ + TUSB_REQ_GET_STATUS = 0 , + TUSB_REQ_CLEAR_FEATURE = 1 , + TUSB_REQ_RESERVED = 2 , + TUSB_REQ_SET_FEATURE = 3 , + TUSB_REQ_RESERVED2 = 4 , + TUSB_REQ_SET_ADDRESS = 5 , + TUSB_REQ_GET_DESCRIPTOR = 6 , + TUSB_REQ_SET_DESCRIPTOR = 7 , + TUSB_REQ_GET_CONFIGURATION = 8 , + TUSB_REQ_SET_CONFIGURATION = 9 , + TUSB_REQ_GET_INTERFACE = 10 , + TUSB_REQ_SET_INTERFACE = 11 , + TUSB_REQ_SYNCH_FRAME = 12 +}tusb_request_code_t; + +typedef enum +{ + TUSB_REQ_FEATURE_EDPT_HALT = 0, + TUSB_REQ_FEATURE_REMOTE_WAKEUP = 1, + TUSB_REQ_FEATURE_TEST_MODE = 2 +}tusb_request_feature_selector_t; + +typedef enum +{ + TUSB_REQ_TYPE_STANDARD = 0, + TUSB_REQ_TYPE_CLASS, + TUSB_REQ_TYPE_VENDOR, + TUSB_REQ_TYPE_INVALID +} tusb_request_type_t; + +typedef enum +{ + TUSB_REQ_RCPT_DEVICE =0, + TUSB_REQ_RCPT_INTERFACE, + TUSB_REQ_RCPT_ENDPOINT, + TUSB_REQ_RCPT_OTHER +} tusb_request_recipient_t; + +// https://www.usb.org/defined-class-codes +typedef enum +{ + TUSB_CLASS_UNSPECIFIED = 0 , + TUSB_CLASS_AUDIO = 1 , + TUSB_CLASS_CDC = 2 , + TUSB_CLASS_HID = 3 , + TUSB_CLASS_RESERVED_4 = 4 , + TUSB_CLASS_PHYSICAL = 5 , + TUSB_CLASS_IMAGE = 6 , + TUSB_CLASS_PRINTER = 7 , + TUSB_CLASS_MSC = 8 , + TUSB_CLASS_HUB = 9 , + TUSB_CLASS_CDC_DATA = 10 , + TUSB_CLASS_SMART_CARD = 11 , + TUSB_CLASS_RESERVED_12 = 12 , + TUSB_CLASS_CONTENT_SECURITY = 13 , + TUSB_CLASS_VIDEO = 14 , + TUSB_CLASS_PERSONAL_HEALTHCARE = 15 , + TUSB_CLASS_AUDIO_VIDEO = 16 , + + TUSB_CLASS_DIAGNOSTIC = 0xDC , + TUSB_CLASS_WIRELESS_CONTROLLER = 0xE0 , + TUSB_CLASS_MISC = 0xEF , + TUSB_CLASS_APPLICATION_SPECIFIC = 0xFE , + TUSB_CLASS_VENDOR_SPECIFIC = 0xFF +}tusb_class_code_t; + +typedef enum +{ + MISC_SUBCLASS_COMMON = 2 +}misc_subclass_type_t; + +typedef enum +{ + MISC_PROTOCOL_IAD = 1 +}misc_protocol_type_t; + +typedef enum +{ + APP_SUBCLASS_USBTMC = 0x03, + APP_SUBCLASS_DFU_RUNTIME = 0x01 +} app_subclass_type_t; + +typedef enum +{ + DEVICE_CAPABILITY_WIRELESS_USB = 0x01, + DEVICE_CAPABILITY_USB20_EXTENSION = 0x02, + DEVICE_CAPABILITY_SUPERSPEED_USB = 0x03, + DEVICE_CAPABILITY_CONTAINER_id = 0x04, + DEVICE_CAPABILITY_PLATFORM = 0x05, + DEVICE_CAPABILITY_POWER_DELIVERY = 0x06, + DEVICE_CAPABILITY_BATTERY_INFO = 0x07, + DEVICE_CAPABILITY_PD_CONSUMER_PORT = 0x08, + DEVICE_CAPABILITY_PD_PROVIDER_PORT = 0x09, + DEVICE_CAPABILITY_SUPERSPEED_PLUS = 0x0A, + DEVICE_CAPABILITY_PRECESION_TIME_MEASUREMENT = 0x0B, + DEVICE_CAPABILITY_WIRELESS_USB_EXT = 0x0C, + DEVICE_CAPABILITY_BILLBOARD = 0x0D, + DEVICE_CAPABILITY_AUTHENTICATION = 0x0E, + DEVICE_CAPABILITY_BILLBOARD_EX = 0x0F, + DEVICE_CAPABILITY_CONFIGURATION_SUMMARY = 0x10 +}device_capability_type_t; + +enum { + TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP = TU_BIT(5), + TUSB_DESC_CONFIG_ATT_SELF_POWERED = TU_BIT(6), +}; + +#define TUSB_DESC_CONFIG_POWER_MA(x) ((x)/2) + +/// Device State TODO remove +typedef enum +{ + TUSB_DEVICE_STATE_UNPLUG = 0 , + TUSB_DEVICE_STATE_CONFIGURED , + TUSB_DEVICE_STATE_SUSPENDED , +}tusb_device_state_t; + +typedef enum +{ + XFER_RESULT_SUCCESS, + XFER_RESULT_FAILED, + XFER_RESULT_STALLED, +}xfer_result_t; + +enum // TODO remove +{ + DESC_OFFSET_LEN = 0, + DESC_OFFSET_TYPE = 1 +}; + +enum +{ + INTERFACE_INVALID_NUMBER = 0xff +}; + + +typedef enum +{ + MS_OS_20_SET_HEADER_DESCRIPTOR = 0x00, + MS_OS_20_SUBSET_HEADER_CONFIGURATION = 0x01, + MS_OS_20_SUBSET_HEADER_FUNCTION = 0x02, + MS_OS_20_FEATURE_COMPATBLE_ID = 0x03, + MS_OS_20_FEATURE_REG_PROPERTY = 0x04, + MS_OS_20_FEATURE_MIN_RESUME_TIME = 0x05, + MS_OS_20_FEATURE_MODEL_ID = 0x06, + MS_OS_20_FEATURE_CCGP_DEVICE = 0x07, + MS_OS_20_FEATURE_VENDOR_REVISION = 0x08 +} microsoft_os_20_type_t; + +enum +{ + CONTROL_STAGE_SETUP, + CONTROL_STAGE_DATA, + CONTROL_STAGE_ACK +}; + +//--------------------------------------------------------------------+ +// USB Descriptors +//--------------------------------------------------------------------+ + +// Start of all packed definitions for compiler without per-type packed +TU_ATTR_PACKED_BEGIN +TU_ATTR_BIT_FIELD_ORDER_BEGIN + +/// USB Device Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes. + uint8_t bDescriptorType ; ///< DEVICE Descriptor Type. + uint16_t bcdUSB ; ///< BUSB Specification Release Number in Binary-Coded Decimal (i.e., 2.10 is 210H). This field identifies the release of the USB Specification with which the device and its descriptors are compliant. + + uint8_t bDeviceClass ; ///< Class code (assigned by the USB-IF). \li If this field is reset to zero, each interface within a configuration specifies its own class information and the various interfaces operate independently. \li If this field is set to a value between 1 and FEH, the device supports different class specifications on different interfaces and the interfaces may not operate independently. This value identifies the class definition used for the aggregate interfaces. \li If this field is set to FFH, the device class is vendor-specific. + uint8_t bDeviceSubClass ; ///< Subclass code (assigned by the USB-IF). These codes are qualified by the value of the bDeviceClass field. \li If the bDeviceClass field is reset to zero, this field must also be reset to zero. \li If the bDeviceClass field is not set to FFH, all values are reserved for assignment by the USB-IF. + uint8_t bDeviceProtocol ; ///< Protocol code (assigned by the USB-IF). These codes are qualified by the value of the bDeviceClass and the bDeviceSubClass fields. If a device supports class-specific protocols on a device basis as opposed to an interface basis, this code identifies the protocols that the device uses as defined by the specification of the device class. \li If this field is reset to zero, the device does not use class-specific protocols on a device basis. However, it may use classspecific protocols on an interface basis. \li If this field is set to FFH, the device uses a vendor-specific protocol on a device basis. + uint8_t bMaxPacketSize0 ; ///< Maximum packet size for endpoint zero (only 8, 16, 32, or 64 are valid). For HS devices is fixed to 64. + + uint16_t idVendor ; ///< Vendor ID (assigned by the USB-IF). + uint16_t idProduct ; ///< Product ID (assigned by the manufacturer). + 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 the device's serial number. + + uint8_t bNumConfigurations ; ///< Number of possible configurations. +} tusb_desc_device_t; + +TU_VERIFY_STATIC( sizeof(tusb_desc_device_t) == 18, "size is not correct"); + +// USB Binary Device Object Store (BOS) Descriptor +typedef struct TU_ATTR_PACKED +{ + 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 descriptor + uint8_t bNumDeviceCaps ; ///< Number of device capability descriptors in the BOS +} tusb_desc_bos_t; + +TU_VERIFY_STATIC( sizeof(tusb_desc_bos_t) == 5, "size is not correct"); + +/// USB Configuration Descriptor +typedef struct TU_ATTR_PACKED +{ + 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. Includes the combined length of all descriptors (configuration, interface, endpoint, and class- or vendor-specific) 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 to select this configuration. + uint8_t iConfiguration ; ///< Index of string descriptor describing this configuration + uint8_t bmAttributes ; ///< Configuration characteristics \n D7: Reserved (set to one)\n D6: Self-powered \n D5: Remote Wakeup \n D4...0: Reserved (reset to zero) \n D7 is reserved and must be set to one for historical reasons. \n A device configuration that uses power from the bus and a local source reports a non-zero value in bMaxPower to indicate the amount of bus power required and sets D6. The actual power source at runtime may be determined using the GetStatus(DEVICE) request (see USB 2.0 spec Section 9.4.5). \n If a device configuration supports remote wakeup, D5 is set to one. + uint8_t bMaxPower ; ///< Maximum power consumption of the USB device from the bus in this specific configuration when the device is fully operational. Expressed in 2 mA units (i.e., 50 = 100 mA). +} tusb_desc_configuration_t; + +TU_VERIFY_STATIC( sizeof(tusb_desc_configuration_t) == 9, "size is not correct"); + +/// USB Interface Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes + uint8_t bDescriptorType ; ///< INTERFACE Descriptor Type + + uint8_t bInterfaceNumber ; ///< Number of this interface. Zero-based value identifying the index in the array of concurrent interfaces supported by this configuration. + uint8_t bAlternateSetting ; ///< Value used to select this alternate setting for the interface identified in the prior field + uint8_t bNumEndpoints ; ///< Number of endpoints used by this interface (excluding endpoint zero). If this value is zero, this interface only uses the Default Control Pipe. + uint8_t bInterfaceClass ; ///< Class code (assigned by the USB-IF). \li A value of zero is reserved for future standardization. \li If this field is set to FFH, the interface class is vendor-specific. \li All other values are reserved for assignment by the USB-IF. + uint8_t bInterfaceSubClass ; ///< Subclass code (assigned by the USB-IF). \n These codes are qualified by the value of the bInterfaceClass field. \li If the bInterfaceClass field is reset to zero, this field must also be reset to zero. \li If the bInterfaceClass field is not set to FFH, all values are reserved for assignment by the USB-IF. + uint8_t bInterfaceProtocol ; ///< Protocol code (assigned by the USB). \n These codes are qualified by the value of the bInterfaceClass and the bInterfaceSubClass fields. If an interface supports class-specific requests, this code identifies the protocols that the device uses as defined by the specification of the device class. \li If this field is reset to zero, the device does not use a class-specific protocol on this interface. \li If this field is set to FFH, the device uses a vendor-specific protocol for this interface. + uint8_t iInterface ; ///< Index of string descriptor describing this interface +} tusb_desc_interface_t; + +TU_VERIFY_STATIC( sizeof(tusb_desc_interface_t) == 9, "size is not correct"); + +/// USB Endpoint Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; // Size of this descriptor in bytes + uint8_t bDescriptorType ; // ENDPOINT Descriptor Type + + uint8_t bEndpointAddress ; // The address of the endpoint + + struct TU_ATTR_PACKED { + uint8_t xfer : 2; // Control, ISO, Bulk, Interrupt + uint8_t sync : 2; // None, Asynchronous, Adaptive, Synchronous + uint8_t usage : 2; // Data, Feedback, Implicit feedback + uint8_t : 2; + } bmAttributes; + + uint16_t wMaxPacketSize ; // Bit 10..0 : max packet size, bit 12..11 additional transaction per highspeed micro-frame + uint8_t bInterval ; // Polling interval, in frames or microframes depending on the operating speed +} tusb_desc_endpoint_t; + +TU_VERIFY_STATIC( sizeof(tusb_desc_endpoint_t) == 7, "size is not correct"); + +/// USB Other Speed Configuration Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of descriptor + uint8_t bDescriptorType ; ///< Other_speed_Configuration Type + uint16_t wTotalLength ; ///< Total length of data returned + + uint8_t bNumInterfaces ; ///< Number of interfaces supported by this speed configuration + uint8_t bConfigurationValue ; ///< Value to use to select configuration + uint8_t iConfiguration ; ///< Index of string descriptor + uint8_t bmAttributes ; ///< Same as Configuration descriptor + uint8_t bMaxPower ; ///< Same as Configuration descriptor +} tusb_desc_other_speed_t; + +/// USB Device Qualifier Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of descriptor + uint8_t bDescriptorType ; ///< Device Qualifier Type + uint16_t bcdUSB ; ///< USB specification version number (e.g., 0200H for V2.00) + + uint8_t bDeviceClass ; ///< Class Code + uint8_t bDeviceSubClass ; ///< SubClass Code + uint8_t bDeviceProtocol ; ///< Protocol Code + + uint8_t bMaxPacketSize0 ; ///< Maximum packet size for other speed + uint8_t bNumConfigurations ; ///< Number of Other-speed Configurations + uint8_t bReserved ; ///< Reserved for future use, must be zero +} tusb_desc_device_qualifier_t; + +TU_VERIFY_STATIC( sizeof(tusb_desc_device_qualifier_t) == 10, "size is not correct"); + +/// USB Interface Association Descriptor (IAD ECN) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of descriptor + uint8_t bDescriptorType ; ///< Other_speed_Configuration Type + + uint8_t bFirstInterface ; ///< Index of the first associated interface. + uint8_t bInterfaceCount ; ///< Total number of associated interfaces. + + uint8_t bFunctionClass ; ///< Interface class ID. + uint8_t bFunctionSubClass ; ///< Interface subclass ID. + uint8_t bFunctionProtocol ; ///< Interface protocol ID. + + uint8_t iFunction ; ///< Index of the string descriptor describing the interface association. +} tusb_desc_interface_assoc_t; + +// USB String Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength ; ///< Size of this descriptor in bytes + uint8_t bDescriptorType ; ///< Descriptor Type + uint16_t unicode_string[]; +} tusb_desc_string_t; + +// USB Binary Device Object Store (BOS) +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength; + uint8_t bDescriptorType ; + uint8_t bDevCapabilityType; + uint8_t bReserved; + uint8_t PlatformCapabilityUUID[16]; + uint8_t CapabilityData[]; +} tusb_desc_bos_platform_t; + +// USB WebuSB URL Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bScheme; + char url[]; +} tusb_desc_webusb_url_t; + +// DFU Functional Descriptor +typedef struct TU_ATTR_PACKED +{ + uint8_t bLength; + uint8_t bDescriptorType; + + union { + struct TU_ATTR_PACKED { + uint8_t bitCanDnload : 1; + uint8_t bitCanUpload : 1; + uint8_t bitManifestationTolerant : 1; + uint8_t bitWillDetach : 1; + uint8_t reserved : 4; + } bmAttributes; + + uint8_t bAttributes; + }; + + uint16_t wDetachTimeOut; + uint16_t wTransferSize; + uint16_t bcdDFUVersion; +} tusb_desc_dfu_functional_t; + +/*------------------------------------------------------------------*/ +/* Types + *------------------------------------------------------------------*/ +typedef struct TU_ATTR_PACKED{ + union { + struct TU_ATTR_PACKED { + uint8_t recipient : 5; ///< Recipient type tusb_request_recipient_t. + uint8_t type : 2; ///< Request type tusb_request_type_t. + uint8_t direction : 1; ///< Direction type. tusb_dir_t + } bmRequestType_bit; + + uint8_t bmRequestType; + }; + + uint8_t bRequest; + uint16_t wValue; + uint16_t wIndex; + uint16_t wLength; +} tusb_control_request_t; + +TU_VERIFY_STATIC( sizeof(tusb_control_request_t) == 8, "size is not correct"); + + +TU_ATTR_PACKED_END // End of all packed definitions +TU_ATTR_BIT_FIELD_ORDER_END + +//--------------------------------------------------------------------+ +// Endpoint helper +//--------------------------------------------------------------------+ + +// Get direction from Endpoint address +static inline tusb_dir_t tu_edpt_dir(uint8_t addr) +{ + return (addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT; +} + +// Get Endpoint number from address +static inline uint8_t tu_edpt_number(uint8_t addr) +{ + return (uint8_t)(addr & (~TUSB_DIR_IN_MASK)); +} + +static inline uint8_t tu_edpt_addr(uint8_t num, uint8_t dir) +{ + return (uint8_t)(num | (dir ? TUSB_DIR_IN_MASK : 0)); +} + +static inline uint16_t tu_edpt_packet_size(tusb_desc_endpoint_t const* desc_ep) +{ + return tu_le16toh(desc_ep->wMaxPacketSize) & TU_GENMASK(10, 0); +} + +//--------------------------------------------------------------------+ +// Descriptor helper +//--------------------------------------------------------------------+ +static inline uint8_t const * tu_desc_next(void const* desc) +{ + uint8_t const* desc8 = (uint8_t const*) desc; + return desc8 + desc8[DESC_OFFSET_LEN]; +} + +static inline uint8_t tu_desc_type(void const* desc) +{ + return ((uint8_t const*) desc)[DESC_OFFSET_TYPE]; +} + +static inline uint8_t tu_desc_len(void const* desc) +{ + return ((uint8_t const*) desc)[DESC_OFFSET_LEN]; +} + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_TYPES_H_ */ + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_verify.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_verify.h new file mode 100644 index 000000000..8fef11dc7 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/common/tusb_verify.h @@ -0,0 +1,181 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ +#ifndef TUSB_VERIFY_H_ +#define TUSB_VERIFY_H_ + +#include +#include +#include "tusb_option.h" +#include "tusb_compiler.h" + +/*------------------------------------------------------------------*/ +/* This file use an advanced macro technique to mimic the default parameter + * as C++ for the sake of code simplicity. Beware of a headache macro + * manipulation that you are told to stay away. + * + * This contains macros for both VERIFY and ASSERT: + * + * VERIFY: Used when there is an error condition which is not the + * fault of the MCU. For example, bounds checking on data + * sent to the micro over USB should use this function. + * Another example is checking for buffer overflows, where + * returning from the active function causes a NAK. + * + * ASSERT: Used for error conditions that are caused by MCU firmware + * bugs. This is used to discover bugs in the code more + * quickly. One example would be adding assertions in library + * function calls to confirm a function's (untainted) + * parameters are valid. + * + * The difference in behavior is that ASSERT triggers a breakpoint while + * verify does not. + * + * #define TU_VERIFY(cond) if(cond) return false; + * #define TU_VERIFY(cond,ret) if(cond) return ret; + * + * #define TU_VERIFY_HDLR(cond,handler) if(cond) {handler; return false;} + * #define TU_VERIFY_HDLR(cond,ret,handler) if(cond) {handler; return ret;} + * + * #define TU_ASSERT(cond) if(cond) {_MESS_FAILED(); TU_BREAKPOINT(), return false;} + * #define TU_ASSERT(cond,ret) if(cond) {_MESS_FAILED(); TU_BREAKPOINT(), return ret;} + * + *------------------------------------------------------------------*/ + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// TU_VERIFY Helper +//--------------------------------------------------------------------+ + +#if CFG_TUSB_DEBUG + #include + #define _MESS_ERR(_err) tu_printf("%s %d: failed, error = %s\r\n", __func__, __LINE__, tusb_strerr[_err]) + #define _MESS_FAILED() tu_printf("%s %d: ASSERT FAILED\r\n", __func__, __LINE__) +#else + #define _MESS_ERR(_err) do {} while (0) + #define _MESS_FAILED() do {} while (0) +#endif + +// Halt CPU (breakpoint) when hitting error, only apply for Cortex M3, M4, M7, M33 +#if defined(__ARM_ARCH_7M__) || defined (__ARM_ARCH_7EM__) || defined(__ARM_ARCH_8M_MAIN__) + #define TU_BREAKPOINT() do \ + { \ + volatile uint32_t* ARM_CM_DHCSR = ((volatile uint32_t*) 0xE000EDF0UL); /* Cortex M CoreDebug->DHCSR */ \ + if ( (*ARM_CM_DHCSR) & 1UL ) __asm("BKPT #0\n"); /* Only halt mcu if debugger is attached */ \ + } while(0) + +#elif defined(__riscv) + #define TU_BREAKPOINT() do { __asm("ebreak\n"); } while(0) + +#else + #define TU_BREAKPOINT() do {} while (0) +#endif + +/*------------------------------------------------------------------*/ +/* Macro Generator + *------------------------------------------------------------------*/ + +// Helper to implement optional parameter for TU_VERIFY Macro family +#define GET_3RD_ARG(arg1, arg2, arg3, ...) arg3 +#define GET_4TH_ARG(arg1, arg2, arg3, arg4, ...) arg4 + +/*------------- Generator for TU_VERIFY and TU_VERIFY_HDLR -------------*/ +#define TU_VERIFY_DEFINE(_cond, _handler, _ret) do \ +{ \ + if ( !(_cond) ) { _handler; return _ret; } \ +} while(0) + +/*------------------------------------------------------------------*/ +/* TU_VERIFY + * - TU_VERIFY_1ARGS : return false if failed + * - TU_VERIFY_2ARGS : return provided value if failed + *------------------------------------------------------------------*/ +#define TU_VERIFY_1ARGS(_cond) TU_VERIFY_DEFINE(_cond, , false) +#define TU_VERIFY_2ARGS(_cond, _ret) TU_VERIFY_DEFINE(_cond, , _ret) + +#define TU_VERIFY(...) GET_3RD_ARG(__VA_ARGS__, TU_VERIFY_2ARGS, TU_VERIFY_1ARGS, UNUSED)(__VA_ARGS__) + + +/*------------------------------------------------------------------*/ +/* TU_VERIFY WITH HANDLER + * - TU_VERIFY_HDLR_2ARGS : execute handler, return false if failed + * - TU_VERIFY_HDLR_3ARGS : execute handler, return provided error if failed + *------------------------------------------------------------------*/ +#define TU_VERIFY_HDLR_2ARGS(_cond, _handler) TU_VERIFY_DEFINE(_cond, _handler, false) +#define TU_VERIFY_HDLR_3ARGS(_cond, _handler, _ret) TU_VERIFY_DEFINE(_cond, _handler, _ret) + +#define TU_VERIFY_HDLR(...) GET_4TH_ARG(__VA_ARGS__, TU_VERIFY_HDLR_3ARGS, TU_VERIFY_HDLR_2ARGS,UNUSED)(__VA_ARGS__) + +/*------------------------------------------------------------------*/ +/* ASSERT + * basically TU_VERIFY with TU_BREAKPOINT() as handler + * - 1 arg : return false if failed + * - 2 arg : return error if failed + *------------------------------------------------------------------*/ +#define ASSERT_1ARGS(_cond) TU_VERIFY_DEFINE(_cond, _MESS_FAILED(); TU_BREAKPOINT(), false) +#define ASSERT_2ARGS(_cond, _ret) TU_VERIFY_DEFINE(_cond, _MESS_FAILED(); TU_BREAKPOINT(), _ret) + +#ifndef TU_ASSERT +#define TU_ASSERT(...) GET_3RD_ARG(__VA_ARGS__, ASSERT_2ARGS, ASSERT_1ARGS,UNUSED)(__VA_ARGS__) +#endif + +// TODO remove TU_ASSERT_ERR() later + +/*------------- Generator for TU_VERIFY_ERR and TU_VERIFY_ERR_HDLR -------------*/ +#define TU_VERIFY_ERR_DEF2(_error, _handler) do \ +{ \ + uint32_t _err = (uint32_t)(_error); \ + if ( 0 != _err ) { _MESS_ERR(_err); _handler; return _err; } \ +} while(0) + +#define TU_VERIFY_ERR_DEF3(_error, _handler, _ret) do \ +{ \ + uint32_t _err = (uint32_t)(_error); \ + if ( 0 != _err ) { _MESS_ERR(_err); _handler; return _ret; } \ +} while(0) + +/*------------------------------------------------------------------*/ +/* ASSERT Error + * basically TU_VERIFY Error with TU_BREAKPOINT() as handler + *------------------------------------------------------------------*/ +#define ASSERT_ERR_1ARGS(_error) TU_VERIFY_ERR_DEF2(_error, TU_BREAKPOINT()) +#define ASSERT_ERR_2ARGS(_error, _ret) TU_VERIFY_ERR_DEF3(_error, TU_BREAKPOINT(), _ret) + +#ifndef TU_ASSERT_ERR +#define TU_ASSERT_ERR(...) GET_3RD_ARG(__VA_ARGS__, ASSERT_ERR_2ARGS, ASSERT_ERR_1ARGS,UNUSED)(__VA_ARGS__) +#endif + +/*------------------------------------------------------------------*/ +/* ASSERT HDLR + *------------------------------------------------------------------*/ + +#ifdef __cplusplus + } +#endif + +#endif /* TUSB_VERIFY_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/dcd.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/dcd.h new file mode 100644 index 000000000..fbcef40d4 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/dcd.h @@ -0,0 +1,193 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_DCD_H_ +#define _TUSB_DCD_H_ + +#include "common/tusb_common.h" +#include "osal/osal.h" +#include "common/tusb_fifo.h" +#include "dcd_attr.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Configuration +//--------------------------------------------------------------------+ + +#ifndef CFG_TUD_ENDPPOINT_MAX + #define CFG_TUD_ENDPPOINT_MAX DCD_ATTR_ENDPOINT_MAX +#endif + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF PROTYPES +//--------------------------------------------------------------------+ + +typedef enum +{ + DCD_EVENT_INVALID = 0, + DCD_EVENT_BUS_RESET, + DCD_EVENT_UNPLUGGED, + DCD_EVENT_SOF, + DCD_EVENT_SUSPEND, // TODO LPM Sleep L1 support + DCD_EVENT_RESUME, + + DCD_EVENT_SETUP_RECEIVED, + DCD_EVENT_XFER_COMPLETE, + + // Not an DCD event, just a convenient way to defer ISR function + USBD_EVENT_FUNC_CALL, + + DCD_EVENT_COUNT +} dcd_eventid_t; + +typedef struct TU_ATTR_ALIGNED(4) +{ + uint8_t rhport; + uint8_t event_id; + + union + { + // BUS RESET + struct { + tusb_speed_t speed; + } bus_reset; + + // SETUP_RECEIVED + tusb_control_request_t setup_received; + + // XFER_COMPLETE + struct { + uint8_t ep_addr; + uint8_t result; + uint32_t len; + }xfer_complete; + + // FUNC_CALL + struct { + void (*func) (void*); + void* param; + }func_call; + }; +} dcd_event_t; + +//TU_VERIFY_STATIC(sizeof(dcd_event_t) <= 12, "size is not correct"); + +//--------------------------------------------------------------------+ +// Controller API +//--------------------------------------------------------------------+ + +// Initialize controller to device mode +void dcd_init (uint8_t rhport); + +// Interrupt Handler +#if __GNUC__ && !defined(__ARMCC_VERSION) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wredundant-decls" +#endif +void dcd_int_handler(uint8_t rhport); +#if __GNUC__ && !defined(__ARMCC_VERSION) +#pragma GCC diagnostic pop +#endif + +// Enable device interrupt +void dcd_int_enable (uint8_t rhport); + +// Disable device interrupt +void dcd_int_disable(uint8_t rhport); + +// Receive Set Address request, mcu port must also include status IN response +void dcd_set_address(uint8_t rhport, uint8_t dev_addr); + +// Wake up host +void dcd_remote_wakeup(uint8_t rhport); + +// Connect by enabling internal pull-up resistor on D+/D- +void dcd_connect(uint8_t rhport) TU_ATTR_WEAK; + +// Disconnect by disabling internal pull-up resistor on D+/D- +void dcd_disconnect(uint8_t rhport) TU_ATTR_WEAK; + +//--------------------------------------------------------------------+ +// Endpoint API +//--------------------------------------------------------------------+ + +// Invoked when a control transfer's status stage is complete. +// May help DCD to prepare for next control transfer, this API is optional. +void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * request) TU_ATTR_WEAK; + +// Configure endpoint's registers according to descriptor +bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_ep); + +// Close all non-control endpoints, cancel all pending transfers if any. +// Invoked when switching from a non-zero Configuration by SET_CONFIGURE therefore +// required for multiple configuration support. +void dcd_edpt_close_all (uint8_t rhport); + +// Close an endpoint. +// Since it is weak, caller must TU_ASSERT this function's existence before calling it. +void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr) TU_ATTR_WEAK; + +// Submit a transfer, When complete dcd_event_xfer_complete() is invoked to notify the stack +bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes); + +// Submit an transfer using fifo, When complete dcd_event_xfer_complete() is invoked to notify the stack +// This API is optional, may be useful for register-based for transferring data. +bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes) TU_ATTR_WEAK; + +// Stall endpoint, any queuing transfer should be removed from endpoint +void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr); + +// clear stall, data toggle is also reset to DATA0 +// This API never calls with control endpoints, since it is auto cleared when receiving setup packet +void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr); + +//--------------------------------------------------------------------+ +// Event API (implemented by stack) +//--------------------------------------------------------------------+ + +// Called by DCD to notify device stack +extern void dcd_event_handler(dcd_event_t const * event, bool in_isr); + +// helper to send bus signal event +extern void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr); + +// helper to send bus reset event +extern void dcd_event_bus_reset (uint8_t rhport, tusb_speed_t speed, bool in_isr); + +// helper to send setup received +extern void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr); + +// helper to send transfer complete event +extern void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_DCD_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/dcd_attr.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/dcd_attr.h new file mode 100644 index 000000000..e16a5ccca --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/dcd_attr.h @@ -0,0 +1,225 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021, Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef TUSB_DCD_ATTR_H_ +#define TUSB_DCD_ATTR_H_ + +#include "tusb_option.h" + +// Attribute includes +// - ENDPOINT_MAX: max (logical) number of endpoint +// - ENDPOINT_EXCLUSIVE_NUMBER: endpoint number with different direction IN and OUT aren't allowed, +// e.g EP1 OUT & EP1 IN cannot exist together +// - PORT_HIGHSPEED: mask to indicate which port support highspeed mode, bit0 for port0 and so on. + +//------------- NXP -------------// +#if TU_CHECK_MCU(OPT_MCU_LPC11UXX, OPT_MCU_LPC13XX, OPT_MCU_LPC15XX) + #define DCD_ATTR_ENDPOINT_MAX 5 + +#elif TU_CHECK_MCU(OPT_MCU_LPC175X_6X, OPT_MCU_LPC177X_8X, OPT_MCU_LPC40XX) + #define DCD_ATTR_ENDPOINT_MAX 16 + +#elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX) + // TODO USB0 has 6, USB1 has 4 + #define DCD_ATTR_CONTROLLER_CHIPIDEA_HS + #define DCD_ATTR_ENDPOINT_MAX 6 + +#elif TU_CHECK_MCU(OPT_MCU_LPC51UXX) + #define DCD_ATTR_ENDPOINT_MAX 5 + +#elif TU_CHECK_MCU(OPT_MCU_LPC54XXX) + // TODO USB0 has 5, USB1 has 6 + #define DCD_ATTR_ENDPOINT_MAX 6 + +#elif TU_CHECK_MCU(OPT_MCU_LPC55XX) + // TODO USB0 has 5, USB1 has 6 + #define DCD_ATTR_ENDPOINT_MAX 6 + +#elif TU_CHECK_MCU(OPT_MCU_MIMXRT10XX) + #define DCD_ATTR_CONTROLLER_CHIPIDEA_HS + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_MKL25ZXX, OPT_MCU_K32L2BXX) + #define DCD_ATTR_ENDPOINT_MAX 16 + +#elif TU_CHECK_MCU(OPT_MCU_MM32F327X) + #define DCD_ATTR_ENDPOINT_MAX 16 + +//------------- Nordic -------------// +#elif TU_CHECK_MCU(OPT_MCU_NRF5X) + // 8 CBI + 1 ISO + #define DCD_ATTR_ENDPOINT_MAX 9 + +//------------- Microchip -------------// +#elif TU_CHECK_MCU(OPT_MCU_SAMD21, OPT_MCU_SAMD51, OPT_MCU_SAME5X) || \ + TU_CHECK_MCU(OPT_MCU_SAMD11, OPT_MCU_SAML21, OPT_MCU_SAML22) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_SAMG) + #define DCD_ATTR_ENDPOINT_MAX 6 + #define DCD_ATTR_ENDPOINT_EXCLUSIVE_NUMBER + +#elif TU_CHECK_MCU(OPT_MCU_SAMX7X) + #define DCD_ATTR_ENDPOINT_MAX 10 + #define DCD_ATTR_ENDPOINT_EXCLUSIVE_NUMBER + +#elif TU_CHECK_MCU(OPT_MCU_PIC32MZ) + #define DCD_ATTR_ENDPOINT_MAX 8 + #define DCD_ATTR_ENDPOINT_EXCLUSIVE_NUMBER + +//------------- ST -------------// +#elif TU_CHECK_MCU(OPT_MCU_STM32F0) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_STM32F1) + #if defined (STM32F105x8) || defined (STM32F105xB) || defined (STM32F105xC) || \ + defined (STM32F107xB) || defined (STM32F107xC) + #define DCD_ATTR_ENDPOINT_MAX 4 + #define DCD_ATTR_DWC2_STM32 + #else + #define DCD_ATTR_ENDPOINT_MAX 8 + #endif + +#elif TU_CHECK_MCU(OPT_MCU_STM32F2) + // FS has 4 ep, HS has 5 ep + #define DCD_ATTR_ENDPOINT_MAX 6 + #define DCD_ATTR_DWC2_STM32 + +#elif TU_CHECK_MCU(OPT_MCU_STM32F3) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_STM32F4) + // For most mcu, FS has 4, HS has 6. TODO 446/469/479 HS has 9 + #define DCD_ATTR_ENDPOINT_MAX 6 + #define DCD_ATTR_DWC2_STM32 + +#elif TU_CHECK_MCU(OPT_MCU_STM32F7) + // FS has 6, HS has 9 + #define DCD_ATTR_ENDPOINT_MAX 9 + #define DCD_ATTR_DWC2_STM32 + +#elif TU_CHECK_MCU(OPT_MCU_STM32H7) + #define DCD_ATTR_ENDPOINT_MAX 9 + #define DCD_ATTR_DWC2_STM32 + +#elif TU_CHECK_MCU(OPT_MCU_STM32G4) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_STM32L0, OPT_MCU_STM32L1) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_STM32L4) + #if defined (STM32L475xx) || defined (STM32L476xx) || \ + defined (STM32L485xx) || defined (STM32L486xx) || defined (STM32L496xx) || \ + defined (STM32L4A6xx) || defined (STM32L4P5xx) || defined (STM32L4Q5xx) || \ + defined (STM32L4R5xx) || defined (STM32L4R7xx) || defined (STM32L4R9xx) || \ + defined (STM32L4S5xx) || defined (STM32L4S7xx) || defined (STM32L4S9xx) + #define DCD_ATTR_ENDPOINT_MAX 6 + #define DCD_ATTR_DWC2_STM32 + #else + #define DCD_ATTR_ENDPOINT_MAX 8 + #endif + +//------------- Sony -------------// +#elif TU_CHECK_MCU(OPT_MCU_CXD56) + #define DCD_ATTR_ENDPOINT_MAX 7 + #define DCD_ATTR_ENDPOINT_EXCLUSIVE_NUMBER + +//------------- TI -------------// +#elif TU_CHECK_MCU(OPT_MCU_MSP430x5xx) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_MSP432E4, OPT_MCU_TM4C123, OPT_MCU_TM4C129) + #define DCD_ATTR_ENDPOINT_MAX 8 + +//------------- ValentyUSB -------------// +#elif TU_CHECK_MCU(OPT_MCU_VALENTYUSB_EPTRI) + #define DCD_ATTR_ENDPOINT_MAX 16 + +//------------- Nuvoton -------------// +#elif TU_CHECK_MCU(OPT_MCU_NUC121, OPT_MCU_NUC126) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_NUC120) + #define DCD_ATTR_ENDPOINT_MAX 6 + +#elif TU_CHECK_MCU(OPT_MCU_NUC505) + #define DCD_ATTR_ENDPOINT_MAX 12 + +//------------- Espressif -------------// +#elif TU_CHECK_MCU(OPT_MCU_ESP32S2, OPT_MCU_ESP32S3) + #define DCD_ATTR_ENDPOINT_MAX 6 + +//------------- Dialog -------------// +#elif TU_CHECK_MCU(OPT_MCU_DA1469X) + #define DCD_ATTR_ENDPOINT_MAX 4 + +//------------- Raspberry Pi -------------// +#elif TU_CHECK_MCU(OPT_MCU_RP2040) + #define DCD_ATTR_ENDPOINT_MAX 16 + +//------------- Silabs -------------// +#elif TU_CHECK_MCU(OPT_MCU_EFM32GG) + #define DCD_ATTR_ENDPOINT_MAX 7 + +//------------- Renesas -------------// +#elif TU_CHECK_MCU(OPT_MCU_RX63X, OPT_MCU_RX65X, OPT_MCU_RX72N) + #define DCD_ATTR_ENDPOINT_MAX 10 + +//------------- GigaDevice -------------// +#elif TU_CHECK_MCU(OPT_MCU_GD32VF103) + #define DCD_ATTR_ENDPOINT_MAX 4 + +//------------- Broadcom -------------// +#elif TU_CHECK_MCU(OPT_MCU_BCM2711, OPT_MCU_BCM2835, OPT_MCU_BCM2837) + #define DCD_ATTR_ENDPOINT_MAX 8 + +//------------- Broadcom -------------// +#elif TU_CHECK_MCU(OPT_MCU_XMC4000) + #define DCD_ATTR_ENDPOINT_MAX 8 + +//------------- BridgeTek -------------// +#elif TU_CHECK_MCU(OPT_MCU_FT90X) + #define DCD_ATTR_ENDPOINT_MAX 8 + +#elif TU_CHECK_MCU(OPT_MCU_FT93X) + #define DCD_ATTR_ENDPOINT_MAX 16 + +//------------ Allwinner -------------// +#elif TU_CHECK_MCU(OPT_MCU_F1C100S) + #define DCD_ATTR_ENDPOINT_MAX 4 + +#else + #warning "DCD_ATTR_ENDPOINT_MAX is not defined for this MCU, default to 8" + #define DCD_ATTR_ENDPOINT_MAX 8 +#endif + +// Default to fullspeed if not defined +//#ifndef PORT_HIGHSPEED +// #define DCD_ATTR_PORT_HIGHSPEED 0x00 +//#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd.c new file mode 100644 index 000000000..448114303 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd.c @@ -0,0 +1,1419 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if TUSB_OPT_DEVICE_ENABLED + +#include "tusb.h" +#include "device/usbd.h" +#include "device/usbd_pvt.h" +#include "device/dcd.h" + +//--------------------------------------------------------------------+ +// USBD Configuration +//--------------------------------------------------------------------+ + +// Debug level of USBD +#define USBD_DBG 2 + +#ifndef CFG_TUD_TASK_QUEUE_SZ + #define CFG_TUD_TASK_QUEUE_SZ 16 +#endif + +//--------------------------------------------------------------------+ +// Device Data +//--------------------------------------------------------------------+ + +// Invalid driver ID in itf2drv[] ep2drv[][] mapping +enum { DRVID_INVALID = 0xFFu }; + +typedef struct +{ + struct TU_ATTR_PACKED + { + volatile uint8_t connected : 1; + volatile uint8_t addressed : 1; + volatile uint8_t suspended : 1; + + uint8_t remote_wakeup_en : 1; // enable/disable by host + uint8_t remote_wakeup_support : 1; // configuration descriptor's attribute + uint8_t self_powered : 1; // configuration descriptor's attribute + }; + + volatile uint8_t cfg_num; // current active configuration (0x00 is not configured) + uint8_t speed; + + uint8_t itf2drv[16]; // map interface number to driver (0xff is invalid) + uint8_t ep2drv[CFG_TUD_ENDPPOINT_MAX][2]; // map endpoint to driver ( 0xff is invalid ) + + struct TU_ATTR_PACKED + { + volatile bool busy : 1; + volatile bool stalled : 1; + volatile bool claimed : 1; + + // TODO merge ep2drv here, 4-bit should be sufficient + }ep_status[CFG_TUD_ENDPPOINT_MAX][2]; + +}usbd_device_t; + +static usbd_device_t _usbd_dev; + +//--------------------------------------------------------------------+ +// Class Driver +//--------------------------------------------------------------------+ +#if CFG_TUSB_DEBUG >= 2 + #define DRIVER_NAME(_name) .name = _name, +#else + #define DRIVER_NAME(_name) +#endif + +// Built-in class drivers +static usbd_class_driver_t const _usbd_driver[] = +{ + #if CFG_TUD_CDC + { + DRIVER_NAME("CDC") + .init = cdcd_init, + .reset = cdcd_reset, + .open = cdcd_open, + .control_xfer_cb = cdcd_control_xfer_cb, + .xfer_cb = cdcd_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_MSC + { + DRIVER_NAME("MSC") + .init = mscd_init, + .reset = mscd_reset, + .open = mscd_open, + .control_xfer_cb = mscd_control_xfer_cb, + .xfer_cb = mscd_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_HID + { + DRIVER_NAME("HID") + .init = hidd_init, + .reset = hidd_reset, + .open = hidd_open, + .control_xfer_cb = hidd_control_xfer_cb, + .xfer_cb = hidd_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_AUDIO + { + DRIVER_NAME("AUDIO") + .init = audiod_init, + .reset = audiod_reset, + .open = audiod_open, + .control_xfer_cb = audiod_control_xfer_cb, + .xfer_cb = audiod_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_VIDEO + { + DRIVER_NAME("VIDEO") + .init = videod_init, + .reset = videod_reset, + .open = videod_open, + .control_xfer_cb = videod_control_xfer_cb, + .xfer_cb = videod_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_MIDI + { + DRIVER_NAME("MIDI") + .init = midid_init, + .open = midid_open, + .reset = midid_reset, + .control_xfer_cb = midid_control_xfer_cb, + .xfer_cb = midid_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_VENDOR + { + DRIVER_NAME("VENDOR") + .init = vendord_init, + .reset = vendord_reset, + .open = vendord_open, + .control_xfer_cb = tud_vendor_control_xfer_cb, + .xfer_cb = vendord_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_USBTMC + { + DRIVER_NAME("TMC") + .init = usbtmcd_init_cb, + .reset = usbtmcd_reset_cb, + .open = usbtmcd_open_cb, + .control_xfer_cb = usbtmcd_control_xfer_cb, + .xfer_cb = usbtmcd_xfer_cb, + .sof = NULL + }, + #endif + + #if CFG_TUD_DFU_RUNTIME + { + DRIVER_NAME("DFU-RUNTIME") + .init = dfu_rtd_init, + .reset = dfu_rtd_reset, + .open = dfu_rtd_open, + .control_xfer_cb = dfu_rtd_control_xfer_cb, + .xfer_cb = NULL, + .sof = NULL + }, + #endif + + #if CFG_TUD_DFU + { + DRIVER_NAME("DFU") + .init = dfu_moded_init, + .reset = dfu_moded_reset, + .open = dfu_moded_open, + .control_xfer_cb = dfu_moded_control_xfer_cb, + .xfer_cb = NULL, + .sof = NULL + }, + #endif + + #if CFG_TUD_ECM_RNDIS || CFG_TUD_NCM + { + DRIVER_NAME("NET") + .init = netd_init, + .reset = netd_reset, + .open = netd_open, + .control_xfer_cb = netd_control_xfer_cb, + .xfer_cb = netd_xfer_cb, + .sof = NULL, + }, + #endif + + #if CFG_TUD_BTH + { + DRIVER_NAME("BTH") + .init = btd_init, + .reset = btd_reset, + .open = btd_open, + .control_xfer_cb = btd_control_xfer_cb, + .xfer_cb = btd_xfer_cb, + .sof = NULL + }, + #endif +}; + +enum { BUILTIN_DRIVER_COUNT = TU_ARRAY_SIZE(_usbd_driver) }; + +// Additional class drivers implemented by application +static usbd_class_driver_t const * _app_driver = NULL; +static uint8_t _app_driver_count = 0; + +// virtually joins built-in and application drivers together. +// Application is positioned first to allow overwriting built-in ones. +static inline usbd_class_driver_t const * get_driver(uint8_t drvid) +{ + // Application drivers + if ( usbd_app_driver_get_cb ) + { + if ( drvid < _app_driver_count ) return &_app_driver[drvid]; + drvid -= _app_driver_count; + } + + // Built-in drivers + if (drvid < BUILTIN_DRIVER_COUNT) return &_usbd_driver[drvid]; + + return NULL; +} + +#define TOTAL_DRIVER_COUNT (_app_driver_count + BUILTIN_DRIVER_COUNT) + +//--------------------------------------------------------------------+ +// DCD Event +//--------------------------------------------------------------------+ + +static bool _usbd_initialized = false; + +// Event queue +// OPT_MODE_DEVICE is used by OS NONE for mutex (disable usb isr) +OSAL_QUEUE_DEF(OPT_MODE_DEVICE, _usbd_qdef, CFG_TUD_TASK_QUEUE_SZ, dcd_event_t); +static osal_queue_t _usbd_q; + +// Mutex for claiming endpoint, only needed when using with preempted RTOS +#if CFG_TUSB_OS != OPT_OS_NONE +static osal_mutex_def_t _ubsd_mutexdef; +static osal_mutex_t _usbd_mutex; +#endif + + +//--------------------------------------------------------------------+ +// Prototypes +//--------------------------------------------------------------------+ +static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request); +static bool process_set_config(uint8_t rhport, uint8_t cfg_num); +static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const * p_request); + +// from usbd_control.c +void usbd_control_reset(void); +void usbd_control_set_request(tusb_control_request_t const *request); +void usbd_control_set_complete_callback( usbd_control_xfer_cb_t fp ); +bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); + + +//--------------------------------------------------------------------+ +// Debug +//--------------------------------------------------------------------+ +#if CFG_TUSB_DEBUG >= 2 +static char const* const _usbd_event_str[DCD_EVENT_COUNT] = +{ + "Invalid" , + "Bus Reset" , + "Unplugged" , + "SOF" , + "Suspend" , + "Resume" , + "Setup Received" , + "Xfer Complete" , + "Func Call" +}; + +static char const* const _tusb_std_request_str[] = +{ + "Get Status" , + "Clear Feature" , + "Reserved" , + "Set Feature" , + "Reserved" , + "Set Address" , + "Get Descriptor" , + "Set Descriptor" , + "Get Configuration" , + "Set Configuration" , + "Get Interface" , + "Set Interface" , + "Synch Frame" +}; + +static char const* const _tusb_speed_str[] = { "Full", "Low", "High" }; + +// for usbd_control to print the name of control complete driver +void usbd_driver_print_control_complete_name(usbd_control_xfer_cb_t callback) +{ + for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) + { + usbd_class_driver_t const * driver = get_driver(i); + if ( driver->control_xfer_cb == callback ) + { + TU_LOG2(" %s control complete\r\n", driver->name); + return; + } + } +} + +#endif + +//--------------------------------------------------------------------+ +// Application API +//--------------------------------------------------------------------+ +tusb_speed_t tud_speed_get(void) +{ + return (tusb_speed_t) _usbd_dev.speed; +} + +bool tud_connected(void) +{ + return _usbd_dev.connected; +} + +bool tud_mounted(void) +{ + return _usbd_dev.cfg_num ? true : false; +} + +bool tud_suspended(void) +{ + return _usbd_dev.suspended; +} + +bool tud_remote_wakeup(void) +{ + // only wake up host if this feature is supported and enabled and we are suspended + TU_VERIFY (_usbd_dev.suspended && _usbd_dev.remote_wakeup_support && _usbd_dev.remote_wakeup_en ); + dcd_remote_wakeup(TUD_OPT_RHPORT); + return true; +} + +bool tud_disconnect(void) +{ + TU_VERIFY(dcd_disconnect); + dcd_disconnect(TUD_OPT_RHPORT); + return true; +} + +bool tud_connect(void) +{ + TU_VERIFY(dcd_connect); + dcd_connect(TUD_OPT_RHPORT); + return true; +} + +//--------------------------------------------------------------------+ +// USBD Task +//--------------------------------------------------------------------+ +bool tud_inited(void) +{ + return _usbd_initialized; +} + +bool tud_init (uint8_t rhport) +{ + // skip if already initialized + if (_usbd_initialized) return _usbd_initialized; + + TU_LOG2("USBD init\r\n"); + + tu_varclr(&_usbd_dev); + +#if CFG_TUSB_OS != OPT_OS_NONE + // Init device mutex + _usbd_mutex = osal_mutex_create(&_ubsd_mutexdef); + TU_ASSERT(_usbd_mutex); +#endif + + // Init device queue & task + _usbd_q = osal_queue_create(&_usbd_qdef); + TU_ASSERT(_usbd_q); + + // Get application driver if available + if ( usbd_app_driver_get_cb ) + { + _app_driver = usbd_app_driver_get_cb(&_app_driver_count); + } + + // Init class drivers + for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) + { + usbd_class_driver_t const * driver = get_driver(i); + TU_LOG2("%s init\r\n", driver->name); + driver->init(); + } + + // Init device controller driver + dcd_init(rhport); + dcd_int_enable(rhport); + + _usbd_initialized = true; + + return true; +} + +static void configuration_reset(uint8_t rhport) +{ + for ( uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++ ) + { + get_driver(i)->reset(rhport); + } + + tu_varclr(&_usbd_dev); + memset(_usbd_dev.itf2drv, DRVID_INVALID, sizeof(_usbd_dev.itf2drv)); // invalid mapping + memset(_usbd_dev.ep2drv , DRVID_INVALID, sizeof(_usbd_dev.ep2drv )); // invalid mapping +} + +static void usbd_reset(uint8_t rhport) +{ + configuration_reset(rhport); + usbd_control_reset(); +} + +bool tud_task_event_ready(void) +{ + // Skip if stack is not initialized + if ( !tusb_inited() ) return false; + + return !osal_queue_empty(_usbd_q); +} + +/* USB Device Driver task + * This top level thread manages all device controller event and delegates events to class-specific drivers. + * This should be called periodically within the mainloop or rtos thread. + * + @code + int main(void) + { + application_init(); + tusb_init(); + + while(1) // the mainloop + { + application_code(); + tud_task(); // tinyusb device task + } + } + @endcode + */ +void tud_task (void) +{ + // Skip if stack is not initialized + if ( !tusb_inited() ) return; + + // Loop until there is no more events in the queue + while (1) + { + dcd_event_t event; + + if ( !osal_queue_receive(_usbd_q, &event) ) return; + +#if CFG_TUSB_DEBUG >= 2 + if (event.event_id == DCD_EVENT_SETUP_RECEIVED) TU_LOG2("\r\n"); // extra line for setup + TU_LOG2("USBD %s ", event.event_id < DCD_EVENT_COUNT ? _usbd_event_str[event.event_id] : "CORRUPTED"); +#endif + + switch ( event.event_id ) + { + case DCD_EVENT_BUS_RESET: + TU_LOG2(": %s Speed\r\n", _tusb_speed_str[event.bus_reset.speed]); + usbd_reset(event.rhport); + _usbd_dev.speed = event.bus_reset.speed; + break; + + case DCD_EVENT_UNPLUGGED: + TU_LOG2("\r\n"); + usbd_reset(event.rhport); + + // invoke callback + if (tud_umount_cb) tud_umount_cb(); + break; + + case DCD_EVENT_SETUP_RECEIVED: + TU_LOG2_VAR(&event.setup_received); + TU_LOG2("\r\n"); + + // Mark as connected after receiving 1st setup packet. + // But it is easier to set it every time instead of wasting time to check then set + _usbd_dev.connected = 1; + + // mark both in & out control as free + _usbd_dev.ep_status[0][TUSB_DIR_OUT].busy = false; + _usbd_dev.ep_status[0][TUSB_DIR_OUT].claimed = 0; + _usbd_dev.ep_status[0][TUSB_DIR_IN ].busy = false; + _usbd_dev.ep_status[0][TUSB_DIR_IN ].claimed = 0; + + // Process control request + if ( !process_control_request(event.rhport, &event.setup_received) ) + { + TU_LOG2(" Stall EP0\r\n"); + // Failed -> stall both control endpoint IN and OUT + dcd_edpt_stall(event.rhport, 0); + dcd_edpt_stall(event.rhport, 0 | TUSB_DIR_IN_MASK); + } + break; + + case DCD_EVENT_XFER_COMPLETE: + { + // Invoke the class callback associated with the endpoint address + uint8_t const ep_addr = event.xfer_complete.ep_addr; + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const ep_dir = tu_edpt_dir(ep_addr); + + TU_LOG2("on EP %02X with %u bytes\r\n", ep_addr, (unsigned int) event.xfer_complete.len); + + _usbd_dev.ep_status[epnum][ep_dir].busy = false; + _usbd_dev.ep_status[epnum][ep_dir].claimed = 0; + + if ( 0 == epnum ) + { + usbd_control_xfer_cb(event.rhport, ep_addr, (xfer_result_t)event.xfer_complete.result, event.xfer_complete.len); + } + else + { + usbd_class_driver_t const * driver = get_driver( _usbd_dev.ep2drv[epnum][ep_dir] ); + TU_ASSERT(driver, ); + + TU_LOG2(" %s xfer callback\r\n", driver->name); + driver->xfer_cb(event.rhport, ep_addr, (xfer_result_t)event.xfer_complete.result, event.xfer_complete.len); + } + } + break; + + case DCD_EVENT_SUSPEND: + // NOTE: When plugging/unplugging device, the D+/D- state are unstable and + // can accidentally meet the SUSPEND condition ( Bus Idle for 3ms ), which result in a series of event + // e.g suspend -> resume -> unplug/plug. Skip suspend/resume if not connected + if ( _usbd_dev.connected ) + { + TU_LOG2(": Remote Wakeup = %u\r\n", _usbd_dev.remote_wakeup_en); + if (tud_suspend_cb) tud_suspend_cb(_usbd_dev.remote_wakeup_en); + }else + { + TU_LOG2(" Skipped\r\n"); + } + break; + + case DCD_EVENT_RESUME: + if ( _usbd_dev.connected ) + { + TU_LOG2("\r\n"); + if (tud_resume_cb) tud_resume_cb(); + }else + { + TU_LOG2(" Skipped\r\n"); + } + break; + + case DCD_EVENT_SOF: + TU_LOG2("\r\n"); + for ( uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++ ) + { + usbd_class_driver_t const * driver = get_driver(i); + if ( driver->sof ) driver->sof(event.rhport); + } + break; + + case USBD_EVENT_FUNC_CALL: + TU_LOG2("\r\n"); + if ( event.func_call.func ) event.func_call.func(event.func_call.param); + break; + + default: + TU_BREAKPOINT(); + break; + } + } +} + +//--------------------------------------------------------------------+ +// Control Request Parser & Handling +//--------------------------------------------------------------------+ + +// Helper to invoke class driver control request handler +static bool invoke_class_control(uint8_t rhport, usbd_class_driver_t const * driver, tusb_control_request_t const * request) +{ + usbd_control_set_complete_callback(driver->control_xfer_cb); + TU_LOG2(" %s control request\r\n", driver->name); + return driver->control_xfer_cb(rhport, CONTROL_STAGE_SETUP, request); +} + +// This handles the actual request and its response. +// return false will cause its caller to stall control endpoint +static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request) +{ + usbd_control_set_complete_callback(NULL); + + TU_ASSERT(p_request->bmRequestType_bit.type < TUSB_REQ_TYPE_INVALID); + + // Vendor request + if ( p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_VENDOR ) + { + TU_VERIFY(tud_vendor_control_xfer_cb); + + usbd_control_set_complete_callback(tud_vendor_control_xfer_cb); + return tud_vendor_control_xfer_cb(rhport, CONTROL_STAGE_SETUP, p_request); + } + +#if CFG_TUSB_DEBUG >= 2 + if (TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type && p_request->bRequest <= TUSB_REQ_SYNCH_FRAME) + { + TU_LOG2(" %s", _tusb_std_request_str[p_request->bRequest]); + if (TUSB_REQ_GET_DESCRIPTOR != p_request->bRequest) TU_LOG2("\r\n"); + } +#endif + + switch ( p_request->bmRequestType_bit.recipient ) + { + //------------- Device Requests e.g in enumeration -------------// + case TUSB_REQ_RCPT_DEVICE: + if ( TUSB_REQ_TYPE_CLASS == p_request->bmRequestType_bit.type ) + { + uint8_t const itf = tu_u16_low(p_request->wIndex); + TU_VERIFY(itf < TU_ARRAY_SIZE(_usbd_dev.itf2drv)); + + usbd_class_driver_t const * driver = get_driver(_usbd_dev.itf2drv[itf]); + TU_VERIFY(driver); + + // forward to class driver: "non-STD request to Interface" + return invoke_class_control(rhport, driver, p_request); + } + + if ( TUSB_REQ_TYPE_STANDARD != p_request->bmRequestType_bit.type ) + { + // Non standard request is not supported + TU_BREAKPOINT(); + return false; + } + + switch ( p_request->bRequest ) + { + case TUSB_REQ_SET_ADDRESS: + // Depending on mcu, status phase could be sent either before or after changing device address, + // or even require stack to not response with status at all + // Therefore DCD must take full responsibility to response and include zlp status packet if needed. + usbd_control_set_request(p_request); // set request since DCD has no access to tud_control_status() API + dcd_set_address(rhport, (uint8_t) p_request->wValue); + // skip tud_control_status() + _usbd_dev.addressed = 1; + break; + + case TUSB_REQ_GET_CONFIGURATION: + { + uint8_t cfg_num = _usbd_dev.cfg_num; + tud_control_xfer(rhport, p_request, &cfg_num, 1); + } + break; + + case TUSB_REQ_SET_CONFIGURATION: + { + uint8_t const cfg_num = (uint8_t) p_request->wValue; + + // Only process if new configure is different + if (_usbd_dev.cfg_num != cfg_num) + { + if ( _usbd_dev.cfg_num ) + { + // already configured: need to clear all endpoints and driver first + TU_LOG(USBD_DBG, " Clear current Configuration (%u) before switching\r\n", _usbd_dev.cfg_num); + + // close all non-control endpoints, cancel all pending transfers if any + dcd_edpt_close_all(rhport); + + // close all drivers and current configured state except bus speed + uint8_t const speed = _usbd_dev.speed; + configuration_reset(rhport); + + _usbd_dev.speed = speed; // restore speed + } + + // switch to new configuration if not zero + if ( cfg_num ) TU_ASSERT( process_set_config(rhport, cfg_num) ); + } + + _usbd_dev.cfg_num = cfg_num; + tud_control_status(rhport, p_request); + } + break; + + case TUSB_REQ_GET_DESCRIPTOR: + TU_VERIFY( process_get_descriptor(rhport, p_request) ); + break; + + case TUSB_REQ_SET_FEATURE: + // Only support remote wakeup for device feature + TU_VERIFY(TUSB_REQ_FEATURE_REMOTE_WAKEUP == p_request->wValue); + + TU_LOG(USBD_DBG, " Enable Remote Wakeup\r\n"); + + // Host may enable remote wake up before suspending especially HID device + _usbd_dev.remote_wakeup_en = true; + tud_control_status(rhport, p_request); + break; + + case TUSB_REQ_CLEAR_FEATURE: + // Only support remote wakeup for device feature + TU_VERIFY(TUSB_REQ_FEATURE_REMOTE_WAKEUP == p_request->wValue); + + TU_LOG(USBD_DBG, " Disable Remote Wakeup\r\n"); + + // Host may disable remote wake up after resuming + _usbd_dev.remote_wakeup_en = false; + tud_control_status(rhport, p_request); + break; + + case TUSB_REQ_GET_STATUS: + { + // Device status bit mask + // - Bit 0: Self Powered + // - Bit 1: Remote Wakeup enabled + uint16_t status = (_usbd_dev.self_powered ? 1 : 0) | (_usbd_dev.remote_wakeup_en ? 2 : 0); + tud_control_xfer(rhport, p_request, &status, 2); + } + break; + + // Unknown/Unsupported request + default: TU_BREAKPOINT(); return false; + } + break; + + //------------- Class/Interface Specific Request -------------// + case TUSB_REQ_RCPT_INTERFACE: + { + uint8_t const itf = tu_u16_low(p_request->wIndex); + TU_VERIFY(itf < TU_ARRAY_SIZE(_usbd_dev.itf2drv)); + + usbd_class_driver_t const * driver = get_driver(_usbd_dev.itf2drv[itf]); + TU_VERIFY(driver); + + // all requests to Interface (STD or Class) is forwarded to class driver. + // notable requests are: GET HID REPORT DESCRIPTOR, SET_INTERFACE, GET_INTERFACE + if ( !invoke_class_control(rhport, driver, p_request) ) + { + // For GET_INTERFACE and SET_INTERFACE, it is mandatory to respond even if the class + // driver doesn't use alternate settings or implement this + TU_VERIFY(TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type); + + switch(p_request->bRequest) + { + case TUSB_REQ_GET_INTERFACE: + case TUSB_REQ_SET_INTERFACE: + // Clear complete callback if driver set since it can also stall the request. + usbd_control_set_complete_callback(NULL); + + if (TUSB_REQ_GET_INTERFACE == p_request->bRequest) + { + uint8_t alternate = 0; + tud_control_xfer(rhport, p_request, &alternate, 1); + }else + { + tud_control_status(rhport, p_request); + } + break; + + default: return false; + } + } + } + break; + + //------------- Endpoint Request -------------// + case TUSB_REQ_RCPT_ENDPOINT: + { + uint8_t const ep_addr = tu_u16_low(p_request->wIndex); + uint8_t const ep_num = tu_edpt_number(ep_addr); + uint8_t const ep_dir = tu_edpt_dir(ep_addr); + + TU_ASSERT(ep_num < TU_ARRAY_SIZE(_usbd_dev.ep2drv) ); + + usbd_class_driver_t const * driver = get_driver(_usbd_dev.ep2drv[ep_num][ep_dir]); + + if ( TUSB_REQ_TYPE_STANDARD != p_request->bmRequestType_bit.type ) + { + // Forward class request to its driver + TU_VERIFY(driver); + return invoke_class_control(rhport, driver, p_request); + } + else + { + // Handle STD request to endpoint + switch ( p_request->bRequest ) + { + case TUSB_REQ_GET_STATUS: + { + uint16_t status = usbd_edpt_stalled(rhport, ep_addr) ? 0x0001 : 0x0000; + tud_control_xfer(rhport, p_request, &status, 2); + } + break; + + case TUSB_REQ_CLEAR_FEATURE: + case TUSB_REQ_SET_FEATURE: + { + if ( TUSB_REQ_FEATURE_EDPT_HALT == p_request->wValue ) + { + if ( TUSB_REQ_CLEAR_FEATURE == p_request->bRequest ) + { + usbd_edpt_clear_stall(rhport, ep_addr); + }else + { + usbd_edpt_stall(rhport, ep_addr); + } + } + + if (driver) + { + // Some classes such as USBTMC needs to clear/re-init its buffer when receiving CLEAR_FEATURE request + // We will also forward std request targeted endpoint to class drivers as well + + // STD request must always be ACKed regardless of driver returned value + // Also clear complete callback if driver set since it can also stall the request. + (void) invoke_class_control(rhport, driver, p_request); + usbd_control_set_complete_callback(NULL); + + // skip ZLP status if driver already did that + if ( !_usbd_dev.ep_status[0][TUSB_DIR_IN].busy ) tud_control_status(rhport, p_request); + } + } + break; + + // Unknown/Unsupported request + default: TU_BREAKPOINT(); return false; + } + } + } + break; + + // Unknown recipient + default: TU_BREAKPOINT(); return false; + } + + return true; +} + +// Process Set Configure Request +// This function parse configuration descriptor & open drivers accordingly +static bool process_set_config(uint8_t rhport, uint8_t cfg_num) +{ + // index is cfg_num-1 + tusb_desc_configuration_t const * desc_cfg = (tusb_desc_configuration_t const *) tud_descriptor_configuration_cb(cfg_num-1); + TU_ASSERT(desc_cfg != NULL && desc_cfg->bDescriptorType == TUSB_DESC_CONFIGURATION); + + // Parse configuration descriptor + _usbd_dev.remote_wakeup_support = (desc_cfg->bmAttributes & TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP) ? 1 : 0; + _usbd_dev.self_powered = (desc_cfg->bmAttributes & TUSB_DESC_CONFIG_ATT_SELF_POWERED ) ? 1 : 0; + + // Parse interface descriptor + uint8_t const * p_desc = ((uint8_t const*) desc_cfg) + sizeof(tusb_desc_configuration_t); + uint8_t const * desc_end = ((uint8_t const*) desc_cfg) + tu_le16toh(desc_cfg->wTotalLength); + + while( p_desc < desc_end ) + { + uint8_t assoc_itf_count = 1; + + // Class will always starts with Interface Association (if any) and then Interface descriptor + if ( TUSB_DESC_INTERFACE_ASSOCIATION == tu_desc_type(p_desc) ) + { + tusb_desc_interface_assoc_t const * desc_iad = (tusb_desc_interface_assoc_t const *) p_desc; + assoc_itf_count = desc_iad->bInterfaceCount; + + p_desc = tu_desc_next(p_desc); // next to Interface + + // IAD's first interface number and class should match with opened interface + //TU_ASSERT(desc_iad->bFirstInterface == desc_itf->bInterfaceNumber && + // desc_iad->bFunctionClass == desc_itf->bInterfaceClass); + } + + TU_ASSERT( TUSB_DESC_INTERFACE == tu_desc_type(p_desc) ); + tusb_desc_interface_t const * desc_itf = (tusb_desc_interface_t const*) p_desc; + + // Find driver for this interface + uint16_t const remaining_len = desc_end-p_desc; + uint8_t drv_id; + for (drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) + { + usbd_class_driver_t const *driver = get_driver(drv_id); + uint16_t const drv_len = driver->open(rhport, desc_itf, remaining_len); + + if ( (sizeof(tusb_desc_interface_t) <= drv_len) && (drv_len <= remaining_len) ) + { + // Open successfully + TU_LOG2(" %s opened\r\n", driver->name); + + // Some drivers use 2 or more interfaces but may not have IAD e.g MIDI (always) or + // BTH (even CDC) with class in device descriptor (single interface) + if ( assoc_itf_count == 1) + { + #if CFG_TUD_CDC + if ( driver->open == cdcd_open ) assoc_itf_count = 2; + #endif + + #if CFG_TUD_MIDI + if ( driver->open == midid_open ) assoc_itf_count = 2; + #endif + + #if CFG_TUD_BTH && CFG_TUD_BTH_ISO_ALT_COUNT + if ( driver->open == btd_open ) assoc_itf_count = 2; + #endif + } + + // bind (associated) interfaces to found driver + for(uint8_t i=0; ibInterfaceNumber+i; + + // Interface number must not be used already + TU_ASSERT(DRVID_INVALID == _usbd_dev.itf2drv[itf_num]); + _usbd_dev.itf2drv[itf_num] = drv_id; + } + + // bind all endpoints to found driver + tu_edpt_bind_driver(_usbd_dev.ep2drv, desc_itf, drv_len, drv_id); + + // next Interface + p_desc += drv_len; + + break; // exit driver find loop + } + } + + // Failed if there is no supported drivers + TU_ASSERT(drv_id < TOTAL_DRIVER_COUNT); + } + + // invoke callback + if (tud_mount_cb) tud_mount_cb(); + + return true; +} + +// return descriptor's buffer and update desc_len +static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const * p_request) +{ + tusb_desc_type_t const desc_type = (tusb_desc_type_t) tu_u16_high(p_request->wValue); + uint8_t const desc_index = tu_u16_low( p_request->wValue ); + + switch(desc_type) + { + case TUSB_DESC_DEVICE: + { + TU_LOG2(" Device\r\n"); + + void* desc_device = (void*) (uintptr_t) tud_descriptor_device_cb(); + + // Only response with exactly 1 Packet if: not addressed and host requested more data than device descriptor has. + // This only happens with the very first get device descriptor and EP0 size = 8 or 16. + if ((CFG_TUD_ENDPOINT0_SIZE < sizeof(tusb_desc_device_t)) && !_usbd_dev.addressed && + ((tusb_control_request_t const*) p_request)->wLength > sizeof(tusb_desc_device_t)) + { + // Hack here: we modify the request length to prevent usbd_control response with zlp + // since we are responding with 1 packet & less data than wLength. + tusb_control_request_t mod_request = *p_request; + mod_request.wLength = CFG_TUD_ENDPOINT0_SIZE; + + return tud_control_xfer(rhport, &mod_request, desc_device, CFG_TUD_ENDPOINT0_SIZE); + }else + { + return tud_control_xfer(rhport, p_request, desc_device, sizeof(tusb_desc_device_t)); + } + } + // break; // unreachable + + case TUSB_DESC_BOS: + { + TU_LOG2(" BOS\r\n"); + + // requested by host if USB > 2.0 ( i.e 2.1 or 3.x ) + if (!tud_descriptor_bos_cb) return false; + + uintptr_t desc_bos = (uintptr_t) tud_descriptor_bos_cb(); + TU_ASSERT(desc_bos); + + // Use offsetof to avoid pointer to the odd/misaligned address + uint16_t const total_len = tu_le16toh( tu_unaligned_read16((const void*) (desc_bos + offsetof(tusb_desc_bos_t, wTotalLength))) ); + + return tud_control_xfer(rhport, p_request, (void*) desc_bos, total_len); + } + // break; // unreachable + + case TUSB_DESC_CONFIGURATION: + case TUSB_DESC_OTHER_SPEED_CONFIG: + { + uintptr_t desc_config; + + if ( desc_type == TUSB_DESC_CONFIGURATION ) + { + TU_LOG2(" Configuration[%u]\r\n", desc_index); + desc_config = (uintptr_t) tud_descriptor_configuration_cb(desc_index); + }else + { + // Host only request this after getting Device Qualifier descriptor + TU_LOG2(" Other Speed Configuration\r\n"); + TU_VERIFY( tud_descriptor_other_speed_configuration_cb ); + desc_config = (uintptr_t) tud_descriptor_other_speed_configuration_cb(desc_index); + } + + TU_ASSERT(desc_config); + + // Use offsetof to avoid pointer to the odd/misaligned address + uint16_t const total_len = tu_le16toh( tu_unaligned_read16((const void*) (desc_config + offsetof(tusb_desc_configuration_t, wTotalLength))) ); + + return tud_control_xfer(rhport, p_request, (void*) desc_config, total_len); + } + // break; // unreachable + + case TUSB_DESC_STRING: + { + TU_LOG2(" String[%u]\r\n", desc_index); + + // String Descriptor always uses the desc set from user + uint8_t const* desc_str = (uint8_t const*) tud_descriptor_string_cb(desc_index, tu_le16toh(p_request->wIndex)); + TU_VERIFY(desc_str); + + // first byte of descriptor is its size + return tud_control_xfer(rhport, p_request, (void*) (uintptr_t) desc_str, tu_desc_len(desc_str)); + } + // break; // unreachable + + case TUSB_DESC_DEVICE_QUALIFIER: + { + TU_LOG2(" Device Qualifier\r\n"); + + TU_VERIFY( tud_descriptor_device_qualifier_cb ); + + uint8_t const* desc_qualifier = tud_descriptor_device_qualifier_cb(); + TU_VERIFY(desc_qualifier); + + // first byte of descriptor is its size + return tud_control_xfer(rhport, p_request, (void*) (uintptr_t) desc_qualifier, tu_desc_len(desc_qualifier)); + } + // break; // unreachable + + default: return false; + } +} + +//--------------------------------------------------------------------+ +// DCD Event Handler +//--------------------------------------------------------------------+ +void dcd_event_handler(dcd_event_t const * event, bool in_isr) +{ + switch (event->event_id) + { + case DCD_EVENT_UNPLUGGED: + _usbd_dev.connected = 0; + _usbd_dev.addressed = 0; + _usbd_dev.cfg_num = 0; + _usbd_dev.suspended = 0; + osal_queue_send(_usbd_q, event, in_isr); + break; + + case DCD_EVENT_SUSPEND: + // NOTE: When plugging/unplugging device, the D+/D- state are unstable and + // can accidentally meet the SUSPEND condition ( Bus Idle for 3ms ). + // In addition, some MCUs such as SAMD or boards that haven no VBUS detection cannot distinguish + // suspended vs disconnected. We will skip handling SUSPEND/RESUME event if not currently connected + if ( _usbd_dev.connected ) + { + _usbd_dev.suspended = 1; + osal_queue_send(_usbd_q, event, in_isr); + } + break; + + case DCD_EVENT_RESUME: + // skip event if not connected (especially required for SAMD) + if ( _usbd_dev.connected ) + { + _usbd_dev.suspended = 0; + osal_queue_send(_usbd_q, event, in_isr); + } + break; + + case DCD_EVENT_SOF: + // Some MCUs after running dcd_remote_wakeup() does not have way to detect the end of remote wakeup + // which last 1-15 ms. DCD can use SOF as a clear indicator that bus is back to operational + if ( _usbd_dev.suspended ) + { + _usbd_dev.suspended = 0; + dcd_event_t const event_resume = { .rhport = event->rhport, .event_id = DCD_EVENT_RESUME }; + osal_queue_send(_usbd_q, &event_resume, in_isr); + } + break; + + default: + osal_queue_send(_usbd_q, event, in_isr); + break; + } +} + +void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr) +{ + dcd_event_t event = { .rhport = rhport, .event_id = eid }; + dcd_event_handler(&event, in_isr); +} + +void dcd_event_bus_reset (uint8_t rhport, tusb_speed_t speed, bool in_isr) +{ + dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_BUS_RESET }; + event.bus_reset.speed = speed; + dcd_event_handler(&event, in_isr); +} + +void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr) +{ + dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_SETUP_RECEIVED }; + memcpy(&event.setup_received, setup, 8); + + dcd_event_handler(&event, in_isr); +} + +void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr) +{ + dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_XFER_COMPLETE }; + + event.xfer_complete.ep_addr = ep_addr; + event.xfer_complete.len = xferred_bytes; + event.xfer_complete.result = result; + + dcd_event_handler(&event, in_isr); +} + +//--------------------------------------------------------------------+ +// USBD API For Class Driver +//--------------------------------------------------------------------+ + +// Parse consecutive endpoint descriptors (IN & OUT) +bool usbd_open_edpt_pair(uint8_t rhport, uint8_t const* p_desc, uint8_t ep_count, uint8_t xfer_type, uint8_t* ep_out, uint8_t* ep_in) +{ + for(int i=0; ibDescriptorType && xfer_type == desc_ep->bmAttributes.xfer); + TU_ASSERT(usbd_edpt_open(rhport, desc_ep)); + + if ( tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN ) + { + (*ep_in) = desc_ep->bEndpointAddress; + }else + { + (*ep_out) = desc_ep->bEndpointAddress; + } + + p_desc = tu_desc_next(p_desc); + } + + return true; +} + +// Helper to defer an isr function +void usbd_defer_func(osal_task_func_t func, void* param, bool in_isr) +{ + dcd_event_t event = + { + .rhport = 0, + .event_id = USBD_EVENT_FUNC_CALL, + }; + + event.func_call.func = func; + event.func_call.param = param; + + dcd_event_handler(&event, in_isr); +} + +//--------------------------------------------------------------------+ +// USBD Endpoint API +//--------------------------------------------------------------------+ + +bool usbd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep) +{ + TU_ASSERT(tu_edpt_number(desc_ep->bEndpointAddress) < CFG_TUD_ENDPPOINT_MAX); + TU_ASSERT(tu_edpt_validate(desc_ep, (tusb_speed_t) _usbd_dev.speed)); + + return dcd_edpt_open(rhport, desc_ep); +} + +bool usbd_edpt_claim(uint8_t rhport, uint8_t ep_addr) +{ + (void) rhport; + + // TODO add this check later, also make sure we don't starve an out endpoint while suspending + // TU_VERIFY(tud_ready()); + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + +#if CFG_TUSB_OS != OPT_OS_NONE + // pre-check to help reducing mutex lock + TU_VERIFY((_usbd_dev.ep_status[epnum][dir].busy == 0) && (_usbd_dev.ep_status[epnum][dir].claimed == 0)); + osal_mutex_lock(_usbd_mutex, OSAL_TIMEOUT_WAIT_FOREVER); +#endif + + // can only claim the endpoint if it is not busy and not claimed yet. + bool const ret = (_usbd_dev.ep_status[epnum][dir].busy == 0) && (_usbd_dev.ep_status[epnum][dir].claimed == 0); + if (ret) + { + _usbd_dev.ep_status[epnum][dir].claimed = 1; + } + +#if CFG_TUSB_OS != OPT_OS_NONE + osal_mutex_unlock(_usbd_mutex); +#endif + + return ret; +} + +bool usbd_edpt_release(uint8_t rhport, uint8_t ep_addr) +{ + (void) rhport; + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + +#if CFG_TUSB_OS != OPT_OS_NONE + osal_mutex_lock(_usbd_mutex, OSAL_TIMEOUT_WAIT_FOREVER); +#endif + + // can only release the endpoint if it is claimed and not busy + bool const ret = (_usbd_dev.ep_status[epnum][dir].busy == 0) && (_usbd_dev.ep_status[epnum][dir].claimed == 1); + if (ret) + { + _usbd_dev.ep_status[epnum][dir].claimed = 0; + } + +#if CFG_TUSB_OS != OPT_OS_NONE + osal_mutex_unlock(_usbd_mutex); +#endif + + return ret; +} + +bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes) +{ + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + // TODO skip ready() check for now since enumeration also use this API + // TU_VERIFY(tud_ready()); + + TU_LOG2(" Queue EP %02X with %u bytes ...\r\n", ep_addr, total_bytes); + + // Attempt to transfer on a busy endpoint, sound like an race condition ! + TU_ASSERT(_usbd_dev.ep_status[epnum][dir].busy == 0); + + // Set busy first since the actual transfer can be complete before dcd_edpt_xfer() + // could return and USBD task can preempt and clear the busy + _usbd_dev.ep_status[epnum][dir].busy = true; + + if ( dcd_edpt_xfer(rhport, ep_addr, buffer, total_bytes) ) + { + return true; + }else + { + // DCD error, mark endpoint as ready to allow next transfer + _usbd_dev.ep_status[epnum][dir].busy = false; + _usbd_dev.ep_status[epnum][dir].claimed = 0; + TU_LOG2("FAILED\r\n"); + TU_BREAKPOINT(); + return false; + } +} + +// The number of bytes has to be given explicitly to allow more flexible control of how many +// bytes should be written and second to keep the return value free to give back a boolean +// success message. If total_bytes is too big, the FIFO will copy only what is available +// into the USB buffer! +bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes) +{ + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + TU_LOG2(" Queue ISO EP %02X with %u bytes ... ", ep_addr, total_bytes); + + // Attempt to transfer on a busy endpoint, sound like an race condition ! + TU_ASSERT(_usbd_dev.ep_status[epnum][dir].busy == 0); + + // Set busy first since the actual transfer can be complete before dcd_edpt_xfer() could return + // and usbd task can preempt and clear the busy + _usbd_dev.ep_status[epnum][dir].busy = true; + + if (dcd_edpt_xfer_fifo(rhport, ep_addr, ff, total_bytes)) + { + TU_LOG2("OK\r\n"); + return true; + }else + { + // DCD error, mark endpoint as ready to allow next transfer + _usbd_dev.ep_status[epnum][dir].busy = false; + _usbd_dev.ep_status[epnum][dir].claimed = 0; + TU_LOG2("failed\r\n"); + TU_BREAKPOINT(); + return false; + } +} + +bool usbd_edpt_busy(uint8_t rhport, uint8_t ep_addr) +{ + (void) rhport; + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + return _usbd_dev.ep_status[epnum][dir].busy; +} + +void usbd_edpt_stall(uint8_t rhport, uint8_t ep_addr) +{ + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + // only stalled if currently cleared + if ( !_usbd_dev.ep_status[epnum][dir].stalled ) + { + TU_LOG(USBD_DBG, " Stall EP %02X\r\n", ep_addr); + dcd_edpt_stall(rhport, ep_addr); + _usbd_dev.ep_status[epnum][dir].stalled = true; + _usbd_dev.ep_status[epnum][dir].busy = true; + } +} + +void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) +{ + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + // only clear if currently stalled + if ( _usbd_dev.ep_status[epnum][dir].stalled ) + { + TU_LOG(USBD_DBG, " Clear Stall EP %02X\r\n", ep_addr); + dcd_edpt_clear_stall(rhport, ep_addr); + _usbd_dev.ep_status[epnum][dir].stalled = false; + _usbd_dev.ep_status[epnum][dir].busy = false; + } +} + +bool usbd_edpt_stalled(uint8_t rhport, uint8_t ep_addr) +{ + (void) rhport; + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + return _usbd_dev.ep_status[epnum][dir].stalled; +} + +/** + * usbd_edpt_close will disable an endpoint. + * + * In progress transfers on this EP may be delivered after this call. + * + */ +void usbd_edpt_close(uint8_t rhport, uint8_t ep_addr) +{ + TU_ASSERT(dcd_edpt_close, /**/); + TU_LOG2(" CLOSING Endpoint: 0x%02X\r\n", ep_addr); + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + dcd_edpt_close(rhport, ep_addr); + _usbd_dev.ep_status[epnum][dir].stalled = false; + _usbd_dev.ep_status[epnum][dir].busy = false; + _usbd_dev.ep_status[epnum][dir].claimed = false; + + return; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd.h new file mode 100644 index 000000000..ec34817fa --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd.h @@ -0,0 +1,853 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_USBD_H_ +#define _TUSB_USBD_H_ + +#include "common/tusb_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Application API +//--------------------------------------------------------------------+ + +// Init device stack +bool tud_init (uint8_t rhport); + +// Check if device stack is already initialized +bool tud_inited(void); + +// Task function should be called in main/rtos loop +void tud_task (void); + +// Check if there is pending events need proccessing by tud_task() +bool tud_task_event_ready(void); + +// Interrupt handler, name alias to DCD +extern void dcd_int_handler(uint8_t rhport); +#define tud_int_handler dcd_int_handler + +// Get current bus speed +tusb_speed_t tud_speed_get(void); + +// Check if device is connected (may not mounted/configured yet) +// True if just got out of Bus Reset and received the very first data from host +bool tud_connected(void); + +// Check if device is connected and configured +bool tud_mounted(void); + +// Check if device is suspended +bool tud_suspended(void); + +// Check if device is ready to transfer +TU_ATTR_ALWAYS_INLINE static inline +bool tud_ready(void) +{ + return tud_mounted() && !tud_suspended(); +} + +// Remote wake up host, only if suspended and enabled by host +bool tud_remote_wakeup(void); + +// Enable pull-up resistor on D+ D- +// Return false on unsupported MCUs +bool tud_disconnect(void); + +// Disable pull-up resistor on D+ D- +// Return false on unsupported MCUs +bool tud_connect(void); + +// Carry out Data and Status stage of control transfer +// - If len = 0, it is equivalent to sending status only +// - If len > wLength : it will be truncated +bool tud_control_xfer(uint8_t rhport, tusb_control_request_t const * request, void* buffer, uint16_t len); + +// Send STATUS (zero length) packet +bool tud_control_status(uint8_t rhport, tusb_control_request_t const * request); + +//--------------------------------------------------------------------+ +// Application Callbacks (WEAK is optional) +//--------------------------------------------------------------------+ + +// Invoked when received GET DEVICE DESCRIPTOR request +// Application return pointer to descriptor +uint8_t const * tud_descriptor_device_cb(void); + +// Invoked when received GET CONFIGURATION DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete +uint8_t const * tud_descriptor_configuration_cb(uint8_t index); + +// Invoked when received GET STRING DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete +uint16_t const* tud_descriptor_string_cb(uint8_t index, uint16_t langid); + +// Invoked when received GET BOS DESCRIPTOR request +// Application return pointer to descriptor +TU_ATTR_WEAK uint8_t const * tud_descriptor_bos_cb(void); + +// Invoked when received GET DEVICE QUALIFIER DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete. +// device_qualifier descriptor describes information about a high-speed capable device that would +// change if the device were operating at the other speed. If not highspeed capable stall this request. +TU_ATTR_WEAK uint8_t const* tud_descriptor_device_qualifier_cb(void); + +// Invoked when received GET OTHER SEED CONFIGURATION DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long enough for transfer to complete +// Configuration descriptor in the other speed e.g if high speed then this is for full speed and vice versa +TU_ATTR_WEAK uint8_t const* tud_descriptor_other_speed_configuration_cb(uint8_t index); + +// Invoked when device is mounted (configured) +TU_ATTR_WEAK void tud_mount_cb(void); + +// Invoked when device is unmounted +TU_ATTR_WEAK void tud_umount_cb(void); + +// Invoked when usb bus is suspended +// Within 7ms, device must draw an average of current less than 2.5 mA from bus +TU_ATTR_WEAK void tud_suspend_cb(bool remote_wakeup_en); + +// Invoked when usb bus is resumed +TU_ATTR_WEAK void tud_resume_cb(void); + +// Invoked when received control request with VENDOR TYPE +TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); + +//--------------------------------------------------------------------+ +// Binary Device Object Store (BOS) Descriptor Templates +//--------------------------------------------------------------------+ + +#define TUD_BOS_DESC_LEN 5 + +// total length, number of device caps +#define TUD_BOS_DESCRIPTOR(_total_len, _caps_num) \ + 5, TUSB_DESC_BOS, U16_TO_U8S_LE(_total_len), _caps_num + +// Device Capability Platform 128-bit UUID + Data +#define TUD_BOS_PLATFORM_DESCRIPTOR(...) \ + 4+TU_ARGS_NUM(__VA_ARGS__), TUSB_DESC_DEVICE_CAPABILITY, DEVICE_CAPABILITY_PLATFORM, 0x00, __VA_ARGS__ + +//------------- WebUSB BOS Platform -------------// + +// Descriptor Length +#define TUD_BOS_WEBUSB_DESC_LEN 24 + +// Vendor Code, iLandingPage +#define TUD_BOS_WEBUSB_DESCRIPTOR(_vendor_code, _ipage) \ + TUD_BOS_PLATFORM_DESCRIPTOR(TUD_BOS_WEBUSB_UUID, U16_TO_U8S_LE(0x0100), _vendor_code, _ipage) + +#define TUD_BOS_WEBUSB_UUID \ + 0x38, 0xB6, 0x08, 0x34, 0xA9, 0x09, 0xA0, 0x47, \ + 0x8B, 0xFD, 0xA0, 0x76, 0x88, 0x15, 0xB6, 0x65 + +//------------- Microsoft OS 2.0 Platform -------------// +#define TUD_BOS_MICROSOFT_OS_DESC_LEN 28 + +// Total Length of descriptor set, vendor code +#define TUD_BOS_MS_OS_20_DESCRIPTOR(_desc_set_len, _vendor_code) \ + TUD_BOS_PLATFORM_DESCRIPTOR(TUD_BOS_MS_OS_20_UUID, U32_TO_U8S_LE(0x06030000), U16_TO_U8S_LE(_desc_set_len), _vendor_code, 0) + +#define TUD_BOS_MS_OS_20_UUID \ + 0xDF, 0x60, 0xDD, 0xD8, 0x89, 0x45, 0xC7, 0x4C, \ + 0x9C, 0xD2, 0x65, 0x9D, 0x9E, 0x64, 0x8A, 0x9F + +//--------------------------------------------------------------------+ +// Configuration Descriptor Templates +//--------------------------------------------------------------------+ + +#define TUD_CONFIG_DESC_LEN (9) + +// Config number, interface count, string index, total length, attribute, power in mA +#define TUD_CONFIG_DESCRIPTOR(config_num, _itfcount, _stridx, _total_len, _attribute, _power_ma) \ + 9, TUSB_DESC_CONFIGURATION, U16_TO_U8S_LE(_total_len), _itfcount, config_num, _stridx, TU_BIT(7) | _attribute, (_power_ma)/2 + +//--------------------------------------------------------------------+ +// CDC Descriptor Templates +//--------------------------------------------------------------------+ + +// Length of template descriptor: 66 bytes +#define TUD_CDC_DESC_LEN (8+9+5+5+4+5+7+9+7+7) + +// CDC Descriptor Template +// Interface number, string index, EP notification address and size, EP data address (out, in) and size. +#define TUD_CDC_DESCRIPTOR(_itfnum, _stridx, _ep_notif, _ep_notif_size, _epout, _epin, _epsize) \ + /* Interface Associate */\ + 8, TUSB_DESC_INTERFACE_ASSOCIATION, _itfnum, 2, TUSB_CLASS_CDC, CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL, CDC_COMM_PROTOCOL_NONE, 0,\ + /* CDC Control Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 1, TUSB_CLASS_CDC, CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL, CDC_COMM_PROTOCOL_NONE, _stridx,\ + /* CDC Header */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_HEADER, U16_TO_U8S_LE(0x0120),\ + /* CDC Call */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_CALL_MANAGEMENT, 0, (uint8_t)((_itfnum) + 1),\ + /* CDC ACM: support line request */\ + 4, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT, 2,\ + /* CDC Union */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_UNION, _itfnum, (uint8_t)((_itfnum) + 1),\ + /* Endpoint Notification */\ + 7, TUSB_DESC_ENDPOINT, _ep_notif, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_ep_notif_size), 16,\ + /* CDC Data Interface */\ + 9, TUSB_DESC_INTERFACE, (uint8_t)((_itfnum)+1), 0, 2, TUSB_CLASS_CDC_DATA, 0, 0, 0,\ + /* Endpoint Out */\ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0,\ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0 + +//--------------------------------------------------------------------+ +// MSC Descriptor Templates +//--------------------------------------------------------------------+ + +// Length of template descriptor: 23 bytes +#define TUD_MSC_DESC_LEN (9 + 7 + 7) + +// Interface number, string index, EP Out & EP In address, EP size +#define TUD_MSC_DESCRIPTOR(_itfnum, _stridx, _epout, _epin, _epsize) \ + /* Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 2, TUSB_CLASS_MSC, MSC_SUBCLASS_SCSI, MSC_PROTOCOL_BOT, _stridx,\ + /* Endpoint Out */\ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0,\ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0 + + +//--------------------------------------------------------------------+ +// HID Descriptor Templates +//--------------------------------------------------------------------+ + +// Length of template descriptor: 25 bytes +#define TUD_HID_DESC_LEN (9 + 9 + 7) + +// HID Input only descriptor +// Interface number, string index, protocol, report descriptor len, EP In address, size & polling interval +#define TUD_HID_DESCRIPTOR(_itfnum, _stridx, _boot_protocol, _report_desc_len, _epin, _epsize, _ep_interval) \ + /* Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 1, TUSB_CLASS_HID, (uint8_t)((_boot_protocol) ? (uint8_t)HID_SUBCLASS_BOOT : 0), _boot_protocol, _stridx,\ + /* HID descriptor */\ + 9, HID_DESC_TYPE_HID, U16_TO_U8S_LE(0x0111), 0, 1, HID_DESC_TYPE_REPORT, U16_TO_U8S_LE(_report_desc_len),\ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_epsize), _ep_interval + +// Length of template descriptor: 32 bytes +#define TUD_HID_INOUT_DESC_LEN (9 + 9 + 7 + 7) + +// HID Input & Output descriptor +// Interface number, string index, protocol, report descriptor len, EP OUT & IN address, size & polling interval +#define TUD_HID_INOUT_DESCRIPTOR(_itfnum, _stridx, _boot_protocol, _report_desc_len, _epout, _epin, _epsize, _ep_interval) \ + /* Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 2, TUSB_CLASS_HID, (uint8_t)((_boot_protocol) ? (uint8_t)HID_SUBCLASS_BOOT : 0), _boot_protocol, _stridx,\ + /* HID descriptor */\ + 9, HID_DESC_TYPE_HID, U16_TO_U8S_LE(0x0111), 0, 1, HID_DESC_TYPE_REPORT, U16_TO_U8S_LE(_report_desc_len),\ + /* Endpoint Out */\ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_epsize), _ep_interval, \ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_epsize), _ep_interval + +//--------------------------------------------------------------------+ +// MIDI Descriptor Templates +// Note: MIDI v1.0 is based on Audio v1.0 +//--------------------------------------------------------------------+ + +#define TUD_MIDI_DESC_HEAD_LEN (9 + 9 + 9 + 7) +#define TUD_MIDI_DESC_HEAD(_itfnum, _stridx, _numcables) \ + /* Audio Control (AC) Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 0, TUSB_CLASS_AUDIO, AUDIO_SUBCLASS_CONTROL, AUDIO_FUNC_PROTOCOL_CODE_UNDEF, _stridx,\ + /* AC Header */\ + 9, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_HEADER, U16_TO_U8S_LE(0x0100), U16_TO_U8S_LE(0x0009), 1, (uint8_t)((_itfnum) + 1),\ + /* MIDI Streaming (MS) Interface */\ + 9, TUSB_DESC_INTERFACE, (uint8_t)((_itfnum) + 1), 0, 2, TUSB_CLASS_AUDIO, AUDIO_SUBCLASS_MIDI_STREAMING, AUDIO_FUNC_PROTOCOL_CODE_UNDEF, 0,\ + /* MS Header */\ + 7, TUSB_DESC_CS_INTERFACE, MIDI_CS_INTERFACE_HEADER, U16_TO_U8S_LE(0x0100), U16_TO_U8S_LE(7 + (_numcables) * TUD_MIDI_DESC_JACK_LEN) + +#define TUD_MIDI_JACKID_IN_EMB(_cablenum) \ + (uint8_t)(((_cablenum) - 1) * 4 + 1) + +#define TUD_MIDI_JACKID_IN_EXT(_cablenum) \ + (uint8_t)(((_cablenum) - 1) * 4 + 2) + +#define TUD_MIDI_JACKID_OUT_EMB(_cablenum) \ + (uint8_t)(((_cablenum) - 1) * 4 + 3) + +#define TUD_MIDI_JACKID_OUT_EXT(_cablenum) \ + (uint8_t)(((_cablenum) - 1) * 4 + 4) + +#define TUD_MIDI_DESC_JACK_LEN (6 + 6 + 9 + 9) +#define TUD_MIDI_DESC_JACK(_cablenum) \ + /* MS In Jack (Embedded) */\ + 6, TUSB_DESC_CS_INTERFACE, MIDI_CS_INTERFACE_IN_JACK, MIDI_JACK_EMBEDDED, TUD_MIDI_JACKID_IN_EMB(_cablenum), 0,\ + /* MS In Jack (External) */\ + 6, TUSB_DESC_CS_INTERFACE, MIDI_CS_INTERFACE_IN_JACK, MIDI_JACK_EXTERNAL, TUD_MIDI_JACKID_IN_EXT(_cablenum), 0,\ + /* MS Out Jack (Embedded), connected to In Jack External */\ + 9, TUSB_DESC_CS_INTERFACE, MIDI_CS_INTERFACE_OUT_JACK, MIDI_JACK_EMBEDDED, TUD_MIDI_JACKID_OUT_EMB(_cablenum), 1, TUD_MIDI_JACKID_IN_EXT(_cablenum), 1, 0,\ + /* MS Out Jack (External), connected to In Jack Embedded */\ + 9, TUSB_DESC_CS_INTERFACE, MIDI_CS_INTERFACE_OUT_JACK, MIDI_JACK_EXTERNAL, TUD_MIDI_JACKID_OUT_EXT(_cablenum), 1, TUD_MIDI_JACKID_IN_EMB(_cablenum), 1, 0 + +#define TUD_MIDI_DESC_EP_LEN(_numcables) (9 + 4 + (_numcables)) +#define TUD_MIDI_DESC_EP(_epout, _epsize, _numcables) \ + /* Endpoint: Note Audio v1.0's endpoint has 9 bytes instead of 7 */\ + 9, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0, 0, 0, \ + /* MS Endpoint (connected to embedded jack) */\ + (uint8_t)(4 + (_numcables)), TUSB_DESC_CS_ENDPOINT, MIDI_CS_ENDPOINT_GENERAL, _numcables + +// Length of template descriptor (88 bytes) +#define TUD_MIDI_DESC_LEN (TUD_MIDI_DESC_HEAD_LEN + TUD_MIDI_DESC_JACK_LEN + TUD_MIDI_DESC_EP_LEN(1) * 2) + +// MIDI simple descriptor +// - 1 Embedded Jack In connected to 1 External Jack Out +// - 1 Embedded Jack out connected to 1 External Jack In +#define TUD_MIDI_DESCRIPTOR(_itfnum, _stridx, _epout, _epin, _epsize) \ + TUD_MIDI_DESC_HEAD(_itfnum, _stridx, 1),\ + TUD_MIDI_DESC_JACK(1),\ + TUD_MIDI_DESC_EP(_epout, _epsize, 1),\ + TUD_MIDI_JACKID_IN_EMB(1),\ + TUD_MIDI_DESC_EP(_epin, _epsize, 1),\ + TUD_MIDI_JACKID_OUT_EMB(1) + +//--------------------------------------------------------------------+ +// Audio v2.0 Descriptor Templates +//--------------------------------------------------------------------+ + +/* Standard Interface Association Descriptor (IAD) */ +#define TUD_AUDIO_DESC_IAD_LEN 8 +#define TUD_AUDIO_DESC_IAD(_firstitfs, _nitfs, _stridx) \ + TUD_AUDIO_DESC_IAD_LEN, TUSB_DESC_INTERFACE_ASSOCIATION, _firstitfs, _nitfs, TUSB_CLASS_AUDIO, AUDIO_FUNCTION_SUBCLASS_UNDEFINED, AUDIO_FUNC_PROTOCOL_CODE_V2, _stridx + +/* Standard AC Interface Descriptor(4.7.1) */ +#define TUD_AUDIO_DESC_STD_AC_LEN 9 +#define TUD_AUDIO_DESC_STD_AC(_itfnum, _nEPs, _stridx) /* _nEPs is 0 or 1 */\ + TUD_AUDIO_DESC_STD_AC_LEN, TUSB_DESC_INTERFACE, _itfnum, /* fixed to zero */ 0x00, _nEPs, TUSB_CLASS_AUDIO, AUDIO_SUBCLASS_CONTROL, AUDIO_INT_PROTOCOL_CODE_V2, _stridx + +/* Class-Specific AC Interface Header Descriptor(4.7.2) */ +#define TUD_AUDIO_DESC_CS_AC_LEN 9 +#define TUD_AUDIO_DESC_CS_AC(_bcdADC, _category, _totallen, _ctrl) /* _bcdADC : Audio Device Class Specification Release Number in Binary-Coded Decimal, _category : see audio_function_t, _totallen : Total number of bytes returned for the class-specific AudioControl interface i.e. Clock Source, Unit and Terminal descriptors - Do not include TUD_AUDIO_DESC_CS_AC_LEN, we already do this here*/ \ + TUD_AUDIO_DESC_CS_AC_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_HEADER, U16_TO_U8S_LE(_bcdADC), _category, U16_TO_U8S_LE(_totallen + TUD_AUDIO_DESC_CS_AC_LEN), _ctrl + +/* Clock Source Descriptor(4.7.2.1) */ +#define TUD_AUDIO_DESC_CLK_SRC_LEN 8 +#define TUD_AUDIO_DESC_CLK_SRC(_clkid, _attr, _ctrl, _assocTerm, _stridx) \ + TUD_AUDIO_DESC_CLK_SRC_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_CLOCK_SOURCE, _clkid, _attr, _ctrl, _assocTerm, _stridx + +/* Input Terminal Descriptor(4.7.2.4) */ +#define TUD_AUDIO_DESC_INPUT_TERM_LEN 17 +#define TUD_AUDIO_DESC_INPUT_TERM(_termid, _termtype, _assocTerm, _clkid, _nchannelslogical, _channelcfg, _idxchannelnames, _ctrl, _stridx) \ + TUD_AUDIO_DESC_INPUT_TERM_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_INPUT_TERMINAL, _termid, U16_TO_U8S_LE(_termtype), _assocTerm, _clkid, _nchannelslogical, U32_TO_U8S_LE(_channelcfg), _idxchannelnames, U16_TO_U8S_LE(_ctrl), _stridx + +/* Output Terminal Descriptor(4.7.2.5) */ +#define TUD_AUDIO_DESC_OUTPUT_TERM_LEN 12 +#define TUD_AUDIO_DESC_OUTPUT_TERM(_termid, _termtype, _assocTerm, _srcid, _clkid, _ctrl, _stridx) \ + TUD_AUDIO_DESC_OUTPUT_TERM_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_OUTPUT_TERMINAL, _termid, U16_TO_U8S_LE(_termtype), _assocTerm, _srcid, _clkid, U16_TO_U8S_LE(_ctrl), _stridx + +/* Feature Unit Descriptor(4.7.2.8) */ +// 1 - Channel +#define TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN 6+(1+1)*4 +#define TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL(_unitid, _srcid, _ctrlch0master, _ctrlch1, _stridx) \ + TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_FEATURE_UNIT, _unitid, _srcid, U32_TO_U8S_LE(_ctrlch0master), U32_TO_U8S_LE(_ctrlch1), _stridx + +// 2 - Channels +#define TUD_AUDIO_DESC_FEATURE_UNIT_TWO_CHANNEL_LEN (6+(2+1)*4) +#define TUD_AUDIO_DESC_FEATURE_UNIT_TWO_CHANNEL(_unitid, _srcid, _ctrlch0master, _ctrlch1, _ctrlch2, _stridx) \ + TUD_AUDIO_DESC_FEATURE_UNIT_TWO_CHANNEL_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_FEATURE_UNIT, _unitid, _srcid, U32_TO_U8S_LE(_ctrlch0master), U32_TO_U8S_LE(_ctrlch1), U32_TO_U8S_LE(_ctrlch2), _stridx +// 4 - Channels +#define TUD_AUDIO_DESC_FEATURE_UNIT_FOUR_CHANNEL_LEN (6+(4+1)*4) +#define TUD_AUDIO_DESC_FEATURE_UNIT_FOUR_CHANNEL(_unitid, _srcid, _ctrlch0master, _ctrlch1, _ctrlch2, _ctrlch3, _ctrlch4, _stridx) \ + TUD_AUDIO_DESC_FEATURE_UNIT_FOUR_CHANNEL_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AC_INTERFACE_FEATURE_UNIT, _unitid, _srcid, U32_TO_U8S_LE(_ctrlch0master), U32_TO_U8S_LE(_ctrlch1), U32_TO_U8S_LE(_ctrlch2), U32_TO_U8S_LE(_ctrlch3), U32_TO_U8S_LE(_ctrlch4), _stridx + +// For more channels, add definitions here + +/* Standard AS Interface Descriptor(4.9.1) */ +#define TUD_AUDIO_DESC_STD_AS_INT_LEN 9 +#define TUD_AUDIO_DESC_STD_AS_INT(_itfnum, _altset, _nEPs, _stridx) \ + TUD_AUDIO_DESC_STD_AS_INT_LEN, TUSB_DESC_INTERFACE, _itfnum, _altset, _nEPs, TUSB_CLASS_AUDIO, AUDIO_SUBCLASS_STREAMING, AUDIO_INT_PROTOCOL_CODE_V2, _stridx + +/* Class-Specific AS Interface Descriptor(4.9.2) */ +#define TUD_AUDIO_DESC_CS_AS_INT_LEN 16 +#define TUD_AUDIO_DESC_CS_AS_INT(_termid, _ctrl, _formattype, _formats, _nchannelsphysical, _channelcfg, _stridx) \ + TUD_AUDIO_DESC_CS_AS_INT_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AS_INTERFACE_AS_GENERAL, _termid, _ctrl, _formattype, U32_TO_U8S_LE(_formats), _nchannelsphysical, U32_TO_U8S_LE(_channelcfg), _stridx + +/* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */ +#define TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN 6 +#define TUD_AUDIO_DESC_TYPE_I_FORMAT(_subslotsize, _bitresolution) /* _subslotsize is number of bytes per sample (i.e. subslot) and can be 1,2,3, or 4 */\ + TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN, TUSB_DESC_CS_INTERFACE, AUDIO_CS_AS_INTERFACE_FORMAT_TYPE, AUDIO_FORMAT_TYPE_I, _subslotsize, _bitresolution + +/* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */ +#define TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN 7 +#define TUD_AUDIO_DESC_STD_AS_ISO_EP(_ep, _attr, _maxEPsize, _interval) \ + TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN, TUSB_DESC_ENDPOINT, _ep, _attr, U16_TO_U8S_LE(_maxEPsize), _interval + +/* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */ +#define TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN 8 +#define TUD_AUDIO_DESC_CS_AS_ISO_EP(_attr, _ctrl, _lockdelayunit, _lockdelay) \ + TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN, TUSB_DESC_CS_ENDPOINT, AUDIO_CS_EP_SUBTYPE_GENERAL, _attr, _ctrl, _lockdelayunit, U16_TO_U8S_LE(_lockdelay) + +/* Standard AS Isochronous Feedback Endpoint Descriptor(4.10.2.1) */ +#define TUD_AUDIO_DESC_STD_AS_ISO_FB_EP_LEN 7 +#define TUD_AUDIO_DESC_STD_AS_ISO_FB_EP(_ep, _interval) \ + TUD_AUDIO_DESC_STD_AS_ISO_FB_EP_LEN, TUSB_DESC_ENDPOINT, _ep, (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_NO_SYNC | TUSB_ISO_EP_ATT_EXPLICIT_FB), U16_TO_U8S_LE(4), _interval + +// AUDIO simple descriptor (UAC2) for 1 microphone input +// - 1 Input Terminal, 1 Feature Unit (Mute and Volume Control), 1 Output Terminal, 1 Clock Source + +#define TUD_AUDIO_MIC_ONE_CH_DESC_LEN (TUD_AUDIO_DESC_IAD_LEN\ + + TUD_AUDIO_DESC_STD_AC_LEN\ + + TUD_AUDIO_DESC_CS_AC_LEN\ + + TUD_AUDIO_DESC_CLK_SRC_LEN\ + + TUD_AUDIO_DESC_INPUT_TERM_LEN\ + + TUD_AUDIO_DESC_OUTPUT_TERM_LEN\ + + TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN\ + + TUD_AUDIO_DESC_STD_AS_INT_LEN\ + + TUD_AUDIO_DESC_STD_AS_INT_LEN\ + + TUD_AUDIO_DESC_CS_AS_INT_LEN\ + + TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN\ + + TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN\ + + TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN) + +#define TUD_AUDIO_MIC_ONE_CH_DESC_N_AS_INT 1 // Number of AS interfaces + +#define TUD_AUDIO_MIC_ONE_CH_DESCRIPTOR(_itfnum, _stridx, _nBytesPerSample, _nBitsUsedPerSample, _epin, _epsize) \ + /* Standard Interface Association Descriptor (IAD) */\ + TUD_AUDIO_DESC_IAD(/*_firstitfs*/ _itfnum, /*_nitfs*/ 0x02, /*_stridx*/ 0x00),\ + /* Standard AC Interface Descriptor(4.7.1) */\ + TUD_AUDIO_DESC_STD_AC(/*_itfnum*/ _itfnum, /*_nEPs*/ 0x00, /*_stridx*/ _stridx),\ + /* Class-Specific AC Interface Header Descriptor(4.7.2) */\ + TUD_AUDIO_DESC_CS_AC(/*_bcdADC*/ 0x0200, /*_category*/ AUDIO_FUNC_MICROPHONE, /*_totallen*/ TUD_AUDIO_DESC_CLK_SRC_LEN+TUD_AUDIO_DESC_INPUT_TERM_LEN+TUD_AUDIO_DESC_OUTPUT_TERM_LEN+TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN, /*_ctrl*/ AUDIO_CS_AS_INTERFACE_CTRL_LATENCY_POS),\ + /* Clock Source Descriptor(4.7.2.1) */\ + TUD_AUDIO_DESC_CLK_SRC(/*_clkid*/ 0x04, /*_attr*/ AUDIO_CLOCK_SOURCE_ATT_INT_FIX_CLK, /*_ctrl*/ (AUDIO_CTRL_R << AUDIO_CLOCK_SOURCE_CTRL_CLK_FRQ_POS), /*_assocTerm*/ 0x01, /*_stridx*/ 0x00),\ + /* Input Terminal Descriptor(4.7.2.4) */\ + TUD_AUDIO_DESC_INPUT_TERM(/*_termid*/ 0x01, /*_termtype*/ AUDIO_TERM_TYPE_IN_GENERIC_MIC, /*_assocTerm*/ 0x03, /*_clkid*/ 0x04, /*_nchannelslogical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_idxchannelnames*/ 0x00, /*_ctrl*/ AUDIO_CTRL_R << AUDIO_IN_TERM_CTRL_CONNECTOR_POS, /*_stridx*/ 0x00),\ + /* Output Terminal Descriptor(4.7.2.5) */\ + TUD_AUDIO_DESC_OUTPUT_TERM(/*_termid*/ 0x03, /*_termtype*/ AUDIO_TERM_TYPE_USB_STREAMING, /*_assocTerm*/ 0x01, /*_srcid*/ 0x02, /*_clkid*/ 0x04, /*_ctrl*/ 0x0000, /*_stridx*/ 0x00),\ + /* Feature Unit Descriptor(4.7.2.8) */\ + TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL(/*_unitid*/ 0x02, /*_srcid*/ 0x01, /*_ctrlch0master*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_ctrlch1*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_stridx*/ 0x00),\ + /* Standard AS Interface Descriptor(4.9.1) */\ + /* Interface 1, Alternate 0 - default alternate setting with 0 bandwidth */\ + TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum)+1), /*_altset*/ 0x00, /*_nEPs*/ 0x00, /*_stridx*/ 0x00),\ + /* Standard AS Interface Descriptor(4.9.1) */\ + /* Interface 1, Alternate 1 - alternate interface for data streaming */\ + TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum)+1), /*_altset*/ 0x01, /*_nEPs*/ 0x01, /*_stridx*/ 0x00),\ + /* Class-Specific AS Interface Descriptor(4.9.2) */\ + TUD_AUDIO_DESC_CS_AS_INT(/*_termid*/ 0x03, /*_ctrl*/ AUDIO_CTRL_NONE, /*_formattype*/ AUDIO_FORMAT_TYPE_I, /*_formats*/ AUDIO_DATA_FORMAT_TYPE_I_PCM, /*_nchannelsphysical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_stridx*/ 0x00),\ + /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\ + TUD_AUDIO_DESC_TYPE_I_FORMAT(_nBytesPerSample, _nBitsUsedPerSample),\ + /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\ + TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ (CFG_TUSB_RHPORT0_MODE & OPT_MODE_HIGH_SPEED) ? 0x04 : 0x01),\ + /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\ + TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000) + +// AUDIO simple descriptor (UAC2) for 4 microphone input +// - 1 Input Terminal, 1 Feature Unit (Mute and Volume Control), 1 Output Terminal, 1 Clock Source + +#define TUD_AUDIO_MIC_FOUR_CH_DESC_LEN (TUD_AUDIO_DESC_IAD_LEN\ + + TUD_AUDIO_DESC_STD_AC_LEN\ + + TUD_AUDIO_DESC_CS_AC_LEN\ + + TUD_AUDIO_DESC_CLK_SRC_LEN\ + + TUD_AUDIO_DESC_INPUT_TERM_LEN\ + + TUD_AUDIO_DESC_OUTPUT_TERM_LEN\ + + TUD_AUDIO_DESC_FEATURE_UNIT_FOUR_CHANNEL_LEN\ + + TUD_AUDIO_DESC_STD_AS_INT_LEN\ + + TUD_AUDIO_DESC_STD_AS_INT_LEN\ + + TUD_AUDIO_DESC_CS_AS_INT_LEN\ + + TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN\ + + TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN\ + + TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN) + +#define TUD_AUDIO_MIC_FOUR_CH_DESC_N_AS_INT 1 // Number of AS interfaces + +#define TUD_AUDIO_MIC_FOUR_CH_DESCRIPTOR(_itfnum, _stridx, _nBytesPerSample, _nBitsUsedPerSample, _epin, _epsize) \ + /* Standard Interface Association Descriptor (IAD) */\ + TUD_AUDIO_DESC_IAD(/*_firstitfs*/ _itfnum, /*_nitfs*/ 0x02, /*_stridx*/ 0x00),\ + /* Standard AC Interface Descriptor(4.7.1) */\ + TUD_AUDIO_DESC_STD_AC(/*_itfnum*/ _itfnum, /*_nEPs*/ 0x00, /*_stridx*/ _stridx),\ + /* Class-Specific AC Interface Header Descriptor(4.7.2) */\ + TUD_AUDIO_DESC_CS_AC(/*_bcdADC*/ 0x0200, /*_category*/ AUDIO_FUNC_MICROPHONE, /*_totallen*/ TUD_AUDIO_DESC_CLK_SRC_LEN+TUD_AUDIO_DESC_INPUT_TERM_LEN+TUD_AUDIO_DESC_OUTPUT_TERM_LEN+TUD_AUDIO_DESC_FEATURE_UNIT_FOUR_CHANNEL_LEN, /*_ctrl*/ AUDIO_CS_AS_INTERFACE_CTRL_LATENCY_POS),\ + /* Clock Source Descriptor(4.7.2.1) */\ + TUD_AUDIO_DESC_CLK_SRC(/*_clkid*/ 0x04, /*_attr*/ AUDIO_CLOCK_SOURCE_ATT_INT_FIX_CLK, /*_ctrl*/ (AUDIO_CTRL_R << AUDIO_CLOCK_SOURCE_CTRL_CLK_FRQ_POS), /*_assocTerm*/ 0x01, /*_stridx*/ 0x00),\ + /* Input Terminal Descriptor(4.7.2.4) */\ + TUD_AUDIO_DESC_INPUT_TERM(/*_termid*/ 0x01, /*_termtype*/ AUDIO_TERM_TYPE_IN_GENERIC_MIC, /*_assocTerm*/ 0x03, /*_clkid*/ 0x04, /*_nchannelslogical*/ 0x04, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_idxchannelnames*/ 0x00, /*_ctrl*/ AUDIO_CTRL_R << AUDIO_IN_TERM_CTRL_CONNECTOR_POS, /*_stridx*/ 0x00),\ + /* Output Terminal Descriptor(4.7.2.5) */\ + TUD_AUDIO_DESC_OUTPUT_TERM(/*_termid*/ 0x03, /*_termtype*/ AUDIO_TERM_TYPE_USB_STREAMING, /*_assocTerm*/ 0x01, /*_srcid*/ 0x02, /*_clkid*/ 0x04, /*_ctrl*/ 0x0000, /*_stridx*/ 0x00),\ + /* Feature Unit Descriptor(4.7.2.8) */\ + TUD_AUDIO_DESC_FEATURE_UNIT_FOUR_CHANNEL(/*_unitid*/ 0x02, /*_srcid*/ 0x01, /*_ctrlch0master*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_ctrlch1*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_ctrlch2*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_ctrlch3*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_ctrlch4*/ AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS, /*_stridx*/ 0x00),\ + /* Standard AS Interface Descriptor(4.9.1) */\ + /* Interface 1, Alternate 0 - default alternate setting with 0 bandwidth */\ + TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum)+1), /*_altset*/ 0x00, /*_nEPs*/ 0x00, /*_stridx*/ 0x00),\ + /* Standard AS Interface Descriptor(4.9.1) */\ + /* Interface 1, Alternate 1 - alternate interface for data streaming */\ + TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum)+1), /*_altset*/ 0x01, /*_nEPs*/ 0x01, /*_stridx*/ 0x00),\ + /* Class-Specific AS Interface Descriptor(4.9.2) */\ + TUD_AUDIO_DESC_CS_AS_INT(/*_termid*/ 0x03, /*_ctrl*/ AUDIO_CTRL_NONE, /*_formattype*/ AUDIO_FORMAT_TYPE_I, /*_formats*/ AUDIO_DATA_FORMAT_TYPE_I_PCM, /*_nchannelsphysical*/ 0x04, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_stridx*/ 0x00),\ + /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\ + TUD_AUDIO_DESC_TYPE_I_FORMAT(_nBytesPerSample, _nBitsUsedPerSample),\ + /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\ + TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ (CFG_TUSB_RHPORT0_MODE & OPT_MODE_HIGH_SPEED) ? 0x04 : 0x01),\ + /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\ + TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000) + +// AUDIO simple descriptor (UAC2) for mono speaker +// - 1 Input Terminal, 2 Feature Unit (Mute and Volume Control), 3 Output Terminal, 4 Clock Source + +#define TUD_AUDIO_SPEAKER_MONO_FB_DESC_LEN (TUD_AUDIO_DESC_IAD_LEN\ + + TUD_AUDIO_DESC_STD_AC_LEN\ + + TUD_AUDIO_DESC_CS_AC_LEN\ + + TUD_AUDIO_DESC_CLK_SRC_LEN\ + + TUD_AUDIO_DESC_INPUT_TERM_LEN\ + + TUD_AUDIO_DESC_OUTPUT_TERM_LEN\ + + TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN\ + + TUD_AUDIO_DESC_STD_AS_INT_LEN\ + + TUD_AUDIO_DESC_STD_AS_INT_LEN\ + + TUD_AUDIO_DESC_CS_AS_INT_LEN\ + + TUD_AUDIO_DESC_TYPE_I_FORMAT_LEN\ + + TUD_AUDIO_DESC_STD_AS_ISO_EP_LEN\ + + TUD_AUDIO_DESC_CS_AS_ISO_EP_LEN\ + + TUD_AUDIO_DESC_STD_AS_ISO_FB_EP_LEN) + +#define TUD_AUDIO_SPEAKER_MONO_FB_DESCRIPTOR(_itfnum, _stridx, _nBytesPerSample, _nBitsUsedPerSample, _epout, _epsize, _epfb) \ + /* Standard Interface Association Descriptor (IAD) */\ + TUD_AUDIO_DESC_IAD(/*_firstitfs*/ _itfnum, /*_nitfs*/ 0x02, /*_stridx*/ 0x00),\ + /* Standard AC Interface Descriptor(4.7.1) */\ + TUD_AUDIO_DESC_STD_AC(/*_itfnum*/ _itfnum, /*_nEPs*/ 0x00, /*_stridx*/ _stridx),\ + /* Class-Specific AC Interface Header Descriptor(4.7.2) */\ + TUD_AUDIO_DESC_CS_AC(/*_bcdADC*/ 0x0200, /*_category*/ AUDIO_FUNC_DESKTOP_SPEAKER, /*_totallen*/ TUD_AUDIO_DESC_CLK_SRC_LEN+TUD_AUDIO_DESC_INPUT_TERM_LEN+TUD_AUDIO_DESC_OUTPUT_TERM_LEN+TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL_LEN, /*_ctrl*/ AUDIO_CS_AS_INTERFACE_CTRL_LATENCY_POS),\ + /* Clock Source Descriptor(4.7.2.1) */\ + TUD_AUDIO_DESC_CLK_SRC(/*_clkid*/ 0x04, /*_attr*/ AUDIO_CLOCK_SOURCE_ATT_INT_FIX_CLK, /*_ctrl*/ (AUDIO_CTRL_R << AUDIO_CLOCK_SOURCE_CTRL_CLK_FRQ_POS), /*_assocTerm*/ 0x01, /*_stridx*/ 0x00),\ + /* Input Terminal Descriptor(4.7.2.4) */\ + TUD_AUDIO_DESC_INPUT_TERM(/*_termid*/ 0x01, /*_termtype*/ AUDIO_TERM_TYPE_USB_STREAMING, /*_assocTerm*/ 0x00, /*_clkid*/ 0x04, /*_nchannelslogical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_idxchannelnames*/ 0x00, /*_ctrl*/ 0 * (AUDIO_CTRL_R << AUDIO_IN_TERM_CTRL_CONNECTOR_POS), /*_stridx*/ 0x00),\ + /* Output Terminal Descriptor(4.7.2.5) */\ + TUD_AUDIO_DESC_OUTPUT_TERM(/*_termid*/ 0x03, /*_termtype*/ AUDIO_TERM_TYPE_OUT_DESKTOP_SPEAKER, /*_assocTerm*/ 0x01, /*_srcid*/ 0x02, /*_clkid*/ 0x04, /*_ctrl*/ 0x0000, /*_stridx*/ 0x00),\ + /* Feature Unit Descriptor(4.7.2.8) */\ + TUD_AUDIO_DESC_FEATURE_UNIT_ONE_CHANNEL(/*_unitid*/ 0x02, /*_srcid*/ 0x01, /*_ctrlch0master*/ 0 * (AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS), /*_ctrlch1*/ 0 * (AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_MUTE_POS | AUDIO_CTRL_RW << AUDIO_FEATURE_UNIT_CTRL_VOLUME_POS), /*_stridx*/ 0x00),\ + /* Standard AS Interface Descriptor(4.9.1) */\ + /* Interface 1, Alternate 0 - default alternate setting with 0 bandwidth */\ + TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum) + 1), /*_altset*/ 0x00, /*_nEPs*/ 0x00, /*_stridx*/ 0x00),\ + /* Standard AS Interface Descriptor(4.9.1) */\ + /* Interface 1, Alternate 1 - alternate interface for data streaming */\ + TUD_AUDIO_DESC_STD_AS_INT(/*_itfnum*/ (uint8_t)((_itfnum) + 1), /*_altset*/ 0x01, /*_nEPs*/ 0x02, /*_stridx*/ 0x00),\ + /* Class-Specific AS Interface Descriptor(4.9.2) */\ + TUD_AUDIO_DESC_CS_AS_INT(/*_termid*/ 0x01, /*_ctrl*/ AUDIO_CTRL_NONE, /*_formattype*/ AUDIO_FORMAT_TYPE_I, /*_formats*/ AUDIO_DATA_FORMAT_TYPE_I_PCM, /*_nchannelsphysical*/ 0x01, /*_channelcfg*/ AUDIO_CHANNEL_CONFIG_NON_PREDEFINED, /*_stridx*/ 0x00),\ + /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\ + TUD_AUDIO_DESC_TYPE_I_FORMAT(_nBytesPerSample, _nBitsUsedPerSample),\ + /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\ + TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epout, /*_attr*/ (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ (CFG_TUSB_RHPORT0_MODE & OPT_MODE_HIGH_SPEED) ? 0x04 : 0x01),\ + /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\ + TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000),\ + /* Standard AS Isochronous Feedback Endpoint Descriptor(4.10.2.1) */\ + TUD_AUDIO_DESC_STD_AS_ISO_FB_EP(/*_ep*/ _epfb, /*_interval*/ 1)\ + +// Calculate wMaxPacketSize of Endpoints +#define TUD_AUDIO_EP_SIZE(_maxFrequency, _nBytesPerSample, _nChannels) \ + ((((_maxFrequency + ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_HIGH_SPEED) ? 7999 : 999)) / ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_HIGH_SPEED) ? 8000 : 1000)) + 1) * _nBytesPerSample * _nChannels) + + +//--------------------------------------------------------------------+ +// USBTMC/USB488 Descriptor Templates +//--------------------------------------------------------------------+ + +#define TUD_USBTMC_APP_CLASS (TUSB_CLASS_APPLICATION_SPECIFIC) +#define TUD_USBTMC_APP_SUBCLASS 0x03u + +#define TUD_USBTMC_PROTOCOL_STD 0x00u +#define TUD_USBTMC_PROTOCOL_USB488 0x01u + +// Interface number, number of endpoints, EP string index, USB_TMC_PROTOCOL*, bulk-out endpoint ID, +// bulk-in endpoint ID +#define TUD_USBTMC_IF_DESCRIPTOR(_itfnum, _bNumEndpoints, _stridx, _itfProtocol) \ + /* Interface */ \ + 0x09, TUSB_DESC_INTERFACE, _itfnum, 0x00, _bNumEndpoints, TUD_USBTMC_APP_CLASS, TUD_USBTMC_APP_SUBCLASS, _itfProtocol, _stridx + +#define TUD_USBTMC_IF_DESCRIPTOR_LEN 9u + +#define TUD_USBTMC_BULK_DESCRIPTORS(_epout, _epin, _bulk_epsize) \ + /* Endpoint Out */ \ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_bulk_epsize), 0u, \ + /* Endpoint In */ \ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_bulk_epsize), 0u + +#define TUD_USBTMC_BULK_DESCRIPTORS_LEN (7u+7u) + +/* optional interrupt endpoint */ \ +// _int_pollingInterval : for LS/FS, expressed in frames (1ms each). 16 may be a good number? +#define TUD_USBTMC_INT_DESCRIPTOR(_ep_interrupt, _ep_interrupt_size, _int_pollingInterval ) \ + 7, TUSB_DESC_ENDPOINT, _ep_interrupt, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_ep_interrupt_size), 0x16 + +#define TUD_USBTMC_INT_DESCRIPTOR_LEN (7u) + +//--------------------------------------------------------------------+ +// Vendor Descriptor Templates +//--------------------------------------------------------------------+ + +#define TUD_VENDOR_DESC_LEN (9+7+7) + +// Interface number, string index, EP Out & IN address, EP size +#define TUD_VENDOR_DESCRIPTOR(_itfnum, _stridx, _epout, _epin, _epsize) \ + /* Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 2, TUSB_CLASS_VENDOR_SPECIFIC, 0x00, 0x00, _stridx,\ + /* Endpoint Out */\ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0,\ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0 + +//--------------------------------------------------------------------+ +// DFU Runtime Descriptor Templates +//--------------------------------------------------------------------+ + +#define TUD_DFU_APP_CLASS (TUSB_CLASS_APPLICATION_SPECIFIC) +#define TUD_DFU_APP_SUBCLASS (APP_SUBCLASS_DFU_RUNTIME) + +// Length of template descriptr: 18 bytes +#define TUD_DFU_RT_DESC_LEN (9 + 9) + +// DFU runtime descriptor +// Interface number, string index, attributes, detach timeout, transfer size +#define TUD_DFU_RT_DESCRIPTOR(_itfnum, _stridx, _attr, _timeout, _xfer_size) \ + /* Interface */ \ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 0, TUD_DFU_APP_CLASS, TUD_DFU_APP_SUBCLASS, DFU_PROTOCOL_RT, _stridx, \ + /* Function */ \ + 9, DFU_DESC_FUNCTIONAL, _attr, U16_TO_U8S_LE(_timeout), U16_TO_U8S_LE(_xfer_size), U16_TO_U8S_LE(0x0101) + +//--------------------------------------------------------------------+ +// DFU Descriptor Templates +//--------------------------------------------------------------------+ + +// Length of template descriptor: 9 bytes + number of alternatives * 9 +#define TUD_DFU_DESC_LEN(_alt_count) (9 + (_alt_count) * 9) + +// Interface number, Alternate count, starting string index, attributes, detach timeout, transfer size +// Note: Alternate count must be numberic or macro, string index is increased by one for each Alt interface +#define TUD_DFU_DESCRIPTOR(_itfnum, _alt_count, _stridx, _attr, _timeout, _xfer_size) \ + TU_XSTRCAT(_TUD_DFU_ALT_,_alt_count)(_itfnum, 0, _stridx), \ + /* Function */ \ + 9, DFU_DESC_FUNCTIONAL, _attr, U16_TO_U8S_LE(_timeout), U16_TO_U8S_LE(_xfer_size), U16_TO_U8S_LE(0x0101) + +#define _TUD_DFU_ALT(_itfnum, _alt, _stridx) \ + /* Interface */ \ + 9, TUSB_DESC_INTERFACE, _itfnum, _alt, 0, TUD_DFU_APP_CLASS, TUD_DFU_APP_SUBCLASS, DFU_PROTOCOL_DFU, _stridx + +#define _TUD_DFU_ALT_1(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx) + +#define _TUD_DFU_ALT_2(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx), \ + _TUD_DFU_ALT_1(_itfnum, _alt_count+1, _stridx+1) + +#define _TUD_DFU_ALT_3(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx), \ + _TUD_DFU_ALT_2(_itfnum, _alt_count+1, _stridx+1) + +#define _TUD_DFU_ALT_4(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx), \ + _TUD_DFU_ALT_3(_itfnum, _alt_count+1, _stridx+1) + +#define _TUD_DFU_ALT_5(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx), \ + _TUD_DFU_ALT_4(_itfnum, _alt_count+1, _stridx+1) + +#define _TUD_DFU_ALT_6(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx), \ + _TUD_DFU_ALT_5(_itfnum, _alt_count+1, _stridx+1) + +#define _TUD_DFU_ALT_7(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx), \ + _TUD_DFU_ALT_6(_itfnum, _alt_count+1, _stridx+1) + +#define _TUD_DFU_ALT_8(_itfnum, _alt_count, _stridx) \ + _TUD_DFU_ALT(_itfnum, _alt_count, _stridx), \ + _TUD_DFU_ALT_7(_itfnum, _alt_count+1, _stridx+1) + +//--------------------------------------------------------------------+ +// CDC-ECM Descriptor Templates +//--------------------------------------------------------------------+ + +// Length of template descriptor: 71 bytes +#define TUD_CDC_ECM_DESC_LEN (8+9+5+5+13+7+9+9+7+7) + +// CDC-ECM Descriptor Template +// Interface number, description string index, MAC address string index, EP notification address and size, EP data address (out, in), and size, max segment size. +#define TUD_CDC_ECM_DESCRIPTOR(_itfnum, _desc_stridx, _mac_stridx, _ep_notif, _ep_notif_size, _epout, _epin, _epsize, _maxsegmentsize) \ + /* Interface Association */\ + 8, TUSB_DESC_INTERFACE_ASSOCIATION, _itfnum, 2, TUSB_CLASS_CDC, CDC_COMM_SUBCLASS_ETHERNET_CONTROL_MODEL, 0, 0,\ + /* CDC Control Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 1, TUSB_CLASS_CDC, CDC_COMM_SUBCLASS_ETHERNET_CONTROL_MODEL, 0, _desc_stridx,\ + /* CDC-ECM Header */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_HEADER, U16_TO_U8S_LE(0x0120),\ + /* CDC-ECM Union */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_UNION, _itfnum, (uint8_t)((_itfnum) + 1),\ + /* CDC-ECM Functional Descriptor */\ + 13, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_ETHERNET_NETWORKING, _mac_stridx, 0, 0, 0, 0, U16_TO_U8S_LE(_maxsegmentsize), U16_TO_U8S_LE(0), 0,\ + /* Endpoint Notification */\ + 7, TUSB_DESC_ENDPOINT, _ep_notif, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_ep_notif_size), 1,\ + /* CDC Data Interface (default inactive) */\ + 9, TUSB_DESC_INTERFACE, (uint8_t)((_itfnum)+1), 0, 0, TUSB_CLASS_CDC_DATA, 0, 0, 0,\ + /* CDC Data Interface (alternative active) */\ + 9, TUSB_DESC_INTERFACE, (uint8_t)((_itfnum)+1), 1, 2, TUSB_CLASS_CDC_DATA, 0, 0, 0,\ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0,\ + /* Endpoint Out */\ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0 + +//--------------------------------------------------------------------+ +// RNDIS Descriptor Templates +//--------------------------------------------------------------------+ + +#if 0 +/* Windows XP */ +#define TUD_RNDIS_ITF_CLASS TUSB_CLASS_CDC +#define TUD_RNDIS_ITF_SUBCLASS CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL +#define TUD_RNDIS_ITF_PROTOCOL 0xFF /* CDC_COMM_PROTOCOL_MICROSOFT_RNDIS */ +#else +/* Windows 7+ */ +#define TUD_RNDIS_ITF_CLASS TUSB_CLASS_WIRELESS_CONTROLLER +#define TUD_RNDIS_ITF_SUBCLASS 0x01 +#define TUD_RNDIS_ITF_PROTOCOL 0x03 +#endif + +// Length of template descriptor: 66 bytes +#define TUD_RNDIS_DESC_LEN (8+9+5+5+4+5+7+9+7+7) + +// RNDIS Descriptor Template +// Interface number, string index, EP notification address and size, EP data address (out, in) and size. +#define TUD_RNDIS_DESCRIPTOR(_itfnum, _stridx, _ep_notif, _ep_notif_size, _epout, _epin, _epsize) \ + /* Interface Association */\ + 8, TUSB_DESC_INTERFACE_ASSOCIATION, _itfnum, 2, TUD_RNDIS_ITF_CLASS, TUD_RNDIS_ITF_SUBCLASS, TUD_RNDIS_ITF_PROTOCOL, 0,\ + /* CDC Control Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 1, TUD_RNDIS_ITF_CLASS, TUD_RNDIS_ITF_SUBCLASS, TUD_RNDIS_ITF_PROTOCOL, _stridx,\ + /* CDC-ACM Header */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_HEADER, U16_TO_U8S_LE(0x0110),\ + /* CDC Call Management */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_CALL_MANAGEMENT, 0, (uint8_t)((_itfnum) + 1),\ + /* ACM */\ + 4, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT, 0,\ + /* CDC Union */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_UNION, _itfnum, (uint8_t)((_itfnum) + 1),\ + /* Endpoint Notification */\ + 7, TUSB_DESC_ENDPOINT, _ep_notif, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_ep_notif_size), 1,\ + /* CDC Data Interface */\ + 9, TUSB_DESC_INTERFACE, (uint8_t)((_itfnum)+1), 0, 2, TUSB_CLASS_CDC_DATA, 0, 0, 0,\ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0,\ + /* Endpoint Out */\ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0 + +//--------------------------------------------------------------------+ +// Bluetooth Radio Descriptor Templates +//--------------------------------------------------------------------+ + +#define TUD_BT_APP_CLASS (TUSB_CLASS_WIRELESS_CONTROLLER) +#define TUD_BT_APP_SUBCLASS 0x01 +#define TUD_BT_PROTOCOL_PRIMARY_CONTROLLER 0x01 +#define TUD_BT_PROTOCOL_AMP_CONTROLLER 0x02 + +#ifndef CFG_TUD_BTH_ISO_ALT_COUNT +#define CFG_TUD_BTH_ISO_ALT_COUNT 0 +#endif + +// Length of template descriptor: 38 bytes + number of ISO alternatives * 23 +#define TUD_BTH_DESC_LEN (8 + 9 + 7 + 7 + 7 + (CFG_TUD_BTH_ISO_ALT_COUNT) * (9 + 7 + 7)) + +/* Primary Interface */ +#define TUD_BTH_PRI_ITF(_itfnum, _stridx, _ep_evt, _ep_evt_size, _ep_evt_interval, _ep_in, _ep_out, _ep_size) \ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 3, TUD_BT_APP_CLASS, TUD_BT_APP_SUBCLASS, TUD_BT_PROTOCOL_PRIMARY_CONTROLLER, _stridx, \ + /* Endpoint In for events */ \ + 7, TUSB_DESC_ENDPOINT, _ep_evt, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_ep_evt_size), _ep_evt_interval, \ + /* Endpoint In for ACL data */ \ + 7, TUSB_DESC_ENDPOINT, _ep_in, TUSB_XFER_BULK, U16_TO_U8S_LE(_ep_size), 1, \ + /* Endpoint Out for ACL data */ \ + 7, TUSB_DESC_ENDPOINT, _ep_out, TUSB_XFER_BULK, U16_TO_U8S_LE(_ep_size), 1 + +#define TUD_BTH_ISO_ITF(_itfnum, _alt, _ep_in, _ep_out, _n) ,\ + /* Interface with 2 endpoints */ \ + 9, TUSB_DESC_INTERFACE, _itfnum, _alt, 2, TUD_BT_APP_CLASS, TUD_BT_APP_SUBCLASS, TUD_BT_PROTOCOL_PRIMARY_CONTROLLER, 0, \ + /* Isochronous endpoints */ \ + 7, TUSB_DESC_ENDPOINT, _ep_in, TUSB_XFER_ISOCHRONOUS, U16_TO_U8S_LE(_n), 1, \ + 7, TUSB_DESC_ENDPOINT, _ep_out, TUSB_XFER_ISOCHRONOUS, U16_TO_U8S_LE(_n), 1 + +#define _FIRST(a, ...) a +#define _REST(a, ...) __VA_ARGS__ + +#define TUD_BTH_ISO_ITF_0(_itfnum, ...) +#define TUD_BTH_ISO_ITF_1(_itfnum, _ep_in, _ep_out, ...) TUD_BTH_ISO_ITF(_itfnum, (CFG_TUD_BTH_ISO_ALT_COUNT) - 1, _ep_in, _ep_out, _FIRST(__VA_ARGS__)) +#define TUD_BTH_ISO_ITF_2(_itfnum, _ep_in, _ep_out, ...) TUD_BTH_ISO_ITF(_itfnum, (CFG_TUD_BTH_ISO_ALT_COUNT) - 2, _ep_in, _ep_out, _FIRST(__VA_ARGS__)) \ + TUD_BTH_ISO_ITF_1(_itfnum, _ep_in, _ep_out, _REST(__VA_ARGS__)) +#define TUD_BTH_ISO_ITF_3(_itfnum, _ep_in, _ep_out, ...) TUD_BTH_ISO_ITF(_itfnum, (CFG_TUD_BTH_ISO_ALT_COUNT) - 3, _ep_in, _ep_out, _FIRST(__VA_ARGS__)) \ + TUD_BTH_ISO_ITF_2(_itfnum, _ep_in, _ep_out, _REST(__VA_ARGS__)) +#define TUD_BTH_ISO_ITF_4(_itfnum, _ep_in, _ep_out, ...) TUD_BTH_ISO_ITF(_itfnum, (CFG_TUD_BTH_ISO_ALT_COUNT) - 4, _ep_in, _ep_out, _FIRST(__VA_ARGS__)) \ + TUD_BTH_ISO_ITF_3(_itfnum, _ep_in, _ep_out, _REST(__VA_ARGS__)) +#define TUD_BTH_ISO_ITF_5(_itfnum, _ep_in, _ep_out, ...) TUD_BTH_ISO_ITF(_itfnum, (CFG_TUD_BTH_ISO_ALT_COUNT) - 5, _ep_in, _ep_out, _FIRST(__VA_ARGS__)) \ + TUD_BTH_ISO_ITF_4(_itfnum, _ep_in, _ep_out, _REST(__VA_ARGS__)) +#define TUD_BTH_ISO_ITF_6(_itfnum, _ep_in, _ep_out, ...) TUD_BTH_ISO_ITF(_itfnum, (CFG_TUD_BTH_ISO_ALT_COUNT) - 6, _ep_in, _ep_out, _FIRST(__VA_ARGS__)) \ + TUD_BTH_ISO_ITF_5(_itfnum, _ep_in, _ep_out, _REST(__VA_ARGS__)) + +#define TUD_BTH_ISO_ITFS(_itfnum, _ep_in, _ep_out, ...) \ + TU_XSTRCAT(TUD_BTH_ISO_ITF_, CFG_TUD_BTH_ISO_ALT_COUNT)(_itfnum, _ep_in, _ep_out, __VA_ARGS__) + +// BT Primary controller descriptor +// Interface number, string index, attributes, event endpoint, event endpoint size, interval, data in, data out, data endpoint size, iso endpoint sizes +// TODO BTH should also use IAD like CDC for composite device +#define TUD_BTH_DESCRIPTOR(_itfnum, _stridx, _ep_evt, _ep_evt_size, _ep_evt_interval, _ep_in, _ep_out, _ep_size,...) \ + /* Interface Associate */\ + 8, TUSB_DESC_INTERFACE_ASSOCIATION, _itfnum, 2, TUD_BT_APP_CLASS, TUD_BT_APP_SUBCLASS, TUD_BT_PROTOCOL_PRIMARY_CONTROLLER, 0,\ + TUD_BTH_PRI_ITF(_itfnum, _stridx, _ep_evt, _ep_evt_size, _ep_evt_interval, _ep_in, _ep_out, _ep_size) \ + TUD_BTH_ISO_ITFS(_itfnum + 1, _ep_in + 1, _ep_out + 1, __VA_ARGS__) + +//--------------------------------------------------------------------+ +// CDC-NCM Descriptor Templates +//--------------------------------------------------------------------+ + +// Length of template descriptor +#define TUD_CDC_NCM_DESC_LEN (8+9+5+5+13+6+7+9+9+7+7) + +// CDC-ECM Descriptor Template +// Interface number, description string index, MAC address string index, EP notification address and size, EP data address (out, in), and size, max segment size. +#define TUD_CDC_NCM_DESCRIPTOR(_itfnum, _desc_stridx, _mac_stridx, _ep_notif, _ep_notif_size, _epout, _epin, _epsize, _maxsegmentsize) \ + /* Interface Association */\ + 8, TUSB_DESC_INTERFACE_ASSOCIATION, _itfnum, 2, TUSB_CLASS_CDC, CDC_COMM_SUBCLASS_NETWORK_CONTROL_MODEL, 0, 0,\ + /* CDC Control Interface */\ + 9, TUSB_DESC_INTERFACE, _itfnum, 0, 1, TUSB_CLASS_CDC, CDC_COMM_SUBCLASS_NETWORK_CONTROL_MODEL, 0, _desc_stridx,\ + /* CDC-NCM Header */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_HEADER, U16_TO_U8S_LE(0x0110),\ + /* CDC-NCM Union */\ + 5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_UNION, _itfnum, (uint8_t)((_itfnum) + 1),\ + /* CDC-NCM Functional Descriptor */\ + 13, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_ETHERNET_NETWORKING, _mac_stridx, 0, 0, 0, 0, U16_TO_U8S_LE(_maxsegmentsize), U16_TO_U8S_LE(0), 0, \ + /* CDC-NCM Functional Descriptor */\ + 6, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_NCM, U16_TO_U8S_LE(0x0100), 0, \ + /* Endpoint Notification */\ + 7, TUSB_DESC_ENDPOINT, _ep_notif, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(_ep_notif_size), 50,\ + /* CDC Data Interface (default inactive) */\ + 9, TUSB_DESC_INTERFACE, (uint8_t)((_itfnum)+1), 0, 0, TUSB_CLASS_CDC_DATA, 0, NCM_DATA_PROTOCOL_NETWORK_TRANSFER_BLOCK, 0,\ + /* CDC Data Interface (alternative active) */\ + 9, TUSB_DESC_INTERFACE, (uint8_t)((_itfnum)+1), 1, 2, TUSB_CLASS_CDC_DATA, 0, NCM_DATA_PROTOCOL_NETWORK_TRANSFER_BLOCK, 0,\ + /* Endpoint In */\ + 7, TUSB_DESC_ENDPOINT, _epin, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0,\ + /* Endpoint Out */\ + 7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0 + +#ifdef __cplusplus +} +#endif + +#endif /* _TUSB_USBD_H_ */ + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd_control.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd_control.c new file mode 100644 index 000000000..7a8244699 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd_control.c @@ -0,0 +1,233 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if TUSB_OPT_DEVICE_ENABLED + +#include "tusb.h" +#include "device/usbd_pvt.h" +#include "dcd.h" + +#if CFG_TUSB_DEBUG >= 2 +extern void usbd_driver_print_control_complete_name(usbd_control_xfer_cb_t callback); +#endif + +enum +{ + EDPT_CTRL_OUT = 0x00, + EDPT_CTRL_IN = 0x80 +}; + +typedef struct +{ + tusb_control_request_t request; + + uint8_t* buffer; + uint16_t data_len; + uint16_t total_xferred; + + usbd_control_xfer_cb_t complete_cb; +} usbd_control_xfer_t; + +static usbd_control_xfer_t _ctrl_xfer; + +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN +static uint8_t _usbd_ctrl_buf[CFG_TUD_ENDPOINT0_SIZE]; + +//--------------------------------------------------------------------+ +// Application API +//--------------------------------------------------------------------+ + +// Queue ZLP status transaction +static inline bool _status_stage_xact(uint8_t rhport, tusb_control_request_t const * request) +{ + // Opposite to endpoint in Data Phase + uint8_t const ep_addr = request->bmRequestType_bit.direction ? EDPT_CTRL_OUT : EDPT_CTRL_IN; + return usbd_edpt_xfer(rhport, ep_addr, NULL, 0); +} + +// Status phase +bool tud_control_status(uint8_t rhport, tusb_control_request_t const * request) +{ + _ctrl_xfer.request = (*request); + _ctrl_xfer.buffer = NULL; + _ctrl_xfer.total_xferred = 0; + _ctrl_xfer.data_len = 0; + + return _status_stage_xact(rhport, request); +} + +// Queue a transaction in Data Stage +// Each transaction has up to Endpoint0's max packet size. +// This function can also transfer an zero-length packet +static bool _data_stage_xact(uint8_t rhport) +{ + uint16_t const xact_len = tu_min16(_ctrl_xfer.data_len - _ctrl_xfer.total_xferred, CFG_TUD_ENDPOINT0_SIZE); + + uint8_t ep_addr = EDPT_CTRL_OUT; + + if ( _ctrl_xfer.request.bmRequestType_bit.direction == TUSB_DIR_IN ) + { + ep_addr = EDPT_CTRL_IN; + if ( xact_len ) memcpy(_usbd_ctrl_buf, _ctrl_xfer.buffer, xact_len); + } + + return usbd_edpt_xfer(rhport, ep_addr, xact_len ? _usbd_ctrl_buf : NULL, xact_len); +} + +// Transmit data to/from the control endpoint. +// If the request's wLength is zero, a status packet is sent instead. +bool tud_control_xfer(uint8_t rhport, tusb_control_request_t const * request, void* buffer, uint16_t len) +{ + _ctrl_xfer.request = (*request); + _ctrl_xfer.buffer = (uint8_t*) buffer; + _ctrl_xfer.total_xferred = 0U; + _ctrl_xfer.data_len = tu_min16(len, request->wLength); + + if (request->wLength > 0U) + { + if(_ctrl_xfer.data_len > 0U) + { + TU_ASSERT(buffer); + } + +// TU_LOG2(" Control total data length is %u bytes\r\n", _ctrl_xfer.data_len); + + // Data stage + TU_ASSERT( _data_stage_xact(rhport) ); + } + else + { + // Status stage + TU_ASSERT( _status_stage_xact(rhport, request) ); + } + + return true; +} + +//--------------------------------------------------------------------+ +// USBD API +//--------------------------------------------------------------------+ + +void usbd_control_reset(void); +void usbd_control_set_request(tusb_control_request_t const *request); +void usbd_control_set_complete_callback( usbd_control_xfer_cb_t fp ); +bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); + +void usbd_control_reset(void) +{ + tu_varclr(&_ctrl_xfer); +} + +// Set complete callback +void usbd_control_set_complete_callback( usbd_control_xfer_cb_t fp ) +{ + _ctrl_xfer.complete_cb = fp; +} + +// for dcd_set_address where DCD is responsible for status response +void usbd_control_set_request(tusb_control_request_t const *request) +{ + _ctrl_xfer.request = (*request); + _ctrl_xfer.buffer = NULL; + _ctrl_xfer.total_xferred = 0; + _ctrl_xfer.data_len = 0; +} + +// callback when a transaction complete on +// - DATA stage of control endpoint or +// - Status stage +bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) result; + + // Endpoint Address is opposite to direction bit, this is Status Stage complete event + if ( tu_edpt_dir(ep_addr) != _ctrl_xfer.request.bmRequestType_bit.direction ) + { + TU_ASSERT(0 == xferred_bytes); + + // invoke optional dcd hook if available + if (dcd_edpt0_status_complete) dcd_edpt0_status_complete(rhport, &_ctrl_xfer.request); + + if (_ctrl_xfer.complete_cb) + { + // TODO refactor with usbd_driver_print_control_complete_name + _ctrl_xfer.complete_cb(rhport, CONTROL_STAGE_ACK, &_ctrl_xfer.request); + } + + return true; + } + + if ( _ctrl_xfer.request.bmRequestType_bit.direction == TUSB_DIR_OUT ) + { + TU_VERIFY(_ctrl_xfer.buffer); + memcpy(_ctrl_xfer.buffer, _usbd_ctrl_buf, xferred_bytes); + TU_LOG_MEM(2, _usbd_ctrl_buf, xferred_bytes, 2); + } + + _ctrl_xfer.total_xferred += xferred_bytes; + _ctrl_xfer.buffer += xferred_bytes; + + // Data Stage is complete when all request's length are transferred or + // a short packet is sent including zero-length packet. + if ( (_ctrl_xfer.request.wLength == _ctrl_xfer.total_xferred) || (xferred_bytes < CFG_TUD_ENDPOINT0_SIZE) ) + { + // DATA stage is complete + bool is_ok = true; + + // invoke complete callback if set + // callback can still stall control in status phase e.g out data does not make sense + if ( _ctrl_xfer.complete_cb ) + { + #if CFG_TUSB_DEBUG >= 2 + usbd_driver_print_control_complete_name(_ctrl_xfer.complete_cb); + #endif + + is_ok = _ctrl_xfer.complete_cb(rhport, CONTROL_STAGE_DATA, &_ctrl_xfer.request); + } + + if ( is_ok ) + { + // Send status + TU_ASSERT( _status_stage_xact(rhport, &_ctrl_xfer.request) ); + }else + { + // Stall both IN and OUT control endpoint + dcd_edpt_stall(rhport, EDPT_CTRL_OUT); + dcd_edpt_stall(rhport, EDPT_CTRL_IN); + } + } + else + { + // More data to transfer + TU_ASSERT( _data_stage_xact(rhport) ); + } + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd_pvt.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd_pvt.h new file mode 100644 index 000000000..7607b9895 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/device/usbd_pvt.h @@ -0,0 +1,115 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ +#ifndef USBD_PVT_H_ +#define USBD_PVT_H_ + +#include "osal/osal.h" +#include "common/tusb_fifo.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Driver API +//--------------------------------------------------------------------+ + +typedef struct +{ + #if CFG_TUSB_DEBUG >= 2 + char const* name; + #endif + + void (* init ) (void); + void (* reset ) (uint8_t rhport); + uint16_t (* open ) (uint8_t rhport, tusb_desc_interface_t const * desc_intf, uint16_t max_len); + bool (* control_xfer_cb ) (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); + bool (* xfer_cb ) (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); + void (* sof ) (uint8_t rhport); /* optional */ +} usbd_class_driver_t; + +// Invoked when initializing device stack to get additional class drivers. +// Can optionally implemented by application to extend/overwrite class driver support. +// Note: The drivers array must be accessible at all time when stack is active +usbd_class_driver_t const* usbd_app_driver_get_cb(uint8_t* driver_count) TU_ATTR_WEAK; + +typedef bool (*usbd_control_xfer_cb_t)(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request); + +//--------------------------------------------------------------------+ +// USBD Endpoint API +//--------------------------------------------------------------------+ + +// Open an endpoint +bool usbd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep); + +// Close an endpoint +void usbd_edpt_close(uint8_t rhport, uint8_t ep_addr); + +// Submit a usb transfer +bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes); + +// Submit a usb ISO transfer by use of a FIFO (ring buffer) - all bytes in FIFO get transmitted +bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes); + +// Claim an endpoint before submitting a transfer. +// If caller does not make any transfer, it must release endpoint for others. +bool usbd_edpt_claim(uint8_t rhport, uint8_t ep_addr); + +// Release an endpoint without submitting a transfer +bool usbd_edpt_release(uint8_t rhport, uint8_t ep_addr); + +// Check if endpoint is busy transferring +bool usbd_edpt_busy(uint8_t rhport, uint8_t ep_addr); + +// Stall endpoint +void usbd_edpt_stall(uint8_t rhport, uint8_t ep_addr); + +// Clear stalled endpoint +void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr); + +// Check if endpoint is stalled +bool usbd_edpt_stalled(uint8_t rhport, uint8_t ep_addr); + +// Check if endpoint is ready (not busy and not stalled) +TU_ATTR_ALWAYS_INLINE static inline +bool usbd_edpt_ready(uint8_t rhport, uint8_t ep_addr) +{ + return !usbd_edpt_busy(rhport, ep_addr) && !usbd_edpt_stalled(rhport, ep_addr); +} + +/*------------------------------------------------------------------*/ +/* Helper + *------------------------------------------------------------------*/ + +bool usbd_open_edpt_pair(uint8_t rhport, uint8_t const* p_desc, uint8_t ep_count, uint8_t xfer_type, uint8_t* ep_out, uint8_t* ep_in); +void usbd_defer_func( osal_task_func_t func, void* param, bool in_isr ); + + +#ifdef __cplusplus + } +#endif + +#endif /* USBD_PVT_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hcd.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hcd.h new file mode 100644 index 000000000..eb53d2e80 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hcd.h @@ -0,0 +1,179 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_HCD_H_ +#define _TUSB_HCD_H_ + +#include "common/tusb_common.h" +#include "osal/osal.h" +#include "common/tusb_fifo.h" +#include "hcd_attr.h" + +#ifdef __cplusplus + extern "C" { +#endif + + //--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef enum +{ + HCD_EVENT_DEVICE_ATTACH, + HCD_EVENT_DEVICE_REMOVE, + HCD_EVENT_XFER_COMPLETE, + + // Not an HCD event, just a convenient way to defer ISR function + USBH_EVENT_FUNC_CALL, + + HCD_EVENT_COUNT +} hcd_eventid_t; + +typedef struct +{ + uint8_t rhport; + uint8_t event_id; + uint8_t dev_addr; + + union + { + // Attach, Remove + struct { + uint8_t hub_addr; + uint8_t hub_port; + uint8_t speed; + } connection; + + // XFER_COMPLETE + struct { + uint8_t ep_addr; + uint8_t result; + uint32_t len; + } xfer_complete; + + // FUNC_CALL + struct { + void (*func) (void*); + void* param; + }func_call; + }; + +} hcd_event_t; + +#if TUSB_OPT_HOST_ENABLED +// Max number of endpoints per device +enum { + // TODO better computation + HCD_MAX_ENDPOINT = CFG_TUH_DEVICE_MAX*(CFG_TUH_HUB + CFG_TUH_HID*2 + CFG_TUH_MSC*2 + CFG_TUH_CDC*3), + HCD_MAX_XFER = HCD_MAX_ENDPOINT*2, +}; + +//#define HCD_MAX_ENDPOINT 16 +//#define HCD_MAX_XFER 16 + +typedef struct { + uint8_t rhport; + uint8_t hub_addr; + uint8_t hub_port; + uint8_t speed; +} hcd_devtree_info_t; + +#endif + +//--------------------------------------------------------------------+ +// Controller API +//--------------------------------------------------------------------+ + +// Initialize controller to host mode +bool hcd_init(uint8_t rhport); + +// Interrupt Handler +void hcd_int_handler(uint8_t rhport); + +// Enable USB interrupt +void hcd_int_enable (uint8_t rhport); + +// Disable USB interrupt +void hcd_int_disable(uint8_t rhport); + +// Get frame number (1ms) +uint32_t hcd_frame_number(uint8_t rhport); + +//--------------------------------------------------------------------+ +// Port API +//--------------------------------------------------------------------+ + +// Get the current connect status of roothub port +bool hcd_port_connect_status(uint8_t rhport); + +// Reset USB bus on the port +void hcd_port_reset(uint8_t rhport); + +// TODO implement later +void hcd_port_reset_end(uint8_t rhport); + +// Get port link speed +tusb_speed_t hcd_port_speed_get(uint8_t rhport); + +// HCD closes all opened endpoints belong to this device +void hcd_device_close(uint8_t rhport, uint8_t dev_addr); + +//--------------------------------------------------------------------+ +// Endpoints API +//--------------------------------------------------------------------+ + +bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8]); +bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc); +bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t buflen); +bool hcd_edpt_clear_stall(uint8_t dev_addr, uint8_t ep_addr); + +//--------------------------------------------------------------------+ +// USBH implemented API +//--------------------------------------------------------------------+ + +// Get device tree information of a device +// USB device tree can be complicated and manged by USBH, this help HCD to retrieve +// needed topology info to carry out its work +extern void hcd_devtree_get_info(uint8_t dev_addr, hcd_devtree_info_t* devtree_info); + +//------------- Event API -------------// + +// Called by HCD to notify stack +extern void hcd_event_handler(hcd_event_t const* event, bool in_isr); + +// Helper to send device attach event +extern void hcd_event_device_attach(uint8_t rhport, bool in_isr); + +// Helper to send device removal event +extern void hcd_event_device_remove(uint8_t rhport, bool in_isr); + +// Helper to send USB transfer event +extern void hcd_event_xfer_complete(uint8_t dev_addr, uint8_t ep_addr, uint32_t xferred_bytes, xfer_result_t result, bool in_isr); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_HCD_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hcd_attr.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hcd_attr.h new file mode 100644 index 000000000..06011c63c --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hcd_attr.h @@ -0,0 +1,105 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021, Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef TUSB_HCD_ATTR_H_ +#define TUSB_HCD_ATTR_H_ + +#include "tusb_option.h" + +// Attribute includes +// - ENDPOINT_MAX: max (logical) number of endpoint +// - PORT_HIGHSPEED: mask to indicate which port support highspeed mode, bit0 for port0 and so on. + +//------------- NXP -------------// +#if TU_CHECK_MCU(OPT_MCU_LPC175X_6X, OPT_MCU_LPC177X_8X, OPT_MCU_LPC40XX) + #define HCD_ATTR_OHCI + +#elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX) + #define HCD_ATTR_EHCI_TRANSDIMENSION + +#elif TU_CHECK_MCU(OPT_MCU_LPC54XXX) + // #define HCD_ATTR_EHCI_NXP_PTD + +#elif TU_CHECK_MCU(OPT_MCU_LPC55XX) + // #define HCD_ATTR_EHCI_NXP_PTD + +#elif TU_CHECK_MCU(OPT_MCU_MIMXRT10XX) + #define HCD_ATTR_EHCI_TRANSDIMENSION + +#elif TU_CHECK_MCU(OPT_MCU_MKL25ZXX) + +//------------- Microchip -------------// +#elif TU_CHECK_MCU(OPT_MCU_SAMD21, OPT_MCU_SAMD51, OPT_MCU_SAME5X) || \ + TU_CHECK_MCU(OPT_MCU_SAMD11, OPT_MCU_SAML21, OPT_MCU_SAML22) + +#elif TU_CHECK_MCU(OPT_MCU_SAMG) + +#elif TU_CHECK_MCU(OPT_MCU_SAMX7X) + +//------------- ST -------------// +#elif TU_CHECK_MCU(OPT_MCU_STM32F0, OPT_MCU_STM32F1, OPT_MCU_STM32F3) || \ + TU_CHECK_MCU(OPT_MCU_STM32L0, OPT_MCU_STM32L1, OPT_MCU_STM32L4) + +#elif TU_CHECK_MCU(OPT_MCU_STM32F2, OPT_MCU_STM32F3, OPT_MCU_STM32F4) + +#elif TU_CHECK_MCU(OPT_MCU_STM32F7) + +#elif TU_CHECK_MCU(OPT_MCU_STM32H7) + +//------------- Sony -------------// +#elif TU_CHECK_MCU(OPT_MCU_CXD56) + +//------------- Nuvoton -------------// +#elif TU_CHECK_MCU(OPT_MCU_NUC505) + +//------------- Espressif -------------// +#elif TU_CHECK_MCU(OPT_MCU_ESP32S2, OPT_MCU_ESP32S3) + +//------------- Raspberry Pi -------------// +#elif TU_CHECK_MCU(OPT_MCU_RP2040) + +//------------- Silabs -------------// +#elif TU_CHECK_MCU(OPT_MCU_EFM32GG) + +//------------- Renesas -------------// +#elif TU_CHECK_MCU(OPT_MCU_RX63X, OPT_MCU_RX65X, OPT_MCU_RX72N) + +//#elif TU_CHECK_MCU(OPT_MCU_MM32F327X) +// #define DCD_ATTR_ENDPOINT_MAX not known yet + +//------------- GigaDevice -------------// +#elif TU_CHECK_MCU(OPT_MCU_GD32VF103) + +#else +// #warning "DCD_ATTR_ENDPOINT_MAX is not defined for this MCU, default to 8" +#endif + +// Default to fullspeed if not defined +//#ifndef PORT_HIGHSPEED +// #define DCD_ATTR_PORT_HIGHSPEED 0x00 +//#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hub.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hub.c new file mode 100644 index 000000000..fd4dbd04a --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hub.c @@ -0,0 +1,388 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (TUSB_OPT_HOST_ENABLED && CFG_TUH_HUB) + +#include "usbh.h" +#include "usbh_classdriver.h" +#include "hub.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ +typedef struct +{ + uint8_t itf_num; + uint8_t ep_in; + uint8_t port_count; + uint8_t status_change; // data from status change interrupt endpoint + + hub_port_status_response_t port_status; +} hub_interface_t; + +CFG_TUSB_MEM_SECTION static hub_interface_t hub_data[CFG_TUH_HUB]; +CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4) static uint8_t _hub_buffer[sizeof(descriptor_hub_desc_t)]; + +TU_ATTR_ALWAYS_INLINE +static inline hub_interface_t* get_itf(uint8_t dev_addr) +{ + return &hub_data[dev_addr-1-CFG_TUH_DEVICE_MAX]; +} + +#if CFG_TUSB_DEBUG +static char const* const _hub_feature_str[] = +{ + [HUB_FEATURE_PORT_CONNECTION ] = "PORT_CONNECTION", + [HUB_FEATURE_PORT_ENABLE ] = "PORT_ENABLE", + [HUB_FEATURE_PORT_SUSPEND ] = "PORT_SUSPEND", + [HUB_FEATURE_PORT_OVER_CURRENT ] = "PORT_OVER_CURRENT", + [HUB_FEATURE_PORT_RESET ] = "PORT_RESET", + [HUB_FEATURE_PORT_POWER ] = "PORT_POWER", + [HUB_FEATURE_PORT_LOW_SPEED ] = "PORT_LOW_SPEED", + [HUB_FEATURE_PORT_CONNECTION_CHANGE ] = "PORT_CONNECTION_CHANGE", + [HUB_FEATURE_PORT_ENABLE_CHANGE ] = "PORT_ENABLE_CHANGE", + [HUB_FEATURE_PORT_SUSPEND_CHANGE ] = "PORT_SUSPEND_CHANGE", + [HUB_FEATURE_PORT_OVER_CURRENT_CHANGE ] = "PORT_OVER_CURRENT_CHANGE", + [HUB_FEATURE_PORT_RESET_CHANGE ] = "PORT_RESET_CHANGE", + [HUB_FEATURE_PORT_TEST ] = "PORT_TEST", + [HUB_FEATURE_PORT_INDICATOR ] = "PORT_INDICATOR", +}; +#endif + +//--------------------------------------------------------------------+ +// HUB +//--------------------------------------------------------------------+ +bool hub_port_clear_feature(uint8_t hub_addr, uint8_t hub_port, uint8_t feature, tuh_control_complete_cb_t complete_cb) +{ + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_OTHER, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = HUB_REQUEST_CLEAR_FEATURE, + .wValue = feature, + .wIndex = hub_port, + .wLength = 0 + }; + + TU_LOG2("HUB Clear Feature: %s, addr = %u port = %u\r\n", _hub_feature_str[feature], hub_addr, hub_port); + TU_ASSERT( tuh_control_xfer(hub_addr, &request, NULL, complete_cb) ); + return true; +} + +bool hub_port_set_feature(uint8_t hub_addr, uint8_t hub_port, uint8_t feature, tuh_control_complete_cb_t complete_cb) +{ + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_OTHER, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_OUT + }, + .bRequest = HUB_REQUEST_SET_FEATURE, + .wValue = feature, + .wIndex = hub_port, + .wLength = 0 + }; + + TU_LOG2("HUB Set Feature: %s, addr = %u port = %u\r\n", _hub_feature_str[feature], hub_addr, hub_port); + TU_ASSERT( tuh_control_xfer(hub_addr, &request, NULL, complete_cb) ); + return true; +} + +bool hub_port_reset(uint8_t hub_addr, uint8_t hub_port, tuh_control_complete_cb_t complete_cb) +{ + return hub_port_set_feature(hub_addr, hub_port, HUB_FEATURE_PORT_RESET, complete_cb); +} + +bool hub_port_get_status(uint8_t hub_addr, uint8_t hub_port, void* resp, tuh_control_complete_cb_t complete_cb) +{ + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_OTHER, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_IN + }, + .bRequest = HUB_REQUEST_GET_STATUS, + .wValue = 0, + .wIndex = hub_port, + .wLength = 4 + }; + + TU_LOG2("HUB Get Port Status: addr = %u port = %u\r\n", hub_addr, hub_port); + TU_ASSERT( tuh_control_xfer( hub_addr, &request, resp, complete_cb) ); + return true; +} + +//--------------------------------------------------------------------+ +// CLASS-USBH API (don't require to verify parameters) +//--------------------------------------------------------------------+ +void hub_init(void) +{ + tu_memclr(hub_data, sizeof(hub_data)); +} + +bool hub_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len) +{ + TU_VERIFY(TUSB_CLASS_HUB == itf_desc->bInterfaceClass && + 0 == itf_desc->bInterfaceSubClass); + + // hub driver does not support multiple TT yet + TU_VERIFY(itf_desc->bInterfaceProtocol <= 1); + + // msc driver length is fixed + uint16_t const drv_len = sizeof(tusb_desc_interface_t) + sizeof(tusb_desc_endpoint_t); + TU_ASSERT(drv_len <= max_len); + + //------------- Interrupt Status endpoint -------------// + tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc); + + TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType && + TUSB_XFER_INTERRUPT == desc_ep->bmAttributes.xfer, 0); + + TU_ASSERT(usbh_edpt_open(rhport, dev_addr, desc_ep)); + + hub_interface_t* p_hub = get_itf(dev_addr); + + p_hub->itf_num = itf_desc->bInterfaceNumber; + p_hub->ep_in = desc_ep->bEndpointAddress; + + return true; +} + +void hub_close(uint8_t dev_addr) +{ + TU_VERIFY(dev_addr > CFG_TUH_DEVICE_MAX, ); + hub_interface_t* p_hub = get_itf(dev_addr); + + if (p_hub->ep_in) tu_memclr(p_hub, sizeof( hub_interface_t)); +} + +bool hub_status_pipe_queue(uint8_t dev_addr) +{ + hub_interface_t* hub_itf = get_itf(dev_addr); + return usbh_edpt_xfer(dev_addr, hub_itf->ep_in, &hub_itf->status_change, 1); +} + + +//--------------------------------------------------------------------+ +// Set Configure +//--------------------------------------------------------------------+ + +static bool config_set_port_power (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool config_port_power_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); + +bool hub_set_config(uint8_t dev_addr, uint8_t itf_num) +{ + hub_interface_t* p_hub = get_itf(dev_addr); + TU_ASSERT(itf_num == p_hub->itf_num); + + // Get Hub Descriptor + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_DEVICE, + .type = TUSB_REQ_TYPE_CLASS, + .direction = TUSB_DIR_IN + }, + .bRequest = HUB_REQUEST_GET_DESCRIPTOR, + .wValue = 0, + .wIndex = 0, + .wLength = sizeof(descriptor_hub_desc_t) + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &request, _hub_buffer, config_set_port_power) ); + + return true; +} + +static bool config_set_port_power (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + hub_interface_t* p_hub = get_itf(dev_addr); + + // only use number of ports in hub descriptor + descriptor_hub_desc_t const* desc_hub = (descriptor_hub_desc_t const*) _hub_buffer; + p_hub->port_count = desc_hub->bNbrPorts; + + // May need to GET_STATUS + + // Set Port Power to be able to detect connection, starting with port 1 + uint8_t const hub_port = 1; + return hub_port_set_feature(dev_addr, hub_port, HUB_FEATURE_PORT_POWER, config_port_power_complete); +} + +static bool config_port_power_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + TU_ASSERT(XFER_RESULT_SUCCESS == result); + hub_interface_t* p_hub = get_itf(dev_addr); + + if (request->wIndex == p_hub->port_count) + { + // All ports are power -> queue notification status endpoint and + // complete the SET CONFIGURATION + TU_ASSERT( usbh_edpt_xfer(dev_addr, p_hub->ep_in, &p_hub->status_change, 1) ); + + usbh_driver_set_config_complete(dev_addr, p_hub->itf_num); + }else + { + // power next port + uint8_t const hub_port = (uint8_t) (request->wIndex + 1); + return hub_port_set_feature(dev_addr, hub_port, HUB_FEATURE_PORT_POWER, config_port_power_complete); + } + + return true; +} + +//--------------------------------------------------------------------+ +// Connection Changes +//--------------------------------------------------------------------+ + +static bool connection_get_status_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool connection_clear_conn_change_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool connection_port_reset_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); + +// callback as response of interrupt endpoint polling +bool hub_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) xferred_bytes; // TODO can be more than 1 for hub with lots of ports + (void) ep_addr; + TU_ASSERT(result == XFER_RESULT_SUCCESS); + + hub_interface_t* p_hub = get_itf(dev_addr); + + TU_LOG2(" Port Status Change = 0x%02X\r\n", p_hub->status_change); + + // Hub ignore bit0 in status change + for (uint8_t port=1; port <= p_hub->port_count; port++) + { + if ( tu_bit_test(p_hub->status_change, port) ) + { + hub_port_get_status(dev_addr, port, &p_hub->port_status, connection_get_status_complete); + break; + } + } + + // NOTE: next status transfer is queued by usbh.c after handling this request + + return true; +} + +static bool connection_get_status_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + TU_ASSERT(result == XFER_RESULT_SUCCESS); + + hub_interface_t* p_hub = get_itf(dev_addr); + uint8_t const port_num = (uint8_t) request->wIndex; + + // Connection change + if (p_hub->port_status.change.connection) + { + // Port is powered and enabled + //TU_VERIFY(port_status.status_current.port_power && port_status.status_current.port_enable, ); + + // Acknowledge Port Connection Change + hub_port_clear_feature(dev_addr, port_num, HUB_FEATURE_PORT_CONNECTION_CHANGE, connection_clear_conn_change_complete); + }else + { + // Other changes are: Enable, Suspend, Over Current, Reset, L1 state + // TODO clear change + + // prepare for next hub status + // TODO continue with status_change, or maybe we can do it again with status + hub_status_pipe_queue(dev_addr); + } + + return true; +} + +static bool connection_clear_conn_change_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + TU_ASSERT(result == XFER_RESULT_SUCCESS); + + hub_interface_t* p_hub = get_itf(dev_addr); + uint8_t const port_num = (uint8_t) request->wIndex; + + if ( p_hub->port_status.status.connection ) + { + // Reset port if attach event + hub_port_reset(dev_addr, port_num, connection_port_reset_complete); + }else + { + // submit detach event + hcd_event_t event = + { + .rhport = usbh_get_rhport(dev_addr), + .event_id = HCD_EVENT_DEVICE_REMOVE, + .connection = + { + .hub_addr = dev_addr, + .hub_port = port_num + } + }; + + hcd_event_handler(&event, false); + } + + return true; +} + +static bool connection_port_reset_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + TU_ASSERT(result == XFER_RESULT_SUCCESS); + + // hub_interface_t* p_hub = get_itf(dev_addr); + uint8_t const port_num = (uint8_t) request->wIndex; + + // submit attach event + hcd_event_t event = + { + .rhport = usbh_get_rhport(dev_addr), + .event_id = HCD_EVENT_DEVICE_ATTACH, + .connection = + { + .hub_addr = dev_addr, + .hub_port = port_num + } + }; + + hcd_event_handler(&event, false); + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hub.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hub.h new file mode 100644 index 000000000..c4d544193 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/hub.h @@ -0,0 +1,196 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +/** \ingroup group_class + * \defgroup ClassDriver_Hub Hub (Host only) + * \details Like most PC's OS, Hub support is completely hidden from Application. In fact, application cannot determine whether + * a device is mounted directly via roothub or via a hub's port. All Hub-related procedures are performed and managed + * by tinyusb stack. Unless you are trying to develop the stack itself, there are nothing else can be used by Application. + * \note Due to my laziness, only 1-level of Hub is supported. In other way, the stack cannot mount a hub via another hub. + * @{ + */ + +#ifndef _TUSB_HUB_H_ +#define _TUSB_HUB_H_ + +#include "common/tusb_common.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//D1...D0: Logical Power Switching Mode +//00: Ganged power switching (all ports’power at +//once) +//01: Individual port power switching +//1X: Reserved. Used only on 1.0 compliant hubs +//that implement no power switching +//D2: Identifies a Compound Device +//0: Hub is not part of a compound device. +//1: Hub is part of a compound device. +//D4...D3: Over-current Protection Mode +//00: Global Over-current Protection. The hub +//reports over-current as a summation of all +//ports’current draw, without a breakdown of +//individual port over-current status. +//01: Individual Port Over-current Protection. The +//hub reports over-current on a per-port basis. +//Each port has an over-current status. +//1X: No Over-current Protection. This option is +//allowed only for bus-powered hubs that do not +//implement over-current protection. +// +//D6...D5: TT Think TIme +//00: TT requires at most 8 FS bit times of inter +//transaction gap on a full-/low-speed +//downstream bus. +//01: TT requires at most 16 FS bit times. +//10: TT requires at most 24 FS bit times. +//11: TT requires at most 32 FS bit times. +//D7: Port Indicators Supported +//0: Port Indicators are not supported on its +//downstream facing ports and the +//PORT_INDICATOR request has no effect. +//1: Port Indicators are supported on its +//downstream facing ports and the +//PORT_INDICATOR request controls the +//indicators. See Section 11.5.3. +//D15...D8: Reserved + +typedef struct TU_ATTR_PACKED{ + uint8_t bLength ; ///< Size of descriptor + uint8_t bDescriptorType ; ///< Other_speed_Configuration Type + uint8_t bNbrPorts; + uint16_t wHubCharacteristics; + uint8_t bPwrOn2PwrGood; + uint8_t bHubContrCurrent; + uint8_t DeviceRemovable; // bitmap each bit for a port (from bit1) + uint8_t PortPwrCtrlMask; // just for compatibility, should be 0xff +} descriptor_hub_desc_t; + +TU_VERIFY_STATIC( sizeof(descriptor_hub_desc_t) == 9, "size is not correct"); + +enum { + HUB_REQUEST_GET_STATUS = 0 , + HUB_REQUEST_CLEAR_FEATURE = 1 , + + HUB_REQUEST_SET_FEATURE = 3 , + + HUB_REQUEST_GET_DESCRIPTOR = 6 , + HUB_REQUEST_SET_DESCRIPTOR = 7 , + HUB_REQUEST_CLEAR_TT_BUFFER = 8 , + HUB_REQUEST_RESET_TT = 9 , + HUB_REQUEST_GET_TT_STATE = 10 , + HUB_REQUEST_STOP_TT = 11 +}; + +enum { + HUB_FEATURE_HUB_LOCAL_POWER_CHANGE = 0, + HUB_FEATURE_HUB_OVER_CURRENT_CHANGE +}; + +enum{ + HUB_FEATURE_PORT_CONNECTION = 0, + HUB_FEATURE_PORT_ENABLE = 1, + HUB_FEATURE_PORT_SUSPEND = 2, + HUB_FEATURE_PORT_OVER_CURRENT = 3, + HUB_FEATURE_PORT_RESET = 4, + + HUB_FEATURE_PORT_POWER = 8, + HUB_FEATURE_PORT_LOW_SPEED = 9, + + HUB_FEATURE_PORT_CONNECTION_CHANGE = 16, + HUB_FEATURE_PORT_ENABLE_CHANGE = 17, + HUB_FEATURE_PORT_SUSPEND_CHANGE = 18, + HUB_FEATURE_PORT_OVER_CURRENT_CHANGE = 19, + HUB_FEATURE_PORT_RESET_CHANGE = 20, + HUB_FEATURE_PORT_TEST = 21, + HUB_FEATURE_PORT_INDICATOR = 22 +}; + +// data in response of HUB_REQUEST_GET_STATUS, wIndex = 0 (hub) +typedef struct { + union{ + struct TU_ATTR_PACKED { + uint16_t local_power_source : 1; + uint16_t over_current : 1; + uint16_t : 14; + }; + + uint16_t value; + } status, change; +} hub_status_response_t; + +TU_VERIFY_STATIC( sizeof(hub_status_response_t) == 4, "size is not correct"); + +// data in response of HUB_REQUEST_GET_STATUS, wIndex = Port num +typedef struct { + union { + struct TU_ATTR_PACKED { + uint16_t connection : 1; + uint16_t port_enable : 1; + uint16_t suspend : 1; + uint16_t over_current : 1; + uint16_t reset : 1; + + uint16_t : 3; + uint16_t port_power : 1; + uint16_t low_speed : 1; + uint16_t high_speed : 1; + uint16_t port_test_mode : 1; + uint16_t port_indicator_control : 1; + uint16_t TU_RESERVED : 3; + }; + + uint16_t value; + } status, change; +} hub_port_status_response_t; + +TU_VERIFY_STATIC( sizeof(hub_port_status_response_t) == 4, "size is not correct"); + +bool hub_port_clear_feature(uint8_t hub_addr, uint8_t hub_port, uint8_t feature, tuh_control_complete_cb_t complete_cb); +bool hub_port_set_feature(uint8_t hub_addr, uint8_t hub_port, uint8_t feature, tuh_control_complete_cb_t complete_cb); + +bool hub_port_reset(uint8_t hub_addr, uint8_t hub_port, tuh_control_complete_cb_t complete_cb); +bool hub_port_get_status(uint8_t hub_addr, uint8_t hub_port, void* resp, tuh_control_complete_cb_t complete_cb); +bool hub_status_pipe_queue(uint8_t dev_addr); + +//--------------------------------------------------------------------+ +// Internal Class Driver API +//--------------------------------------------------------------------+ +void hub_init (void); +bool hub_open (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len); +bool hub_set_config (uint8_t dev_addr, uint8_t itf_num); +bool hub_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes); +void hub_close (uint8_t dev_addr); + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_HUB_H_ */ + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh.c new file mode 100644 index 000000000..b8439addc --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh.c @@ -0,0 +1,1204 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if TUSB_OPT_HOST_ENABLED + +#include "tusb.h" +#include "host/usbh.h" +#include "host/usbh_classdriver.h" +#include "hub.h" + +//--------------------------------------------------------------------+ +// USBH Configuration +//--------------------------------------------------------------------+ + +// TODO remove,update +#ifndef CFG_TUH_EP_MAX +#define CFG_TUH_EP_MAX 9 +#endif + +#ifndef CFG_TUH_TASK_QUEUE_SZ +#define CFG_TUH_TASK_QUEUE_SZ 16 +#endif + +// Debug level of USBD +#define USBH_DBG_LVL 2 + +//--------------------------------------------------------------------+ +// USBH-HCD common data structure +//--------------------------------------------------------------------+ + +// device0 struct must be strictly a subset of normal device struct +typedef struct +{ + // port + uint8_t rhport; + uint8_t hub_addr; + uint8_t hub_port; + uint8_t speed; + + volatile struct TU_ATTR_PACKED + { + uint8_t connected : 1; + uint8_t addressed : 1; + uint8_t configured : 1; + uint8_t suspended : 1; + }; +} usbh_dev0_t; + +typedef struct { + // port + uint8_t rhport; + uint8_t hub_addr; + uint8_t hub_port; + uint8_t speed; + + volatile struct TU_ATTR_PACKED + { + uint8_t connected : 1; + uint8_t addressed : 1; + uint8_t configured : 1; + uint8_t suspended : 1; + }; + + //------------- device descriptor -------------// + uint16_t vid; + uint16_t pid; + + uint8_t ep0_size; + uint8_t i_manufacturer; + uint8_t i_product; + uint8_t i_serial; + + //------------- configuration descriptor -------------// + // uint8_t interface_count; // bNumInterfaces alias + + //------------- device -------------// + volatile uint8_t state; // device state, value from enum tusbh_device_state_t + + uint8_t itf2drv[16]; // map interface number to driver (0xff is invalid) + uint8_t ep2drv[CFG_TUH_EP_MAX][2]; // map endpoint to driver ( 0xff is invalid ) + + struct TU_ATTR_PACKED + { + volatile bool busy : 1; + volatile bool stalled : 1; + volatile bool claimed : 1; + + // TODO merge ep2drv here, 4-bit should be sufficient + }ep_status[CFG_TUH_EP_MAX][2]; + + // Mutex for claiming endpoint, only needed when using with preempted RTOS +#if CFG_TUSB_OS != OPT_OS_NONE + osal_mutex_def_t mutexdef; + osal_mutex_t mutex; +#endif + +} usbh_device_t; + + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +// Invalid driver ID in itf2drv[] ep2drv[][] mapping +enum { DRVID_INVALID = 0xFFu }; +enum { ADDR_INVALID = 0xFFu }; + +#if CFG_TUSB_DEBUG >= 2 + #define DRIVER_NAME(_name) .name = _name, +#else + #define DRIVER_NAME(_name) +#endif + +static usbh_class_driver_t const usbh_class_drivers[] = +{ + #if CFG_TUH_CDC + { + DRIVER_NAME("CDC") + .init = cdch_init, + .open = cdch_open, + .set_config = cdch_set_config, + .xfer_cb = cdch_xfer_cb, + .close = cdch_close + }, + #endif + + #if CFG_TUH_MSC + { + DRIVER_NAME("MSC") + .init = msch_init, + .open = msch_open, + .set_config = msch_set_config, + .xfer_cb = msch_xfer_cb, + .close = msch_close + }, + #endif + + #if CFG_TUH_HID + { + DRIVER_NAME("HID") + .init = hidh_init, + .open = hidh_open, + .set_config = hidh_set_config, + .xfer_cb = hidh_xfer_cb, + .close = hidh_close + }, + #endif + + #if CFG_TUH_HUB + { + DRIVER_NAME("HUB") + .init = hub_init, + .open = hub_open, + .set_config = hub_set_config, + .xfer_cb = hub_xfer_cb, + .close = hub_close + }, + #endif + + #if CFG_TUH_VENDOR + { + DRIVER_NAME("VENDOR") + .init = cush_init, + .open = cush_open_subtask, + .xfer_cb = cush_isr, + .close = cush_close + } + #endif +}; + +enum { USBH_CLASS_DRIVER_COUNT = TU_ARRAY_SIZE(usbh_class_drivers) }; + +enum { RESET_DELAY = 500 }; // 200 USB specs say only 50ms but many devices require much longer + +enum { CONFIG_NUM = 1 }; // default to use configuration 1 + + +//--------------------------------------------------------------------+ +// INTERNAL OBJECT & FUNCTION DECLARATION +//--------------------------------------------------------------------+ + +static bool _usbh_initialized = false; + +// Device with address = 0 for enumeration +static usbh_dev0_t _dev0; + +// all devices excluding zero-address +// hub address start from CFG_TUH_DEVICE_MAX+1 +CFG_TUSB_MEM_SECTION usbh_device_t _usbh_devices[CFG_TUH_DEVICE_MAX + CFG_TUH_HUB]; + +// Event queue +// role device/host is used by OS NONE for mutex (disable usb isr) +OSAL_QUEUE_DEF(OPT_MODE_HOST, _usbh_qdef, CFG_TUH_TASK_QUEUE_SZ, hcd_event_t); +static osal_queue_t _usbh_q; + +CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _usbh_ctrl_buf[CFG_TUH_ENUMERATION_BUFSIZE]; + +//------------- Helper Function -------------// + +TU_ATTR_ALWAYS_INLINE +static inline usbh_device_t* get_device(uint8_t dev_addr) +{ + TU_ASSERT(dev_addr, NULL); + return &_usbh_devices[dev_addr-1]; +} + +static bool enum_new_device(hcd_event_t* event); +static void process_device_unplugged(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port); +static bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size); + +// from usbh_control.c +extern bool usbh_control_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); + +//--------------------------------------------------------------------+ +// PUBLIC API (Parameter Verification is required) +//--------------------------------------------------------------------+ +bool tuh_mounted(uint8_t dev_addr) +{ + return get_device(dev_addr)->configured; +} + +bool tuh_vid_pid_get(uint8_t dev_addr, uint16_t* vid, uint16_t* pid) +{ + *vid = *pid = 0; + + TU_VERIFY(tuh_mounted(dev_addr)); + + usbh_device_t const* dev = get_device(dev_addr); + + *vid = dev->vid; + *pid = dev->pid; + + return true; +} + +tusb_speed_t tuh_speed_get (uint8_t dev_addr) +{ + return (tusb_speed_t) (dev_addr ? get_device(dev_addr)->speed : _dev0.speed); +} + +#if CFG_TUSB_OS == OPT_OS_NONE +void osal_task_delay(uint32_t msec) +{ + (void) msec; + + const uint32_t start = hcd_frame_number(TUH_OPT_RHPORT); + while ( ( hcd_frame_number(TUH_OPT_RHPORT) - start ) < msec ) {} +} +#endif + +//--------------------------------------------------------------------+ +// CLASS-USBD API (don't require to verify parameters) +//--------------------------------------------------------------------+ + +bool tuh_inited(void) +{ + return _usbh_initialized; +} + +bool tuh_init(uint8_t rhport) +{ + // skip if already initialized + if (_usbh_initialized) return _usbh_initialized; + + TU_LOG2("USBH init\r\n"); + + tu_memclr(_usbh_devices, sizeof(_usbh_devices)); + tu_memclr(&_dev0, sizeof(_dev0)); + + //------------- Enumeration & Reporter Task init -------------// + _usbh_q = osal_queue_create( &_usbh_qdef ); + TU_ASSERT(_usbh_q != NULL); + + //------------- Semaphore, Mutex for Control Pipe -------------// + for(uint8_t i=0; imutex = osal_mutex_create(&dev->mutexdef); + TU_ASSERT(dev->mutex); +#endif + + memset(dev->itf2drv, DRVID_INVALID, sizeof(dev->itf2drv)); // invalid mapping + memset(dev->ep2drv , DRVID_INVALID, sizeof(dev->ep2drv )); // invalid mapping + } + + // Class drivers init + for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++) + { + TU_LOG2("%s init\r\n", usbh_class_drivers[drv_id].name); + usbh_class_drivers[drv_id].init(); + } + + TU_ASSERT(hcd_init(rhport)); + hcd_int_enable(rhport); + + _usbh_initialized = true; + return true; +} + +/* USB Host Driver task + * This top level thread manages all host controller event and delegates events to class-specific drivers. + * This should be called periodically within the mainloop or rtos thread. + * + @code + int main(void) + { + application_init(); + tusb_init(); + + while(1) // the mainloop + { + application_code(); + tuh_task(); // tinyusb host task + } + } + @endcode + */ +void tuh_task(void) +{ + // Skip if stack is not initialized + if ( !tusb_inited() ) return; + + // Loop until there is no more events in the queue + while (1) + { + hcd_event_t event; + if ( !osal_queue_receive(_usbh_q, &event) ) return; + + switch (event.event_id) + { + case HCD_EVENT_DEVICE_ATTACH: + // TODO due to the shared _usbh_ctrl_buf, we must complete enumerating + // one device before enumerating another one. + TU_LOG2("USBH DEVICE ATTACH\r\n"); + enum_new_device(&event); + break; + + case HCD_EVENT_DEVICE_REMOVE: + TU_LOG2("USBH DEVICE REMOVED\r\n"); + process_device_unplugged(event.rhport, event.connection.hub_addr, event.connection.hub_port); + + #if CFG_TUH_HUB + // TODO remove + if ( event.connection.hub_addr != 0) + { + // done with hub, waiting for next data on status pipe + (void) hub_status_pipe_queue( event.connection.hub_addr ); + } + #endif + break; + + case HCD_EVENT_XFER_COMPLETE: + { + uint8_t const ep_addr = event.xfer_complete.ep_addr; + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const ep_dir = tu_edpt_dir(ep_addr); + + TU_LOG2("on EP %02X with %u bytes\r\n", ep_addr, (unsigned int) event.xfer_complete.len); + + if (event.dev_addr == 0) + { + // device 0 only has control endpoint + TU_ASSERT(epnum == 0, ); + usbh_control_xfer_cb(event.dev_addr, ep_addr, event.xfer_complete.result, event.xfer_complete.len); + } + else + { + usbh_device_t* dev = get_device(event.dev_addr); + dev->ep_status[epnum][ep_dir].busy = false; + dev->ep_status[epnum][ep_dir].claimed = 0; + + if ( 0 == epnum ) + { + usbh_control_xfer_cb(event.dev_addr, ep_addr, event.xfer_complete.result, event.xfer_complete.len); + }else + { + uint8_t drv_id = dev->ep2drv[epnum][ep_dir]; + TU_ASSERT(drv_id < USBH_CLASS_DRIVER_COUNT, ); + + TU_LOG2("%s xfer callback\r\n", usbh_class_drivers[drv_id].name); + usbh_class_drivers[drv_id].xfer_cb(event.dev_addr, ep_addr, event.xfer_complete.result, event.xfer_complete.len); + } + } + } + break; + + case USBH_EVENT_FUNC_CALL: + if ( event.func_call.func ) event.func_call.func(event.func_call.param); + break; + + default: break; + } + } +} + +//--------------------------------------------------------------------+ +// USBH API For Class Driver +//--------------------------------------------------------------------+ + +uint8_t usbh_get_rhport(uint8_t dev_addr) +{ + return (dev_addr == 0) ? _dev0.rhport : get_device(dev_addr)->rhport; +} + +uint8_t* usbh_get_enum_buf(void) +{ + return _usbh_ctrl_buf; +} + +//--------------------------------------------------------------------+ +// HCD Event Handler +//--------------------------------------------------------------------+ + +void hcd_devtree_get_info(uint8_t dev_addr, hcd_devtree_info_t* devtree_info) +{ + if (dev_addr) + { + usbh_device_t const* dev = get_device(dev_addr); + + devtree_info->rhport = dev->rhport; + devtree_info->hub_addr = dev->hub_addr; + devtree_info->hub_port = dev->hub_port; + devtree_info->speed = dev->speed; + }else + { + devtree_info->rhport = _dev0.rhport; + devtree_info->hub_addr = _dev0.hub_addr; + devtree_info->hub_port = _dev0.hub_port; + devtree_info->speed = _dev0.speed; + } +} + +void hcd_event_handler(hcd_event_t const* event, bool in_isr) +{ + switch (event->event_id) + { + default: + osal_queue_send(_usbh_q, event, in_isr); + break; + } +} + +void hcd_event_xfer_complete(uint8_t dev_addr, uint8_t ep_addr, uint32_t xferred_bytes, xfer_result_t result, bool in_isr) +{ + hcd_event_t event = + { + .rhport = 0, // TODO correct rhport + .event_id = HCD_EVENT_XFER_COMPLETE, + .dev_addr = dev_addr, + .xfer_complete = + { + .ep_addr = ep_addr, + .result = result, + .len = xferred_bytes + } + }; + + hcd_event_handler(&event, in_isr); +} + +void hcd_event_device_attach(uint8_t rhport, bool in_isr) +{ + hcd_event_t event = + { + .rhport = rhport, + .event_id = HCD_EVENT_DEVICE_ATTACH + }; + + event.connection.hub_addr = 0; + event.connection.hub_port = 0; + + hcd_event_handler(&event, in_isr); +} + +void hcd_event_device_remove(uint8_t hostid, bool in_isr) +{ + hcd_event_t event = + { + .rhport = hostid, + .event_id = HCD_EVENT_DEVICE_REMOVE + }; + + event.connection.hub_addr = 0; + event.connection.hub_port = 0; + + hcd_event_handler(&event, in_isr); +} + + +// a device unplugged on hostid, hub_addr, hub_port +// return true if found and unmounted device, false if cannot find +void process_device_unplugged(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port) +{ + //------------- find the all devices (star-network) under port that is unplugged -------------// + // TODO mark as disconnected in ISR, also handle dev0 + for ( uint8_t dev_id = 0; dev_id < TU_ARRAY_SIZE(_usbh_devices); dev_id++ ) + { + usbh_device_t* dev = &_usbh_devices[dev_id]; + uint8_t const dev_addr = dev_id+1; + + // TODO Hub multiple level + if (dev->rhport == rhport && + (hub_addr == 0 || dev->hub_addr == hub_addr) && // hub_addr == 0 & hub_port == 0 means roothub + (hub_port == 0 || dev->hub_port == hub_port) && + dev->state != TUSB_DEVICE_STATE_UNPLUG) + { + // Invoke callback before close driver + if (tuh_umount_cb) tuh_umount_cb(dev_addr); + + // Close class driver + for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++) + { + TU_LOG2("%s close\r\n", usbh_class_drivers[drv_id].name); + usbh_class_drivers[drv_id].close(dev_addr); + } + + hcd_device_close(rhport, dev_addr); + + // release all endpoints associated with the device + memset(dev->itf2drv, DRVID_INVALID, sizeof(dev->itf2drv)); // invalid mapping + memset(dev->ep2drv , DRVID_INVALID, sizeof(dev->ep2drv )); // invalid mapping + tu_memclr(dev->ep_status, sizeof(dev->ep_status)); + + dev->state = TUSB_DEVICE_STATE_UNPLUG; + } + } +} + +//--------------------------------------------------------------------+ +// INTERNAL HELPER +//--------------------------------------------------------------------+ +static uint8_t get_new_address(bool is_hub) +{ + uint8_t const start = (is_hub ? CFG_TUH_DEVICE_MAX : 0) + 1; + uint8_t const count = (is_hub ? CFG_TUH_HUB : CFG_TUH_DEVICE_MAX); + + for (uint8_t i=0; i < count; i++) + { + uint8_t const addr = start + i; + if (get_device(addr)->state == TUSB_DEVICE_STATE_UNPLUG) return addr; + } + return ADDR_INVALID; +} + +void usbh_driver_set_config_complete(uint8_t dev_addr, uint8_t itf_num) +{ + usbh_device_t* dev = get_device(dev_addr); + + for(itf_num++; itf_num < sizeof(dev->itf2drv); itf_num++) + { + // continue with next valid interface + // TODO skip IAD binding interface such as CDCs + uint8_t const drv_id = dev->itf2drv[itf_num]; + if (drv_id != DRVID_INVALID) + { + usbh_class_driver_t const * driver = &usbh_class_drivers[drv_id]; + TU_LOG2("%s set config: itf = %u\r\n", driver->name, itf_num); + driver->set_config(dev_addr, itf_num); + break; + } + } + + // all interface are configured + if (itf_num == sizeof(dev->itf2drv)) + { + // Invoke callback if available + if (tuh_mount_cb) tuh_mount_cb(dev_addr); + } +} + +//--------------------------------------------------------------------+ +// Enumeration Process +// is a lengthy process with a seires of control transfer to configure +// newly attached device. Each step is handled by a function in this +// section +// TODO due to the shared _usbh_ctrl_buf, we must complete enumerating +// one device before enumerating another one. +//--------------------------------------------------------------------+ + +static bool enum_request_addr0_device_desc(void); +static bool enum_request_set_addr(void); + +static bool enum_get_addr0_device_desc_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool enum_set_address_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool enum_get_device_desc_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool enum_get_9byte_config_desc_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool enum_get_config_desc_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool enum_set_config_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); +static bool parse_configuration_descriptor (uint8_t dev_addr, tusb_desc_configuration_t const* desc_cfg); + +#if CFG_TUH_HUB +static bool enum_hub_clear_reset0_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) dev_addr; (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + enum_request_addr0_device_desc(); + return true; +} + +static bool enum_hub_clear_reset1_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) dev_addr; (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + enum_request_set_addr(); + + // done with hub, waiting for next data on status pipe + (void) hub_status_pipe_queue( _dev0.hub_addr ); + + return true; +} + +static bool enum_hub_get_status1_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) dev_addr; (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + hub_port_status_response_t port_status; + memcpy(&port_status, _usbh_ctrl_buf, sizeof(hub_port_status_response_t)); + + // Acknowledge Port Reset Change if Reset Successful + if (port_status.change.reset) + { + TU_ASSERT( hub_port_clear_feature(_dev0.hub_addr, _dev0.hub_port, HUB_FEATURE_PORT_RESET_CHANGE, enum_hub_clear_reset1_complete) ); + } + + return true; +} + +static bool enum_hub_get_status0_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) dev_addr; (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + hub_port_status_response_t port_status; + memcpy(&port_status, _usbh_ctrl_buf, sizeof(hub_port_status_response_t)); + + if ( !port_status.status.connection ) + { + // device unplugged while delaying, nothing else to do, queue hub status + return hub_status_pipe_queue(dev_addr); + } + + _dev0.speed = (port_status.status.high_speed) ? TUSB_SPEED_HIGH : + (port_status.status.low_speed ) ? TUSB_SPEED_LOW : TUSB_SPEED_FULL; + + // Acknowledge Port Reset Change + if (port_status.change.reset) + { + hub_port_clear_feature(_dev0.hub_addr, _dev0.hub_port, HUB_FEATURE_PORT_RESET_CHANGE, enum_hub_clear_reset0_complete); + } + + return true; +} +#endif + +static bool enum_new_device(hcd_event_t* event) +{ + _dev0.rhport = event->rhport; // TODO refractor integrate to device_pool + _dev0.hub_addr = event->connection.hub_addr; + _dev0.hub_port = event->connection.hub_port; + + //------------- connected/disconnected directly with roothub -------------// + if (_dev0.hub_addr == 0) + { + // wait until device is stable TODO non blocking + osal_task_delay(RESET_DELAY); + + // device unplugged while delaying + if ( !hcd_port_connect_status(_dev0.rhport) ) return true; + + _dev0.speed = hcd_port_speed_get(_dev0.rhport ); + + enum_request_addr0_device_desc(); + } +#if CFG_TUH_HUB + //------------- connected/disconnected via hub -------------// + else + { + // wait until device is stable + osal_task_delay(RESET_DELAY); + TU_ASSERT( hub_port_get_status(_dev0.hub_addr, _dev0.hub_port, _usbh_ctrl_buf, enum_hub_get_status0_complete) ); + } +#endif // CFG_TUH_HUB + + return true; +} + +static bool enum_request_addr0_device_desc(void) +{ + // TODO probably doesn't need to open/close each enumeration + uint8_t const addr0 = 0; + TU_ASSERT( usbh_edpt_control_open(addr0, 8) ); + + //------------- Get first 8 bytes of device descriptor to get Control Endpoint Size -------------// + TU_LOG2("Get 8 byte of Device Descriptor\r\n"); + tusb_control_request_t const request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_DEVICE, + .type = TUSB_REQ_TYPE_STANDARD, + .direction = TUSB_DIR_IN + }, + .bRequest = TUSB_REQ_GET_DESCRIPTOR, + .wValue = TUSB_DESC_DEVICE << 8, + .wIndex = 0, + .wLength = 8 + }; + TU_ASSERT( tuh_control_xfer(addr0, &request, _usbh_ctrl_buf, enum_get_addr0_device_desc_complete) ); + + return true; +} + +// After Get Device Descriptor of Address 0 +static bool enum_get_addr0_device_desc_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) request; + TU_ASSERT(0 == dev_addr); + + if (XFER_RESULT_SUCCESS != result) + { +#if CFG_TUH_HUB + // TODO remove, waiting for next data on status pipe + if (_dev0.hub_addr != 0) hub_status_pipe_queue(_dev0.hub_addr); +#endif + + return false; + } + + tusb_desc_device_t const * desc_device = (tusb_desc_device_t const*) _usbh_ctrl_buf; + TU_ASSERT( tu_desc_type(desc_device) == TUSB_DESC_DEVICE ); + + // Reset device again before Set Address + TU_LOG2("Port reset \r\n"); + + if (_dev0.hub_addr == 0) + { + // connected directly to roothub + hcd_port_reset( _dev0.rhport ); // reset port after 8 byte descriptor + osal_task_delay(RESET_DELAY); + + enum_request_set_addr(); + } +#if CFG_TUH_HUB + else + { + // after RESET_DELAY the hub_port_reset() already complete + TU_ASSERT( hub_port_reset(_dev0.hub_addr, _dev0.hub_port, NULL) ); + osal_task_delay(RESET_DELAY); + + tuh_task(); // FIXME temporarily to clean up port_reset control transfer + + TU_ASSERT( hub_port_get_status(_dev0.hub_addr, _dev0.hub_port, _usbh_ctrl_buf, enum_hub_get_status1_complete) ); + } +#endif + + return true; +} + +static bool enum_request_set_addr(void) +{ + uint8_t const addr0 = 0; + tusb_desc_device_t const * desc_device = (tusb_desc_device_t const*) _usbh_ctrl_buf; + + // Get new address + uint8_t const new_addr = get_new_address(desc_device->bDeviceClass == TUSB_CLASS_HUB); + TU_ASSERT(new_addr != ADDR_INVALID); + + TU_LOG2("Set Address = %d\r\n", new_addr); + + usbh_device_t* new_dev = get_device(new_addr); + + new_dev->rhport = _dev0.rhport; + new_dev->hub_addr = _dev0.hub_addr; + new_dev->hub_port = _dev0.hub_port; + new_dev->speed = _dev0.speed; + new_dev->connected = 1; + new_dev->ep0_size = desc_device->bMaxPacketSize0; + + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_DEVICE, + .type = TUSB_REQ_TYPE_STANDARD, + .direction = TUSB_DIR_OUT + }, + .bRequest = TUSB_REQ_SET_ADDRESS, + .wValue = new_addr, + .wIndex = 0, + .wLength = 0 + }; + + TU_ASSERT( tuh_control_xfer(addr0, &new_request, NULL, enum_set_address_complete) ); + + return true; +} + +// After SET_ADDRESS is complete +static bool enum_set_address_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + TU_ASSERT(0 == dev_addr); + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + uint8_t const new_addr = (uint8_t const) request->wValue; + + usbh_device_t* new_dev = get_device(new_addr); + new_dev->addressed = 1; + + // TODO close device 0, may not be needed + hcd_device_close(_dev0.rhport, 0); + + // open control pipe for new address + TU_ASSERT( usbh_edpt_control_open(new_addr, new_dev->ep0_size) ); + + // Get full device descriptor + TU_LOG2("Get Device Descriptor\r\n"); + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_DEVICE, + .type = TUSB_REQ_TYPE_STANDARD, + .direction = TUSB_DIR_IN + }, + .bRequest = TUSB_REQ_GET_DESCRIPTOR, + .wValue = TUSB_DESC_DEVICE << 8, + .wIndex = 0, + .wLength = sizeof(tusb_desc_device_t) + }; + + TU_ASSERT(tuh_control_xfer(new_addr, &new_request, _usbh_ctrl_buf, enum_get_device_desc_complete)); + + return true; +} + +static bool enum_get_device_desc_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + tusb_desc_device_t const * desc_device = (tusb_desc_device_t const*) _usbh_ctrl_buf; + usbh_device_t* dev = get_device(dev_addr); + + dev->vid = desc_device->idVendor; + dev->pid = desc_device->idProduct; + dev->i_manufacturer = desc_device->iManufacturer; + dev->i_product = desc_device->iProduct; + dev->i_serial = desc_device->iSerialNumber; + +// if (tuh_attach_cb) tuh_attach_cb((tusb_desc_device_t*) _usbh_ctrl_buf); + + TU_LOG2("Get 9 bytes of Configuration Descriptor\r\n"); + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_DEVICE, + .type = TUSB_REQ_TYPE_STANDARD, + .direction = TUSB_DIR_IN + }, + .bRequest = TUSB_REQ_GET_DESCRIPTOR, + .wValue = (TUSB_DESC_CONFIGURATION << 8) | (CONFIG_NUM - 1), + .wIndex = 0, + .wLength = 9 + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &new_request, _usbh_ctrl_buf, enum_get_9byte_config_desc_complete) ); + + return true; +} + +static bool enum_get_9byte_config_desc_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + // TODO not enough buffer to hold configuration descriptor + uint8_t const * desc_config = _usbh_ctrl_buf; + + // Use offsetof to avoid pointer to the odd/misaligned address + uint16_t const total_len = tu_le16toh( tu_unaligned_read16(desc_config + offsetof(tusb_desc_configuration_t, wTotalLength)) ); + + TU_ASSERT(total_len <= CFG_TUH_ENUMERATION_BUFSIZE); + + // Get full configuration descriptor + TU_LOG2("Get Configuration Descriptor\r\n"); + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_DEVICE, + .type = TUSB_REQ_TYPE_STANDARD, + .direction = TUSB_DIR_IN + }, + .bRequest = TUSB_REQ_GET_DESCRIPTOR, + .wValue = (TUSB_DESC_CONFIGURATION << 8) | (CONFIG_NUM - 1), + .wIndex = 0, + .wLength = total_len + + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &new_request, _usbh_ctrl_buf, enum_get_config_desc_complete) ); + + return true; +} + +static bool enum_get_config_desc_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + // Parse configuration & set up drivers + // Driver open aren't allowed to make any usb transfer yet + TU_ASSERT( parse_configuration_descriptor(dev_addr, (tusb_desc_configuration_t*) _usbh_ctrl_buf) ); + + TU_LOG2("Set Configuration = %d\r\n", CONFIG_NUM); + tusb_control_request_t const new_request = + { + .bmRequestType_bit = + { + .recipient = TUSB_REQ_RCPT_DEVICE, + .type = TUSB_REQ_TYPE_STANDARD, + .direction = TUSB_DIR_OUT + }, + .bRequest = TUSB_REQ_SET_CONFIGURATION, + .wValue = CONFIG_NUM, + .wIndex = 0, + .wLength = 0 + }; + + TU_ASSERT( tuh_control_xfer(dev_addr, &new_request, NULL, enum_set_config_complete) ); + + return true; +} + +static bool enum_set_config_complete(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result) +{ + (void) request; + TU_ASSERT(XFER_RESULT_SUCCESS == result); + + TU_LOG2("Device configured\r\n"); + usbh_device_t* dev = get_device(dev_addr); + dev->configured = 1; + dev->state = TUSB_DEVICE_STATE_CONFIGURED; + + // Start the Set Configuration process for interfaces (itf = DRVID_INVALID) + // Since driver can perform control transfer within its set_config, this is done asynchronously. + // The process continue with next interface when class driver complete its sequence with usbh_driver_set_config_complete() + // TODO use separated API instead of using DRVID_INVALID + usbh_driver_set_config_complete(dev_addr, DRVID_INVALID); + + return true; +} + +static bool parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configuration_t const* desc_cfg) +{ + usbh_device_t* dev = get_device(dev_addr); + + uint8_t const* desc_end = ((uint8_t const*) desc_cfg) + tu_le16toh(desc_cfg->wTotalLength); + uint8_t const* p_desc = tu_desc_next(desc_cfg); + + // parse each interfaces + while( p_desc < desc_end ) + { + uint8_t assoc_itf_count = 1; + + // Class will always starts with Interface Association (if any) and then Interface descriptor + if ( TUSB_DESC_INTERFACE_ASSOCIATION == tu_desc_type(p_desc) ) + { + tusb_desc_interface_assoc_t const * desc_iad = (tusb_desc_interface_assoc_t const *) p_desc; + assoc_itf_count = desc_iad->bInterfaceCount; + + p_desc = tu_desc_next(p_desc); // next to Interface + + // IAD's first interface number and class should match with opened interface + //TU_ASSERT(desc_iad->bFirstInterface == desc_itf->bInterfaceNumber && + // desc_iad->bFunctionClass == desc_itf->bInterfaceClass); + } + + TU_ASSERT( TUSB_DESC_INTERFACE == tu_desc_type(p_desc) ); + tusb_desc_interface_t const* desc_itf = (tusb_desc_interface_t const*) p_desc; + +#if CFG_TUH_MIDI + // MIDI has 2 interfaces (Audio Control v1 + MIDIStreaming) but does not have IAD + // manually increase the associated count + if (1 == assoc_itf_count && + TUSB_CLASS_AUDIO == desc_itf->bInterfaceClass && + AUDIO_SUBCLASS_CONTROL == desc_itf->bInterfaceSubClass && + AUDIO_FUNC_PROTOCOL_CODE_UNDEF == desc_itf->bInterfaceProtocol) + { + assoc_itf_count = 2; + } +#endif + + uint16_t const drv_len = tu_desc_get_interface_total_len(desc_itf, assoc_itf_count, desc_end-p_desc); + TU_ASSERT(drv_len >= sizeof(tusb_desc_interface_t)); + + if (desc_itf->bInterfaceClass == TUSB_CLASS_HUB && dev->hub_addr != 0) + { + // TODO Attach hub to Hub is not currently supported + // skip this interface + TU_LOG(USBH_DBG_LVL, "Only 1 level of HUB is supported\r\n"); + } + else + { + // Find driver for this interface + uint8_t drv_id; + for (drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++) + { + usbh_class_driver_t const * driver = &usbh_class_drivers[drv_id]; + + if ( driver->open(dev->rhport, dev_addr, desc_itf, drv_len) ) + { + // open successfully + TU_LOG2(" %s opened\r\n", driver->name); + + // bind (associated) interfaces to found driver + for(uint8_t i=0; ibInterfaceNumber+i; + + // Interface number must not be used already + TU_ASSERT( DRVID_INVALID == dev->itf2drv[itf_num] ); + dev->itf2drv[itf_num] = drv_id; + } + + // bind all endpoints to found driver + tu_edpt_bind_driver(dev->ep2drv, desc_itf, drv_len, drv_id); + + break; // exit driver find loop + } + } + + if( drv_id >= USBH_CLASS_DRIVER_COUNT ) + { + TU_LOG(USBH_DBG_LVL, "Interface %u: class = %u subclass = %u protocol = %u is not supported\r\n", + desc_itf->bInterfaceNumber, desc_itf->bInterfaceClass, desc_itf->bInterfaceSubClass, desc_itf->bInterfaceProtocol); + } + } + + // next Interface or IAD descriptor + p_desc += drv_len; + } + + return true; +} + +//--------------------------------------------------------------------+ +// Endpoint API +//--------------------------------------------------------------------+ + +// TODO has some duplication code with device, refactor later +bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr) +{ + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + usbh_device_t* dev = get_device(dev_addr); + +#if CFG_TUSB_OS != OPT_OS_NONE + // pre-check to help reducing mutex lock + TU_VERIFY((dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 0)); + osal_mutex_lock(dev->mutex, OSAL_TIMEOUT_WAIT_FOREVER); +#endif + + // can only claim the endpoint if it is not busy and not claimed yet. + bool const ret = (dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 0); + if (ret) + { + dev->ep_status[epnum][dir].claimed = 1; + } + +#if CFG_TUSB_OS != OPT_OS_NONE + osal_mutex_unlock(dev->mutex); +#endif + + return ret; +} + +// TODO has some duplication code with device, refactor later +bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr) +{ + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + usbh_device_t* dev = get_device(dev_addr); + +#if CFG_TUSB_OS != OPT_OS_NONE + osal_mutex_lock(dev->mutex, OSAL_TIMEOUT_WAIT_FOREVER); +#endif + + // can only release the endpoint if it is claimed and not busy + bool const ret = (dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 1); + if (ret) + { + dev->ep_status[epnum][dir].claimed = 0; + } + +#if CFG_TUSB_OS != OPT_OS_NONE + osal_mutex_unlock(dev->mutex); +#endif + + return ret; +} + +// TODO has some duplication code with device, refactor later +bool usbh_edpt_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes) +{ + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + usbh_device_t* dev = get_device(dev_addr); + + TU_LOG2(" Queue EP %02X with %u bytes ... ", ep_addr, total_bytes); + + // Attempt to transfer on a busy endpoint, sound like an race condition ! + TU_ASSERT(dev->ep_status[epnum][dir].busy == 0); + + // Set busy first since the actual transfer can be complete before hcd_edpt_xfer() + // could return and USBH task can preempt and clear the busy + dev->ep_status[epnum][dir].busy = true; + + if ( hcd_edpt_xfer(dev->rhport, dev_addr, ep_addr, buffer, total_bytes) ) + { + TU_LOG2("OK\r\n"); + return true; + }else + { + // HCD error, mark endpoint as ready to allow next transfer + dev->ep_status[epnum][dir].busy = false; + dev->ep_status[epnum][dir].claimed = 0; + TU_LOG2("failed\r\n"); + TU_BREAKPOINT(); + return false; + } +} + +static bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size) +{ + TU_LOG2("Open EP0 with Size = %u (addr = %u)\r\n", max_packet_size, dev_addr); + + tusb_desc_endpoint_t ep0_desc = + { + .bLength = sizeof(tusb_desc_endpoint_t), + .bDescriptorType = TUSB_DESC_ENDPOINT, + .bEndpointAddress = 0, + .bmAttributes = { .xfer = TUSB_XFER_CONTROL }, + .wMaxPacketSize = max_packet_size, + .bInterval = 0 + }; + + return hcd_edpt_open(usbh_get_rhport(dev_addr), dev_addr, &ep0_desc); +} + +bool usbh_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * desc_ep) +{ + usbh_device_t* dev = get_device(dev_addr); + TU_ASSERT(tu_edpt_validate(desc_ep, (tusb_speed_t) dev->speed)); + + return hcd_edpt_open(rhport, dev_addr, desc_ep); +} + +bool usbh_edpt_busy(uint8_t dev_addr, uint8_t ep_addr) +{ + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + usbh_device_t* dev = get_device(dev_addr); + + return dev->ep_status[epnum][dir].busy; +} + + + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh.h new file mode 100644 index 000000000..8411cad28 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh.h @@ -0,0 +1,99 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_USBH_H_ +#define _TUSB_USBH_H_ + +#ifdef __cplusplus + extern "C" { +#endif + +#include "common/tusb_common.h" +#include "hcd.h" + +//--------------------------------------------------------------------+ +// MACRO CONSTANT TYPEDEF +//--------------------------------------------------------------------+ + +typedef bool (*tuh_control_complete_cb_t)(uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result); + +//--------------------------------------------------------------------+ +// APPLICATION API +//--------------------------------------------------------------------+ + +// Init host stack +bool tuh_init(uint8_t rhport); + +// Check if host stack is already initialized +bool tuh_inited(void); + +// Task function should be called in main/rtos loop +void tuh_task(void); + +// Interrupt handler, name alias to HCD +extern void hcd_int_handler(uint8_t rhport); +#define tuh_int_handler hcd_int_handler + +bool tuh_vid_pid_get(uint8_t dev_addr, uint16_t* vid, uint16_t* pid); +tusb_speed_t tuh_speed_get(uint8_t dev_addr); + +// Check if device is connected and configured +bool tuh_mounted(uint8_t dev_addr); + +// Check if device is suspended +static inline bool tuh_suspended(uint8_t dev_addr) +{ + // TODO implement suspend & resume on host + (void) dev_addr; + return false; +} + +// Check if device is ready to communicate with +TU_ATTR_ALWAYS_INLINE +static inline bool tuh_ready(uint8_t dev_addr) +{ + return tuh_mounted(dev_addr) && !tuh_suspended(dev_addr); +} + +// Carry out control transfer +bool tuh_control_xfer (uint8_t dev_addr, tusb_control_request_t const* request, void* buffer, tuh_control_complete_cb_t complete_cb); + +//--------------------------------------------------------------------+ +// APPLICATION CALLBACK +//--------------------------------------------------------------------+ +//TU_ATTR_WEAK uint8_t tuh_attach_cb (tusb_desc_device_t const *desc_device); + +// Invoked when device is mounted (configured) +TU_ATTR_WEAK void tuh_mount_cb (uint8_t dev_addr); + +/// Invoked when device is unmounted (bus reset/unplugged) +TU_ATTR_WEAK void tuh_umount_cb(uint8_t dev_addr); + +#ifdef __cplusplus + } +#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh_classdriver.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh_classdriver.h new file mode 100644 index 000000000..8bc2622aa --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh_classdriver.h @@ -0,0 +1,83 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021, Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_USBH_CLASSDRIVER_H_ +#define _TUSB_USBH_CLASSDRIVER_H_ + +#include "osal/osal.h" +#include "common/tusb_fifo.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Class Driver API +//--------------------------------------------------------------------+ + +typedef struct { + #if CFG_TUSB_DEBUG >= 2 + char const* name; + #endif + + void (* const init )(void); + bool (* const open )(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const * itf_desc, uint16_t max_len); + bool (* const set_config )(uint8_t dev_addr, uint8_t itf_num); + bool (* const xfer_cb )(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes); + void (* const close )(uint8_t dev_addr); +} usbh_class_driver_t; + +// Call by class driver to tell USBH that it has complete the enumeration +void usbh_driver_set_config_complete(uint8_t dev_addr, uint8_t itf_num); + +uint8_t usbh_get_rhport(uint8_t dev_addr); + +uint8_t* usbh_get_enum_buf(void); + +//--------------------------------------------------------------------+ +// USBH Endpoint API +//--------------------------------------------------------------------+ + +// Open an endpoint +bool usbh_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * desc_ep); + +// Submit a usb transfer +bool usbh_edpt_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes); + +// Claim an endpoint before submitting a transfer. +// If caller does not make any transfer, it must release endpoint for others. +bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr); + +bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr); + +// Check if endpoint transferring is complete +bool usbh_edpt_busy(uint8_t dev_addr, uint8_t ep_addr); + +#ifdef __cplusplus + } +#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh_control.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh_control.c new file mode 100644 index 000000000..9204576ac --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/host/usbh_control.c @@ -0,0 +1,138 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020, Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if TUSB_OPT_HOST_ENABLED + +#include "tusb.h" +#include "usbh_classdriver.h" + +enum +{ + STAGE_SETUP, + STAGE_DATA, + STAGE_ACK +}; + +typedef struct +{ + tusb_control_request_t request TU_ATTR_ALIGNED(4); + + uint8_t stage; + uint8_t* buffer; + tuh_control_complete_cb_t complete_cb; +} usbh_control_xfer_t; + +static usbh_control_xfer_t _ctrl_xfer; + +//CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN +//static uint8_t _tuh_ctrl_buf[CFG_TUH_ENUMERATION_BUFSIZE]; + +//--------------------------------------------------------------------+ +// MACRO TYPEDEF CONSTANT ENUM DECLARATION +//--------------------------------------------------------------------+ + +bool tuh_control_xfer (uint8_t dev_addr, tusb_control_request_t const* request, void* buffer, tuh_control_complete_cb_t complete_cb) +{ + // TODO need to claim the endpoint first + const uint8_t rhport = usbh_get_rhport(dev_addr); + + _ctrl_xfer.request = (*request); + _ctrl_xfer.buffer = buffer; + _ctrl_xfer.stage = STAGE_SETUP; + _ctrl_xfer.complete_cb = complete_cb; + + TU_LOG2("Control Setup (addr = %u): ", dev_addr); + TU_LOG2_VAR(request); + TU_LOG2("\r\n"); + + // Send setup packet + TU_ASSERT( hcd_setup_send(rhport, dev_addr, (uint8_t const*) &_ctrl_xfer.request) ); + + return true; +} + +static void _xfer_complete(uint8_t dev_addr, xfer_result_t result) +{ + TU_LOG2("\r\n"); + if (_ctrl_xfer.complete_cb) _ctrl_xfer.complete_cb(dev_addr, &_ctrl_xfer.request, result); +} + +bool usbh_control_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) +{ + (void) ep_addr; + (void) xferred_bytes; + + const uint8_t rhport = usbh_get_rhport(dev_addr); + + tusb_control_request_t const * request = &_ctrl_xfer.request; + + if (XFER_RESULT_SUCCESS != result) + { + TU_LOG2("Control failed: result = %d\r\n", result); + + // terminate transfer if any stage failed + _xfer_complete(dev_addr, result); + }else + { + switch(_ctrl_xfer.stage) + { + case STAGE_SETUP: + _ctrl_xfer.stage = STAGE_DATA; + if (request->wLength) + { + // DATA stage: initial data toggle is always 1 + hcd_edpt_xfer(rhport, dev_addr, tu_edpt_addr(0, request->bmRequestType_bit.direction), _ctrl_xfer.buffer, request->wLength); + return true; + } + __attribute__((fallthrough)); + + case STAGE_DATA: + _ctrl_xfer.stage = STAGE_ACK; + + if (request->wLength) + { + TU_LOG2("Control data (addr = %u):\r\n", dev_addr); + TU_LOG2_MEM(_ctrl_xfer.buffer, request->wLength, 2); + } + + // ACK stage: toggle is always 1 + hcd_edpt_xfer(rhport, dev_addr, tu_edpt_addr(0, 1-request->bmRequestType_bit.direction), NULL, 0); + break; + + case STAGE_ACK: + _xfer_complete(dev_addr, result); + break; + + default: return false; + } + } + + return true; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal.h new file mode 100644 index 000000000..b2943cc79 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal.h @@ -0,0 +1,113 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_OSAL_H_ +#define _TUSB_OSAL_H_ + +#ifdef __cplusplus + extern "C" { +#endif + +/** \addtogroup group_osal + * @{ */ + +#include "common/tusb_common.h" + +// Return immediately +#define OSAL_TIMEOUT_NOTIMEOUT (0) +// Default timeout +#define OSAL_TIMEOUT_NORMAL (10) +// Wait forever +#define OSAL_TIMEOUT_WAIT_FOREVER (UINT32_MAX) + +#define OSAL_TIMEOUT_CONTROL_XFER OSAL_TIMEOUT_WAIT_FOREVER + +typedef void (*osal_task_func_t)( void * ); + +#if CFG_TUSB_OS == OPT_OS_NONE + #include "osal_none.h" +#elif CFG_TUSB_OS == OPT_OS_FREERTOS + #include "osal_freertos.h" +#elif CFG_TUSB_OS == OPT_OS_MYNEWT + #include "osal_mynewt.h" +#elif CFG_TUSB_OS == OPT_OS_PICO + #include "osal_pico.h" +#elif CFG_TUSB_OS == OPT_OS_RTTHREAD + #include "osal_rtthread.h" +#elif CFG_TUSB_OS == OPT_OS_RTX4 + #include "osal_rtx4.h" +#elif CFG_TUSB_OS == OPT_OS_CUSTOM + #include "tusb_os_custom.h" // implemented by application +#else + #error OS is not supported yet +#endif + +//--------------------------------------------------------------------+ +// OSAL Porting API +//--------------------------------------------------------------------+ + +#if __GNUC__ && !defined(__ARMCC_VERSION) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wredundant-decls" +#endif +//------------- Semaphore -------------// +static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef); +static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr); +static inline bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec); + +static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl); // TODO removed + +//------------- Mutex -------------// +static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef); +static inline bool osal_mutex_lock (osal_mutex_t sem_hdl, uint32_t msec); +static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl); + +//------------- Queue -------------// +static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef); +static inline bool osal_queue_receive(osal_queue_t qhdl, void* data); +static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr); +static inline bool osal_queue_empty(osal_queue_t qhdl); +#if __GNUC__ && !defined(__ARMCC_VERSION) +#pragma GCC diagnostic pop +#endif + +#if 0 // TODO remove subtask related macros later +// Sub Task +#define OSAL_SUBTASK_BEGIN +#define OSAL_SUBTASK_END return TUSB_ERROR_NONE; + +#define STASK_RETURN(_error) return _error; +#define STASK_INVOKE(_subtask, _status) (_status) = _subtask +#define STASK_ASSERT(_cond) TU_VERIFY(_cond, TUSB_ERROR_OSAL_TASK_FAILED) +#endif + +#ifdef __cplusplus + } +#endif + +/** @} */ + +#endif /* _TUSB_OSAL_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_freertos.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_freertos.h new file mode 100644 index 000000000..aa102b15c --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_freertos.h @@ -0,0 +1,174 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_OSAL_FREERTOS_H_ +#define _TUSB_OSAL_FREERTOS_H_ + +// FreeRTOS Headers +#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,FreeRTOS.h) +#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,semphr.h) +#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,queue.h) +#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,task.h) + +#ifdef __cplusplus +extern "C" { +#endif + +//--------------------------------------------------------------------+ +// TASK API +//--------------------------------------------------------------------+ +static inline void osal_task_delay(uint32_t msec) +{ + vTaskDelay( pdMS_TO_TICKS(msec) ); +} + +//--------------------------------------------------------------------+ +// Semaphore API +//--------------------------------------------------------------------+ +typedef StaticSemaphore_t osal_semaphore_def_t; +typedef SemaphoreHandle_t osal_semaphore_t; + +static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef) +{ + return xSemaphoreCreateBinaryStatic(semdef); +} + +static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) +{ + if ( !in_isr ) + { + return xSemaphoreGive(sem_hdl) != 0; + } + else + { + BaseType_t xHigherPriorityTaskWoken; + BaseType_t res = xSemaphoreGiveFromISR(sem_hdl, &xHigherPriorityTaskWoken); + +#if CFG_TUSB_MCU == OPT_MCU_ESP32S2 || CFG_TUSB_MCU == OPT_MCU_ESP32S3 + // not needed after https://github.com/espressif/esp-idf/commit/c5fd79547ac9b7bae06fa660e9f814d18d3390b7 + if ( xHigherPriorityTaskWoken ) portYIELD_FROM_ISR(); +#else + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +#endif + + return res != 0; + } +} + +static inline bool osal_semaphore_wait (osal_semaphore_t sem_hdl, uint32_t msec) +{ + uint32_t const ticks = (msec == OSAL_TIMEOUT_WAIT_FOREVER) ? portMAX_DELAY : pdMS_TO_TICKS(msec); + return xSemaphoreTake(sem_hdl, ticks); +} + +static inline void osal_semaphore_reset(osal_semaphore_t const sem_hdl) +{ + xQueueReset(sem_hdl); +} + +//--------------------------------------------------------------------+ +// MUTEX API (priority inheritance) +//--------------------------------------------------------------------+ +typedef StaticSemaphore_t osal_mutex_def_t; +typedef SemaphoreHandle_t osal_mutex_t; + +static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) +{ + return xSemaphoreCreateMutexStatic(mdef); +} + +static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec) +{ + return osal_semaphore_wait(mutex_hdl, msec); +} + +static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) +{ + return xSemaphoreGive(mutex_hdl); +} + +//--------------------------------------------------------------------+ +// QUEUE API +//--------------------------------------------------------------------+ + +// role device/host is used by OS NONE for mutex (disable usb isr) only +#define OSAL_QUEUE_DEF(_role, _name, _depth, _type) \ + static _type _name##_##buf[_depth];\ + osal_queue_def_t _name = { .depth = _depth, .item_sz = sizeof(_type), .buf = _name##_##buf }; + +typedef struct +{ + uint16_t depth; + uint16_t item_sz; + void* buf; + + StaticQueue_t sq; +}osal_queue_def_t; + +typedef QueueHandle_t osal_queue_t; + +static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) +{ + return xQueueCreateStatic(qdef->depth, qdef->item_sz, (uint8_t*) qdef->buf, &qdef->sq); +} + +static inline bool osal_queue_receive(osal_queue_t qhdl, void* data) +{ + return xQueueReceive(qhdl, data, portMAX_DELAY); +} + +static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr) +{ + if ( !in_isr ) + { + return xQueueSendToBack(qhdl, data, OSAL_TIMEOUT_WAIT_FOREVER) != 0; + } + else + { + BaseType_t xHigherPriorityTaskWoken; + BaseType_t res = xQueueSendToBackFromISR(qhdl, data, &xHigherPriorityTaskWoken); + +#if CFG_TUSB_MCU == OPT_MCU_ESP32S2 || CFG_TUSB_MCU == OPT_MCU_ESP32S3 + // not needed after https://github.com/espressif/esp-idf/commit/c5fd79547ac9b7bae06fa660e9f814d18d3390b7 + if ( xHigherPriorityTaskWoken ) portYIELD_FROM_ISR(); +#else + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +#endif + + return res != 0; + } +} + +static inline bool osal_queue_empty(osal_queue_t qhdl) +{ + return uxQueueMessagesWaiting(qhdl) == 0; +} + +#ifdef __cplusplus + } +#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_mynewt.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_mynewt.h new file mode 100644 index 000000000..6882329c1 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_mynewt.h @@ -0,0 +1,174 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef OSAL_MYNEWT_H_ +#define OSAL_MYNEWT_H_ + +#include "os/os.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// TASK API +//--------------------------------------------------------------------+ +static inline void osal_task_delay(uint32_t msec) +{ + os_time_delay( os_time_ms_to_ticks32(msec) ); +} + +//--------------------------------------------------------------------+ +// Semaphore API +//--------------------------------------------------------------------+ +typedef struct os_sem osal_semaphore_def_t; +typedef struct os_sem* osal_semaphore_t; + +static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef) +{ + return (os_sem_init(semdef, 0) == OS_OK) ? (osal_semaphore_t) semdef : NULL; +} + +static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) +{ + (void) in_isr; + return os_sem_release(sem_hdl) == OS_OK; +} + +static inline bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec) +{ + uint32_t const ticks = (msec == OSAL_TIMEOUT_WAIT_FOREVER) ? OS_TIMEOUT_NEVER : os_time_ms_to_ticks32(msec); + return os_sem_pend(sem_hdl, ticks) == OS_OK; +} + +static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl) +{ + // TODO implement later +} + +//--------------------------------------------------------------------+ +// MUTEX API (priority inheritance) +//--------------------------------------------------------------------+ +typedef struct os_mutex osal_mutex_def_t; +typedef struct os_mutex* osal_mutex_t; + +static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) +{ + return (os_mutex_init(mdef) == OS_OK) ? (osal_mutex_t) mdef : NULL; +} + +static inline bool osal_mutex_lock(osal_mutex_t mutex_hdl, uint32_t msec) +{ + uint32_t const ticks = (msec == OSAL_TIMEOUT_WAIT_FOREVER) ? OS_TIMEOUT_NEVER : os_time_ms_to_ticks32(msec); + return os_mutex_pend(mutex_hdl, ticks) == OS_OK; +} + +static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) +{ + return os_mutex_release(mutex_hdl) == OS_OK; +} + +//--------------------------------------------------------------------+ +// QUEUE API +//--------------------------------------------------------------------+ + +// role device/host is used by OS NONE for mutex (disable usb isr) only +#define OSAL_QUEUE_DEF(_role, _name, _depth, _type) \ + static _type _name##_##buf[_depth];\ + static struct os_event _name##_##evbuf[_depth];\ + osal_queue_def_t _name = { .depth = _depth, .item_sz = sizeof(_type), .buf = _name##_##buf, .evbuf = _name##_##evbuf};\ + +typedef struct +{ + uint16_t depth; + uint16_t item_sz; + void* buf; + void* evbuf; + + struct os_mempool mpool; + struct os_mempool epool; + + struct os_eventq evq; +}osal_queue_def_t; + +typedef osal_queue_def_t* osal_queue_t; + +static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) +{ + if ( OS_OK != os_mempool_init(&qdef->mpool, qdef->depth, qdef->item_sz, qdef->buf, "usbd queue") ) return NULL; + if ( OS_OK != os_mempool_init(&qdef->epool, qdef->depth, sizeof(struct os_event), qdef->evbuf, "usbd evqueue") ) return NULL; + + os_eventq_init(&qdef->evq); + return (osal_queue_t) qdef; +} + +static inline bool osal_queue_receive(osal_queue_t qhdl, void* data) +{ + struct os_event* ev; + ev = os_eventq_get(&qhdl->evq); + + memcpy(data, ev->ev_arg, qhdl->item_sz); // copy message + os_memblock_put(&qhdl->mpool, ev->ev_arg); // put back mem block + os_memblock_put(&qhdl->epool, ev); // put back ev block + + return true; +} + +static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr) +{ + (void) in_isr; + + // get a block from mem pool for data + void* ptr = os_memblock_get(&qhdl->mpool); + if (!ptr) return false; + memcpy(ptr, data, qhdl->item_sz); + + // get a block from event pool to put into queue + struct os_event* ev = (struct os_event*) os_memblock_get(&qhdl->epool); + if (!ev) + { + os_memblock_put(&qhdl->mpool, ptr); + return false; + } + tu_memclr(ev, sizeof(struct os_event)); + ev->ev_arg = ptr; + + os_eventq_put(&qhdl->evq, ev); + + return true; +} + +static inline bool osal_queue_empty(osal_queue_t qhdl) +{ + return STAILQ_EMPTY(&qhdl->evq.evq_list); +} + + +#ifdef __cplusplus + } +#endif + +#endif /* OSAL_MYNEWT_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_none.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_none.h new file mode 100644 index 000000000..a1f997cf2 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_none.h @@ -0,0 +1,204 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_OSAL_NONE_H_ +#define _TUSB_OSAL_NONE_H_ + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// TASK API +//--------------------------------------------------------------------+ + + +//--------------------------------------------------------------------+ +// Binary Semaphore API +//--------------------------------------------------------------------+ +typedef struct +{ + volatile uint16_t count; +}osal_semaphore_def_t; + +typedef osal_semaphore_def_t* osal_semaphore_t; + +static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef) +{ + semdef->count = 0; + return semdef; +} + +static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) +{ + (void) in_isr; + sem_hdl->count++; + return true; +} + +// TODO blocking for now +static inline bool osal_semaphore_wait (osal_semaphore_t sem_hdl, uint32_t msec) +{ + (void) msec; + + while (sem_hdl->count == 0) { } + sem_hdl->count--; + + return true; +} + +static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl) +{ + sem_hdl->count = 0; +} + +//--------------------------------------------------------------------+ +// MUTEX API +// Within tinyusb, mutex is never used in ISR context +//--------------------------------------------------------------------+ +typedef osal_semaphore_def_t osal_mutex_def_t; +typedef osal_semaphore_t osal_mutex_t; + +static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) +{ + mdef->count = 1; + return mdef; +} + +static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec) +{ + return osal_semaphore_wait(mutex_hdl, msec); +} + +static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) +{ + return osal_semaphore_post(mutex_hdl, false); +} + +//--------------------------------------------------------------------+ +// QUEUE API +//--------------------------------------------------------------------+ +#include "common/tusb_fifo.h" + +// extern to avoid including dcd.h and hcd.h +#if TUSB_OPT_DEVICE_ENABLED +extern void dcd_int_disable(uint8_t rhport); +extern void dcd_int_enable(uint8_t rhport); +#endif + +#if TUSB_OPT_HOST_ENABLED +extern void hcd_int_disable(uint8_t rhport); +extern void hcd_int_enable(uint8_t rhport); +#endif + +typedef struct +{ + uint8_t role; // device or host + tu_fifo_t ff; +}osal_queue_def_t; + +typedef osal_queue_def_t* osal_queue_t; + +// role device/host is used by OS NONE for mutex (disable usb isr) only +#define OSAL_QUEUE_DEF(_role, _name, _depth, _type) \ + uint8_t _name##_buf[_depth*sizeof(_type)]; \ + osal_queue_def_t _name = { \ + .role = _role, \ + .ff = TU_FIFO_INIT(_name##_buf, _depth, _type, false) \ + } + +// lock queue by disable USB interrupt +static inline void _osal_q_lock(osal_queue_t qhdl) +{ + (void) qhdl; + +#if TUSB_OPT_DEVICE_ENABLED + if (qhdl->role == OPT_MODE_DEVICE) dcd_int_disable(TUD_OPT_RHPORT); +#endif + +#if TUSB_OPT_HOST_ENABLED + if (qhdl->role == OPT_MODE_HOST) hcd_int_disable(TUH_OPT_RHPORT); +#endif +} + +// unlock queue +static inline void _osal_q_unlock(osal_queue_t qhdl) +{ + (void) qhdl; + +#if TUSB_OPT_DEVICE_ENABLED + if (qhdl->role == OPT_MODE_DEVICE) dcd_int_enable(TUD_OPT_RHPORT); +#endif + +#if TUSB_OPT_HOST_ENABLED + if (qhdl->role == OPT_MODE_HOST) hcd_int_enable(TUH_OPT_RHPORT); +#endif +} + +static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) +{ + tu_fifo_clear(&qdef->ff); + return (osal_queue_t) qdef; +} + +static inline bool osal_queue_receive(osal_queue_t qhdl, void* data) +{ + _osal_q_lock(qhdl); + bool success = tu_fifo_read(&qhdl->ff, data); + _osal_q_unlock(qhdl); + + return success; +} + +static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr) +{ + if (!in_isr) { + _osal_q_lock(qhdl); + } + + bool success = tu_fifo_write(&qhdl->ff, data); + + if (!in_isr) { + _osal_q_unlock(qhdl); + } + + TU_ASSERT(success); + + return success; +} + +static inline bool osal_queue_empty(osal_queue_t qhdl) +{ + // Skip queue lock/unlock since this function is primarily called + // with interrupt disabled before going into low power mode + return tu_fifo_empty(&qhdl->ff); +} + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_OSAL_NONE_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_pico.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_pico.h new file mode 100644 index 000000000..1c3366e01 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_pico.h @@ -0,0 +1,187 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 Raspberry Pi (Trading) Ltd. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_OSAL_PICO_H_ +#define _TUSB_OSAL_PICO_H_ + +#include "pico/time.h" +#include "pico/sem.h" +#include "pico/mutex.h" +#include "pico/critical_section.h" + +#ifdef __cplusplus + extern "C" { +#endif + +//--------------------------------------------------------------------+ +// TASK API +//--------------------------------------------------------------------+ +static inline void osal_task_delay(uint32_t msec) +{ + sleep_ms(msec); +} + +//--------------------------------------------------------------------+ +// Binary Semaphore API +//--------------------------------------------------------------------+ +typedef struct semaphore osal_semaphore_def_t, *osal_semaphore_t; + +static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef) +{ + sem_init(semdef, 0, 255); + return semdef; +} + +static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) +{ + (void) in_isr; + sem_release(sem_hdl); + return true; +} + +static inline bool osal_semaphore_wait (osal_semaphore_t sem_hdl, uint32_t msec) +{ + return sem_acquire_timeout_ms(sem_hdl, msec); +} + +static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl) +{ + sem_reset(sem_hdl, 0); +} + +//--------------------------------------------------------------------+ +// MUTEX API +// Within tinyusb, mutex is never used in ISR context +//--------------------------------------------------------------------+ +typedef struct mutex osal_mutex_def_t, *osal_mutex_t; + +static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) +{ + mutex_init(mdef); + return mdef; +} + +static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec) +{ + return mutex_enter_timeout_ms(mutex_hdl, msec); +} + +static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) +{ + mutex_exit(mutex_hdl); + return true; +} + +//--------------------------------------------------------------------+ +// QUEUE API +//--------------------------------------------------------------------+ +#include "common/tusb_fifo.h" + +#if TUSB_OPT_HOST_ENABLED +extern void hcd_int_disable(uint8_t rhport); +extern void hcd_int_enable(uint8_t rhport); +#endif + +typedef struct +{ + tu_fifo_t ff; + struct critical_section critsec; // osal_queue may be used in IRQs, so need critical section +} osal_queue_def_t; + +typedef osal_queue_def_t* osal_queue_t; + +// role device/host is used by OS NONE for mutex (disable usb isr) only +#define OSAL_QUEUE_DEF(_role, _name, _depth, _type) \ + uint8_t _name##_buf[_depth*sizeof(_type)]; \ + osal_queue_def_t _name = { \ + .ff = TU_FIFO_INIT(_name##_buf, _depth, _type, false) \ + } + +// lock queue by disable USB interrupt +static inline void _osal_q_lock(osal_queue_t qhdl) +{ + critical_section_enter_blocking(&qhdl->critsec); +} + +// unlock queue +static inline void _osal_q_unlock(osal_queue_t qhdl) +{ + critical_section_exit(&qhdl->critsec); +} + +static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) +{ + critical_section_init(&qdef->critsec); + tu_fifo_clear(&qdef->ff); + return (osal_queue_t) qdef; +} + +static inline bool osal_queue_receive(osal_queue_t qhdl, void* data) +{ + // TODO: revisit... docs say that mutexes are never used from IRQ context, + // however osal_queue_recieve may be. therefore my assumption is that + // the fifo mutex is not populated for queues used from an IRQ context + //assert(!qhdl->ff.mutex); + + _osal_q_lock(qhdl); + bool success = tu_fifo_read(&qhdl->ff, data); + _osal_q_unlock(qhdl); + + return success; +} + +static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr) +{ + // TODO: revisit... docs say that mutexes are never used from IRQ context, + // however osal_queue_recieve may be. therefore my assumption is that + // the fifo mutex is not populated for queues used from an IRQ context + //assert(!qhdl->ff.mutex); + (void) in_isr; + + _osal_q_lock(qhdl); + bool success = tu_fifo_write(&qhdl->ff, data); + _osal_q_unlock(qhdl); + + TU_ASSERT(success); + + return success; +} + +static inline bool osal_queue_empty(osal_queue_t qhdl) +{ + // TODO: revisit; whether this is true or not currently, tu_fifo_empty is a single + // volatile read. + + // Skip queue lock/unlock since this function is primarily called + // with interrupt disabled before going into low power mode + return tu_fifo_empty(&qhdl->ff); +} + +#ifdef __cplusplus + } +#endif + +#endif /* _TUSB_OSAL_PICO_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_rtthread.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_rtthread.h new file mode 100644 index 000000000..d5c062ac1 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_rtthread.h @@ -0,0 +1,130 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2020 tfx2001 (2479727366@qq.com) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_OSAL_RTTHREAD_H_ +#define _TUSB_OSAL_RTTHREAD_H_ + +// RT-Thread Headers +#include "rtthread.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//--------------------------------------------------------------------+ +// TASK API +//--------------------------------------------------------------------+ +static inline void osal_task_delay(uint32_t msec) { + rt_thread_mdelay(msec); +} + +//--------------------------------------------------------------------+ +// Semaphore API +//--------------------------------------------------------------------+ +typedef struct rt_semaphore osal_semaphore_def_t; +typedef rt_sem_t osal_semaphore_t; + +static inline osal_semaphore_t +osal_semaphore_create(osal_semaphore_def_t *semdef) { + rt_sem_init(semdef, "tusb", 0, RT_IPC_FLAG_FIFO); + return semdef; +} + +static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) { + (void) in_isr; + return rt_sem_release(sem_hdl) == RT_EOK; +} + +static inline bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec) { + return rt_sem_take(sem_hdl, rt_tick_from_millisecond(msec)) == RT_EOK; +} + +static inline void osal_semaphore_reset(osal_semaphore_t const sem_hdl) { + // TODO: implement +} + +//--------------------------------------------------------------------+ +// MUTEX API (priority inheritance) +//--------------------------------------------------------------------+ +typedef struct rt_mutex osal_mutex_def_t; +typedef rt_mutex_t osal_mutex_t; + +static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t *mdef) { + rt_mutex_init(mdef, "tusb", RT_IPC_FLAG_FIFO); + return mdef; +} + +static inline bool osal_mutex_lock(osal_mutex_t mutex_hdl, uint32_t msec) { + return rt_mutex_take(mutex_hdl, rt_tick_from_millisecond(msec)) == RT_EOK; +} + +static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) { + return rt_mutex_release(mutex_hdl) == RT_EOK; +} + +//--------------------------------------------------------------------+ +// QUEUE API +//--------------------------------------------------------------------+ + +// role device/host is used by OS NONE for mutex (disable usb isr) only +#define OSAL_QUEUE_DEF(_role, _name, _depth, _type) \ + static _type _name##_##buf[_depth]; \ + osal_queue_def_t _name = { .depth = _depth, .item_sz = sizeof(_type), .buf = _name##_##buf }; + +typedef struct { + uint16_t depth; + uint16_t item_sz; + void *buf; + + struct rt_messagequeue sq; +} osal_queue_def_t; + +typedef rt_mq_t osal_queue_t; + +static inline osal_queue_t osal_queue_create(osal_queue_def_t *qdef) { + rt_mq_init(&(qdef->sq), "tusb", qdef->buf, qdef->item_sz, + qdef->item_sz * qdef->depth, RT_IPC_FLAG_FIFO); + return &(qdef->sq); +} + +static inline bool osal_queue_receive(osal_queue_t qhdl, void *data) { + return rt_mq_recv(qhdl, data, qhdl->msg_size, RT_WAITING_FOREVER) == RT_EOK; +} + +static inline bool osal_queue_send(osal_queue_t qhdl, void const *data, bool in_isr) { + (void) in_isr; + return rt_mq_send(qhdl, (void *)data, qhdl->msg_size) == RT_EOK; +} + +static inline bool osal_queue_empty(osal_queue_t qhdl) { + return (qhdl->entry) == 0; +} + +#ifdef __cplusplus +} +#endif + +#endif /* _TUSB_OSAL_RTTHREAD_H_ */ diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_rtx4.h b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_rtx4.h new file mode 100644 index 000000000..f7e88e322 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/osal/osal_rtx4.h @@ -0,0 +1,170 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2021 Tian Yunhao (t123yh) + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#ifndef _TUSB_OSAL_RTX4_H_ +#define _TUSB_OSAL_RTX4_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//--------------------------------------------------------------------+ +// TASK API +//--------------------------------------------------------------------+ +static inline void osal_task_delay(uint32_t msec) +{ + uint16_t hi = msec >> 16; + uint16_t lo = msec; + while (hi--) { + os_dly_wait(0xFFFE); + } + os_dly_wait(lo); +} + +static inline uint16_t msec2wait(uint32_t msec) { + if (msec == OSAL_TIMEOUT_WAIT_FOREVER) + return 0xFFFF; + else if (msec >= 0xFFFE) + return 0xFFFE; + else + return msec; +} + +//--------------------------------------------------------------------+ +// Semaphore API +//--------------------------------------------------------------------+ +typedef OS_SEM osal_semaphore_def_t; +typedef OS_ID osal_semaphore_t; + +static inline OS_ID osal_semaphore_create(osal_semaphore_def_t* semdef) { + os_sem_init(semdef, 0); + return semdef; +} + +static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) { + if ( !in_isr ) { + os_sem_send(sem_hdl); + } else { + isr_sem_send(sem_hdl); + } + return true; +} + +static inline bool osal_semaphore_wait (osal_semaphore_t sem_hdl, uint32_t msec) { + return os_sem_wait(sem_hdl, msec2wait(msec)) != OS_R_TMO; +} + +static inline void osal_semaphore_reset(osal_semaphore_t const sem_hdl) { + // TODO: implement +} + +//--------------------------------------------------------------------+ +// MUTEX API (priority inheritance) +//--------------------------------------------------------------------+ +typedef OS_MUT osal_mutex_def_t; +typedef OS_ID osal_mutex_t; + +static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) +{ + os_mut_init(mdef); + return mdef; +} + +static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec) +{ + return os_mut_wait(mutex_hdl, msec2wait(msec)) != OS_R_TMO; +} + +static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) +{ + return os_mut_release(mutex_hdl) == OS_R_OK; +} + +//--------------------------------------------------------------------+ +// QUEUE API +//--------------------------------------------------------------------+ + +// role device/host is used by OS NONE for mutex (disable usb isr) only +#define OSAL_QUEUE_DEF(_role, _name, _depth, _type) \ + os_mbx_declare(_name##__mbox, _depth); \ + _declare_box(_name##__pool, sizeof(_type), _depth); \ + osal_queue_def_t _name = { .depth = _depth, .item_sz = sizeof(_type), .pool = _name##__pool, .mbox = _name##__mbox }; + + +typedef struct +{ + uint16_t depth; + uint16_t item_sz; + U32* pool; + U32* mbox; +}osal_queue_def_t; + +typedef osal_queue_def_t* osal_queue_t; + +static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) +{ + os_mbx_init(qdef->mbox, (qdef->depth + 4) * 4); + _init_box(qdef->pool, ((qdef->item_sz+3)/4)*(qdef->depth) + 3, qdef->item_sz); + return qdef; +} + +static inline bool osal_queue_receive(osal_queue_t qhdl, void* data) +{ + void* buf; + os_mbx_wait(qhdl->mbox, &buf, 0xFFFF); + memcpy(data, buf, qhdl->item_sz); + _free_box(qhdl->pool, buf); + return true; +} + +static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr) +{ + void* buf = _alloc_box(qhdl->pool); + memcpy(buf, data, qhdl->item_sz); + if ( !in_isr ) + { + os_mbx_send(qhdl->mbox, buf, 0xFFFF); + } + else + { + isr_mbx_send(qhdl->mbox, buf); + } + return true; +} + +static inline bool osal_queue_empty(osal_queue_t qhdl) +{ + return os_mbx_check(qhdl->mbox) == qhdl->depth; +} + +#ifdef __cplusplus + } +#endif + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/portable/espressif/esp32sx/dcd_esp32sx.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/portable/espressif/esp32sx/dcd_esp32sx.c new file mode 100644 index 000000000..a9c311324 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/portable/espressif/esp32sx/dcd_esp32sx.c @@ -0,0 +1,854 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2018 Scott Shawcroft, 2019 William D. Jones for Adafruit Industries + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * Additions Copyright (c) 2020, Espressif Systems (Shanghai) Co. Ltd. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if (((CFG_TUSB_MCU == OPT_MCU_ESP32S2) || (CFG_TUSB_MCU == OPT_MCU_ESP32S3)) && TUSB_OPT_DEVICE_ENABLED) + +// Espressif +#include "freertos/xtensa_api.h" +#include "esp_intr_alloc.h" +#include "esp_log.h" +#include "driver/gpio.h" +#include "soc/dport_reg.h" +#include "soc/gpio_sig_map.h" +#include "soc/usb_periph.h" +#include "soc/periph_defs.h" // for interrupt source + +#include "device/dcd.h" + +// Max number of bi-directional endpoints including EP0 +// Note: ESP32S2 specs say there are only up to 5 IN active endpoints include EP0 +// We should probably prohibit enabling Endpoint IN > 4 (not done yet) +#define EP_MAX USB_OUT_EP_NUM + +// FIFO size in bytes +#define EP_FIFO_SIZE 1024 + +// Max number of IN EP FIFOs +#define EP_FIFO_NUM 5 + +typedef struct { + uint8_t *buffer; + // tu_fifo_t * ff; // TODO support dcd_edpt_xfer_fifo API + uint16_t total_len; + uint16_t queued_len; + uint16_t max_size; + bool short_packet; +} xfer_ctl_t; + +static const char *TAG = "TUSB:DCD"; +static intr_handle_t usb_ih; + + +static uint32_t _setup_packet[2]; + +#define XFER_CTL_BASE(_ep, _dir) &xfer_status[_ep][_dir] +static xfer_ctl_t xfer_status[EP_MAX][2]; + +// Keep count of how many FIFOs are in use +static uint8_t _allocated_fifos = 1; //FIFO0 is always in use + +// Will either return an unused FIFO number, or 0 if all are used. +static uint8_t get_free_fifo(void) +{ + if (_allocated_fifos < EP_FIFO_NUM) return _allocated_fifos++; + return 0; +} + +// Setup the control endpoint 0. +static void bus_reset(void) +{ + for (int ep_num = 0; ep_num < USB_OUT_EP_NUM; ep_num++) { + USB0.out_ep_reg[ep_num].doepctl |= USB_DO_SNAK0_M; // DOEPCTL0_SNAK + } + + // clear device address + USB0.dcfg &= ~USB_DEVADDR_M; + + USB0.daintmsk = USB_OUTEPMSK0_M | USB_INEPMSK0_M; + USB0.doepmsk = USB_SETUPMSK_M | USB_XFERCOMPLMSK; + USB0.diepmsk = USB_TIMEOUTMSK_M | USB_DI_XFERCOMPLMSK_M /*| USB_INTKNTXFEMPMSK_M*/; + + // "USB Data FIFOs" section in reference manual + // Peripheral FIFO architecture + // + // --------------- 320 or 1024 ( 1280 or 4096 bytes ) + // | IN FIFO MAX | + // --------------- + // | ... | + // --------------- y + x + 16 + GRXFSIZ + // | IN FIFO 2 | + // --------------- x + 16 + GRXFSIZ + // | IN FIFO 1 | + // --------------- 16 + GRXFSIZ + // | IN FIFO 0 | + // --------------- GRXFSIZ + // | OUT FIFO | + // | ( Shared ) | + // --------------- 0 + // + // According to "FIFO RAM allocation" section in RM, FIFO RAM are allocated as follows (each word 32-bits): + // - Each EP IN needs at least max packet size, 16 words is sufficient for EP0 IN + // + // - All EP OUT shared a unique OUT FIFO which uses + // * 10 locations in hardware for setup packets + setup control words (up to 3 setup packets). + // * 2 locations for OUT endpoint control words. + // * 16 for largest packet size of 64 bytes. ( TODO Highspeed is 512 bytes) + // * 1 location for global NAK (not required/used here). + // * It is recommended to allocate 2 times the largest packet size, therefore + // Recommended value = 10 + 1 + 2 x (16+2) = 47 --> Let's make it 52 + USB0.grstctl |= 0x10 << USB_TXFNUM_S; // fifo 0x10, + USB0.grstctl |= USB_TXFFLSH_M; // Flush fifo + USB0.grxfsiz = 52; + + // Control IN uses FIFO 0 with 64 bytes ( 16 32-bit word ) + USB0.gnptxfsiz = (16 << USB_NPTXFDEP_S) | (USB0.grxfsiz & 0x0000ffffUL); + + // Ready to receive SETUP packet + USB0.out_ep_reg[0].doeptsiz |= USB_SUPCNT0_M; + + USB0.gintmsk |= USB_IEPINTMSK_M | USB_OEPINTMSK_M; +} + +static void enum_done_processing(void) +{ + ESP_EARLY_LOGV(TAG, "dcd_int_handler - Speed enumeration done! Sending DCD_EVENT_BUS_RESET then"); + // On current silicon on the Full Speed core, speed is fixed to Full Speed. + // However, keep for debugging and in case Low Speed is ever supported. + uint32_t enum_spd = (USB0.dsts >> USB_ENUMSPD_S) & (USB_ENUMSPD_V); + + // Maximum packet size for EP 0 is set for both directions by writing DIEPCTL + if (enum_spd == 0x03) { // Full-Speed (PHY on 48 MHz) + USB0.in_ep_reg[0].diepctl &= ~USB_D_MPS0_V; // 64 bytes + USB0.in_ep_reg[0].diepctl &= ~USB_D_STALL0_M; // clear Stall + xfer_status[0][TUSB_DIR_OUT].max_size = 64; + xfer_status[0][TUSB_DIR_IN].max_size = 64; + } else { + USB0.in_ep_reg[0].diepctl |= USB_D_MPS0_V; // 8 bytes + USB0.in_ep_reg[0].diepctl &= ~USB_D_STALL0_M; // clear Stall + xfer_status[0][TUSB_DIR_OUT].max_size = 8; + xfer_status[0][TUSB_DIR_IN].max_size = 8; + } +} + + +/*------------------------------------------------------------------*/ +/* Controller API + *------------------------------------------------------------------*/ +void dcd_init(uint8_t rhport) +{ + ESP_LOGV(TAG, "DCD init - Start"); + + // A. Disconnect + ESP_LOGV(TAG, "DCD init - Soft DISCONNECT and Setting up"); + USB0.dctl |= USB_SFTDISCON_M; // Soft disconnect + + // B. Programming DCFG + /* If USB host misbehaves during status portion of control xfer + (non zero-length packet), send STALL back and discard. Full speed. */ + USB0.dcfg |= USB_NZSTSOUTHSHK_M | // NonZero .... STALL + (3 << 0); // dev speed: fullspeed 1.1 on 48 mhz // TODO no value in usb_reg.h (IDF-1476) + + USB0.gahbcfg |= USB_NPTXFEMPLVL_M | USB_GLBLLNTRMSK_M; // Global interruptions ON + USB0.gusbcfg |= USB_FORCEDEVMODE_M; // force devmode + USB0.gotgctl &= ~(USB_BVALIDOVVAL_M | USB_BVALIDOVEN_M | USB_VBVALIDOVVAL_M); //no overrides + + // C. Setting SNAKs, then connect + for (int n = 0; n < USB_OUT_EP_NUM; n++) { + USB0.out_ep_reg[n].doepctl |= USB_DO_SNAK0_M; // DOEPCTL0_SNAK + } + + // D. Interruption masking + USB0.gintmsk = 0; //mask all + USB0.gotgint = ~0U; //clear OTG ints + USB0.gintsts = ~0U; //clear pending ints + USB0.gintmsk = USB_OTGINTMSK_M | + USB_MODEMISMSK_M | + USB_RXFLVIMSK_M | + USB_ERLYSUSPMSK_M | + USB_USBSUSPMSK_M | + USB_USBRSTMSK_M | + USB_ENUMDONEMSK_M | + USB_RESETDETMSK_M | + USB_DISCONNINTMSK_M; // host most only + + dcd_connect(rhport); +} + +void dcd_set_address(uint8_t rhport, uint8_t dev_addr) +{ + (void)rhport; + ESP_LOGV(TAG, "DCD init - Set address : %u", dev_addr); + USB0.dcfg |= ((dev_addr & USB_DEVADDR_V) << USB_DEVADDR_S); + // Response with status after changing device address + dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0); +} + +void dcd_remote_wakeup(uint8_t rhport) +{ + (void)rhport; + + // set remote wakeup + USB0.dctl |= USB_RMTWKUPSIG_M; + + // enable SOF to detect bus resume + USB0.gintsts = USB_SOF_M; + USB0.gintmsk |= USB_SOFMSK_M; + + // Per specs: remote wakeup signal bit must be clear within 1-15ms + vTaskDelay(pdMS_TO_TICKS(1)); + + USB0.dctl &= ~USB_RMTWKUPSIG_M; +} + +// connect by enabling internal pull-up resistor on D+/D- +void dcd_connect(uint8_t rhport) +{ + (void) rhport; + USB0.dctl &= ~USB_SFTDISCON_M; +} + +// disconnect by disabling internal pull-up resistor on D+/D- +void dcd_disconnect(uint8_t rhport) +{ + (void) rhport; + USB0.dctl |= USB_SFTDISCON_M; +} + +/*------------------------------------------------------------------*/ +/* DCD Endpoint port + *------------------------------------------------------------------*/ + +bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt) +{ + ESP_LOGV(TAG, "DCD endpoint opened"); + (void)rhport; + + usb_out_endpoint_t *out_ep = &(USB0.out_ep_reg[0]); + usb_in_endpoint_t *in_ep = &(USB0.in_ep_reg[0]); + + uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress); + uint8_t const dir = tu_edpt_dir(desc_edpt->bEndpointAddress); + + TU_ASSERT(epnum < EP_MAX); + + xfer_ctl_t *xfer = XFER_CTL_BASE(epnum, dir); + xfer->max_size = tu_edpt_packet_size(desc_edpt); + + if (dir == TUSB_DIR_OUT) { + out_ep[epnum].doepctl |= USB_USBACTEP1_M | + desc_edpt->bmAttributes.xfer << USB_EPTYPE1_S | + (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? USB_DO_SETD0PID1_M : 0) | + xfer->max_size << USB_MPS1_S; + USB0.daintmsk |= (1 << (16 + epnum)); + } else { + // "USB Data FIFOs" section in reference manual + // Peripheral FIFO architecture + // + // --------------- 320 or 1024 ( 1280 or 4096 bytes ) + // | IN FIFO MAX | + // --------------- + // | ... | + // --------------- y + x + 16 + GRXFSIZ + // | IN FIFO 2 | + // --------------- x + 16 + GRXFSIZ + // | IN FIFO 1 | + // --------------- 16 + GRXFSIZ + // | IN FIFO 0 | + // --------------- GRXFSIZ + // | OUT FIFO | + // | ( Shared ) | + // --------------- 0 + // + // Since OUT FIFO = GRXFSIZ, FIFO 0 = 16, for simplicity, we equally allocated for the rest of endpoints + // - Size : (FIFO_SIZE/4 - GRXFSIZ - 16) / (EP_MAX-1) + // - Offset: GRXFSIZ + 16 + Size*(epnum-1) + // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n". + + uint8_t fifo_num = get_free_fifo(); + TU_ASSERT(fifo_num != 0); + + in_ep[epnum].diepctl &= ~(USB_D_TXFNUM1_M | USB_D_EPTYPE1_M | USB_DI_SETD0PID1 | USB_D_MPS1_M); + in_ep[epnum].diepctl |= USB_D_USBACTEP1_M | + fifo_num << USB_D_TXFNUM1_S | + desc_edpt->bmAttributes.xfer << USB_D_EPTYPE1_S | + (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? (1 << USB_DI_SETD0PID1_S) : 0) | + xfer->max_size << 0; + + USB0.daintmsk |= (1 << (0 + epnum)); + + // Both TXFD and TXSA are in unit of 32-bit words. + // IN FIFO 0 was configured during enumeration, hence the "+ 16". + uint16_t const allocated_size = (USB0.grxfsiz & 0x0000ffff) + 16; + uint16_t const fifo_size = (EP_FIFO_SIZE/4 - allocated_size) / (EP_FIFO_NUM-1); + uint32_t const fifo_offset = allocated_size + fifo_size*(fifo_num-1); + + // DIEPTXF starts at FIFO #1. + USB0.dieptxf[epnum - 1] = (fifo_size << USB_NPTXFDEP_S) | fifo_offset; + } + return true; +} + +void dcd_edpt_close_all(uint8_t rhport) +{ + (void) rhport; + + usb_out_endpoint_t *out_ep = &(USB0.out_ep_reg[0]); + usb_in_endpoint_t *in_ep = &(USB0.in_ep_reg[0]); + + // Disable non-control interrupt + USB0.daintmsk = USB_OUTEPMSK0_M | USB_INEPMSK0_M; + + for(uint8_t n = 1; n < EP_MAX; n++) + { + // disable OUT endpoint + out_ep[n].doepctl = 0; + xfer_status[n][TUSB_DIR_OUT].max_size = 0; + + // disable IN endpoint + in_ep[n].diepctl = 0; + xfer_status[n][TUSB_DIR_IN].max_size = 0; + } + + _allocated_fifos = 1; +} + +bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes) +{ + (void)rhport; + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir); + xfer->buffer = buffer; + // xfer->ff = NULL; // TODO support dcd_edpt_xfer_fifo API + xfer->total_len = total_bytes; + xfer->queued_len = 0; + xfer->short_packet = false; + + uint16_t num_packets = (total_bytes / xfer->max_size); + uint8_t short_packet_size = total_bytes % xfer->max_size; + + // Zero-size packet is special case. + if (short_packet_size > 0 || (total_bytes == 0)) { + num_packets++; + } + + ESP_LOGV(TAG, "Transfer <-> EP%i, %s, pkgs: %i, bytes: %i", + epnum, ((dir == TUSB_DIR_IN) ? "USB0.HOST (in)" : "HOST->DEV (out)"), + num_packets, total_bytes); + + // IN and OUT endpoint xfers are interrupt-driven, we just schedule them + // here. + if (dir == TUSB_DIR_IN) { + // A full IN transfer (multiple packets, possibly) triggers XFRC. + USB0.in_ep_reg[epnum].dieptsiz = (num_packets << USB_D_PKTCNT0_S) | total_bytes; + USB0.in_ep_reg[epnum].diepctl |= USB_D_EPENA1_M | USB_D_CNAK1_M; // Enable | CNAK + + // Enable fifo empty interrupt only if there are something to put in the fifo. + if(total_bytes != 0) { + USB0.dtknqr4_fifoemptymsk |= (1 << epnum); + } + } else { + // Each complete packet for OUT xfers triggers XFRC. + USB0.out_ep_reg[epnum].doeptsiz |= USB_PKTCNT0_M | ((xfer->max_size & USB_XFERSIZE0_V) << USB_XFERSIZE0_S); + USB0.out_ep_reg[epnum].doepctl |= USB_EPENA0_M | USB_CNAK0_M; + } + return true; +} + +#if 0 // TODO support dcd_edpt_xfer_fifo API +bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes) +{ + (void)rhport; +} +#endif + +void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) +{ + (void)rhport; + + usb_out_endpoint_t *out_ep = &(USB0.out_ep_reg[0]); + usb_in_endpoint_t *in_ep = &(USB0.in_ep_reg[0]); + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + if (dir == TUSB_DIR_IN) { + // Only disable currently enabled non-control endpoint + if ((epnum == 0) || !(in_ep[epnum].diepctl & USB_D_EPENA1_M)) { + in_ep[epnum].diepctl |= (USB_DI_SNAK1_M | USB_D_STALL1_M); + } else { + // Stop transmitting packets and NAK IN xfers. + in_ep[epnum].diepctl |= USB_DI_SNAK1_M; + while ((in_ep[epnum].diepint & USB_DI_SNAK1_M) == 0) ; + + // Disable the endpoint. Note that both SNAK and STALL are set here. + in_ep[epnum].diepctl |= (USB_DI_SNAK1_M | USB_D_STALL1_M | USB_D_EPDIS1_M); + while ((in_ep[epnum].diepint & USB_D_EPDISBLD0_M) == 0) ; + in_ep[epnum].diepint = USB_D_EPDISBLD0_M; + } + + // Flush the FIFO, and wait until we have confirmed it cleared. + uint8_t const fifo_num = ((in_ep[epnum].diepctl >> USB_D_TXFNUM1_S) & USB_D_TXFNUM1_V); + USB0.grstctl |= (fifo_num << USB_TXFNUM_S); + USB0.grstctl |= USB_TXFFLSH_M; + while ((USB0.grstctl & USB_TXFFLSH_M) != 0) ; + } else { + // Only disable currently enabled non-control endpoint + if ((epnum == 0) || !(out_ep[epnum].doepctl & USB_EPENA0_M)) { + out_ep[epnum].doepctl |= USB_STALL0_M; + } else { + // Asserting GONAK is required to STALL an OUT endpoint. + // Simpler to use polling here, we don't use the "B"OUTNAKEFF interrupt + // anyway, and it can't be cleared by user code. If this while loop never + // finishes, we have bigger problems than just the stack. + USB0.dctl |= USB_SGOUTNAK_M; + while ((USB0.gintsts & USB_GOUTNAKEFF_M) == 0) ; + + // Ditto here- disable the endpoint. Note that only STALL and not SNAK + // is set here. + out_ep[epnum].doepctl |= (USB_STALL0_M | USB_EPDIS0_M); + while ((out_ep[epnum].doepint & USB_EPDISBLD0_M) == 0) ; + out_ep[epnum].doepint = USB_EPDISBLD0_M; + + // Allow other OUT endpoints to keep receiving. + USB0.dctl |= USB_CGOUTNAK_M; + } + } +} + +void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) +{ + (void)rhport; + + usb_out_endpoint_t *out_ep = &(USB0.out_ep_reg[0]); + usb_in_endpoint_t *in_ep = &(USB0.in_ep_reg[0]); + + uint8_t const epnum = tu_edpt_number(ep_addr); + uint8_t const dir = tu_edpt_dir(ep_addr); + + if (dir == TUSB_DIR_IN) { + in_ep[epnum].diepctl &= ~USB_D_STALL1_M; + + uint8_t eptype = (in_ep[epnum].diepctl & USB_D_EPTYPE1_M) >> USB_D_EPTYPE1_S; + // Required by USB spec to reset DATA toggle bit to DATA0 on interrupt + // and bulk endpoints. + if (eptype == 2 || eptype == 3) { + in_ep[epnum].diepctl |= USB_DI_SETD0PID1_M; + } + } else { + out_ep[epnum].doepctl &= ~USB_STALL1_M; + + uint8_t eptype = (out_ep[epnum].doepctl & USB_EPTYPE1_M) >> USB_EPTYPE1_S; + // Required by USB spec to reset DATA toggle bit to DATA0 on interrupt + // and bulk endpoints. + if (eptype == 2 || eptype == 3) { + out_ep[epnum].doepctl |= USB_DO_SETD0PID1_M; + } + } +} + +/*------------------------------------------------------------------*/ + +static void receive_packet(xfer_ctl_t *xfer, /* usb_out_endpoint_t * out_ep, */ uint16_t xfer_size) +{ + ESP_EARLY_LOGV(TAG, "USB - receive_packet"); + volatile uint32_t *rx_fifo = USB0.fifo[0]; + + // See above TODO + // uint16_t remaining = (out_ep->DOEPTSIZ & UsbDOEPTSIZ_XFRSIZ_Msk) >> UsbDOEPTSIZ_XFRSIZ_Pos; + // xfer->queued_len = xfer->total_len - remaining; + + uint16_t remaining = xfer->total_len - xfer->queued_len; + uint16_t to_recv_size; + + if (remaining <= xfer->max_size) { + // Avoid buffer overflow. + to_recv_size = (xfer_size > remaining) ? remaining : xfer_size; + } else { + // Room for full packet, choose recv_size based on what the microcontroller + // claims. + to_recv_size = (xfer_size > xfer->max_size) ? xfer->max_size : xfer_size; + } + + // Common buffer read +#if 0 // TODO support dcd_edpt_xfer_fifo API + if (xfer->ff) + { + // Ring buffer + tu_fifo_write_n_const_addr_full_words(xfer->ff, (const void *) rx_fifo, to_recv_size); + } + else +#endif + { + uint8_t to_recv_rem = to_recv_size % 4; + uint16_t to_recv_size_aligned = to_recv_size - to_recv_rem; + + // Do not assume xfer buffer is aligned. + uint8_t *base = (xfer->buffer + xfer->queued_len); + + // This for loop always runs at least once- skip if less than 4 bytes + // to collect. + if (to_recv_size >= 4) { + for (uint16_t i = 0; i < to_recv_size_aligned; i += 4) { + uint32_t tmp = (*rx_fifo); + base[i] = tmp & 0x000000FF; + base[i + 1] = (tmp & 0x0000FF00) >> 8; + base[i + 2] = (tmp & 0x00FF0000) >> 16; + base[i + 3] = (tmp & 0xFF000000) >> 24; + } + } + + // Do not read invalid bytes from RX FIFO. + if (to_recv_rem != 0) { + uint32_t tmp = (*rx_fifo); + uint8_t *last_32b_bound = base + to_recv_size_aligned; + + last_32b_bound[0] = tmp & 0x000000FF; + if (to_recv_rem > 1) { + last_32b_bound[1] = (tmp & 0x0000FF00) >> 8; + } + if (to_recv_rem > 2) { + last_32b_bound[2] = (tmp & 0x00FF0000) >> 16; + } + } + } + + xfer->queued_len += xfer_size; + + // Per USB spec, a short OUT packet (including length 0) is always + // indicative of the end of a transfer (at least for ctl, bulk, int). + xfer->short_packet = (xfer_size < xfer->max_size); +} + +static void transmit_packet(xfer_ctl_t *xfer, volatile usb_in_endpoint_t *in_ep, uint8_t fifo_num) +{ + ESP_EARLY_LOGV(TAG, "USB - transmit_packet"); + volatile uint32_t *tx_fifo = USB0.fifo[fifo_num]; + + uint16_t remaining = (in_ep->dieptsiz & 0x7FFFFU) >> USB_D_XFERSIZE0_S; + xfer->queued_len = xfer->total_len - remaining; + + uint16_t to_xfer_size = (remaining > xfer->max_size) ? xfer->max_size : remaining; + +#if 0 // TODO support dcd_edpt_xfer_fifo API + if (xfer->ff) + { + tu_fifo_read_n_const_addr_full_words(xfer->ff, (void *) tx_fifo, to_xfer_size); + } + else +#endif + { + uint8_t to_xfer_rem = to_xfer_size % 4; + uint16_t to_xfer_size_aligned = to_xfer_size - to_xfer_rem; + + // Buffer might not be aligned to 32b, so we need to force alignment + // by copying to a temp var. + uint8_t *base = (xfer->buffer + xfer->queued_len); + + // This for loop always runs at least once- skip if less than 4 bytes + // to send off. + if (to_xfer_size >= 4) { + for (uint16_t i = 0; i < to_xfer_size_aligned; i += 4) { + uint32_t tmp = base[i] | (base[i + 1] << 8) | + (base[i + 2] << 16) | (base[i + 3] << 24); + (*tx_fifo) = tmp; + } + } + + // Do not read beyond end of buffer if not divisible by 4. + if (to_xfer_rem != 0) { + uint32_t tmp = 0; + uint8_t *last_32b_bound = base + to_xfer_size_aligned; + + tmp |= last_32b_bound[0]; + if (to_xfer_rem > 1) { + tmp |= (last_32b_bound[1] << 8); + } + if (to_xfer_rem > 2) { + tmp |= (last_32b_bound[2] << 16); + } + + (*tx_fifo) = tmp; + } + } +} + +static void read_rx_fifo(void) +{ + // Pop control word off FIFO (completed xfers will have 2 control words, + // we only pop one ctl word each interrupt). + uint32_t const ctl_word = USB0.grxstsp; + uint8_t const pktsts = (ctl_word & USB_PKTSTS_M) >> USB_PKTSTS_S; + uint8_t const epnum = (ctl_word & USB_CHNUM_M ) >> USB_CHNUM_S; + uint16_t const bcnt = (ctl_word & USB_BCNT_M ) >> USB_BCNT_S; + + switch (pktsts) { + case 0x01: // Global OUT NAK (Interrupt) + ESP_EARLY_LOGV(TAG, "TUSB IRQ - RX type : Global OUT NAK"); + break; + + case 0x02: { // Out packet recvd + ESP_EARLY_LOGV(TAG, "TUSB IRQ - RX type : Out packet"); + xfer_ctl_t *xfer = XFER_CTL_BASE(epnum, TUSB_DIR_OUT); + receive_packet(xfer, bcnt); + } + break; + + case 0x03: // Out packet done (Interrupt) + ESP_EARLY_LOGV(TAG, "TUSB IRQ - RX type : Out packet done"); + break; + + case 0x04: // Step 2: Setup transaction completed (Interrupt) + // After this event, OEPINT interrupt will occur with SETUP bit set + ESP_EARLY_LOGV(TAG, "TUSB IRQ - RX : Setup packet done"); + USB0.out_ep_reg[epnum].doeptsiz |= USB_SUPCNT0_M; + break; + + case 0x06: { // Step1: Setup data packet received + volatile uint32_t *rx_fifo = USB0.fifo[0]; + + // We can receive up to three setup packets in succession, but + // only the last one is valid. Therefore we just overwrite it + _setup_packet[0] = (*rx_fifo); + _setup_packet[1] = (*rx_fifo); + + ESP_EARLY_LOGV(TAG, "TUSB IRQ - RX : Setup packet : 0x%08x 0x%08x", _setup_packet[0], _setup_packet[1]); + } + break; + + default: // Invalid, do something here, like breakpoint? + TU_BREAKPOINT(); + break; + } +} + +static void handle_epout_ints(void) +{ + // GINTSTS will be cleared with DAINT == 0 + // DAINT for a given EP clears when DOEPINTx is cleared. + // DOEPINT will be cleared when DAINT's out bits are cleared. + for (int n = 0; n < USB_OUT_EP_NUM; n++) { + xfer_ctl_t *xfer = XFER_CTL_BASE(n, TUSB_DIR_OUT); + + if (USB0.daint & (1 << (16 + n))) { + // SETUP packet Setup Phase done. + if ((USB0.out_ep_reg[n].doepint & USB_SETUP0_M)) { + USB0.out_ep_reg[n].doepint = USB_STUPPKTRCVD0_M | USB_SETUP0_M; // clear + dcd_event_setup_received(0, (uint8_t *)&_setup_packet[0], true); + } + + // OUT XFER complete (single packet).q + if (USB0.out_ep_reg[n].doepint & USB_XFERCOMPL0_M) { + + ESP_EARLY_LOGV(TAG, "TUSB IRQ - EP OUT - XFER complete (single packet)"); + USB0.out_ep_reg[n].doepint = USB_XFERCOMPL0_M; + + // Transfer complete if short packet or total len is transferred + if (xfer->short_packet || (xfer->queued_len == xfer->total_len)) { + xfer->short_packet = false; + dcd_event_xfer_complete(0, n, xfer->queued_len, XFER_RESULT_SUCCESS, true); + } else { + // Schedule another packet to be received. + USB0.out_ep_reg[n].doeptsiz |= USB_PKTCNT0_M | ((xfer->max_size & USB_XFERSIZE0_V) << USB_XFERSIZE0_S); + USB0.out_ep_reg[n].doepctl |= USB_EPENA0_M | USB_CNAK0_M; + } + } + } + } +} + +static void handle_epin_ints(void) +{ + // GINTSTS will be cleared with DAINT == 0 + // DAINT for a given EP clears when DIEPINTx is cleared. + // IEPINT will be cleared when DAINT's out bits are cleared. + for (uint32_t n = 0; n < USB_IN_EP_NUM; n++) { + xfer_ctl_t *xfer = &xfer_status[n][TUSB_DIR_IN]; + + if (USB0.daint & (1 << (0 + n))) { + ESP_EARLY_LOGV(TAG, "TUSB IRQ - EP IN %u", n); + // IN XFER complete (entire xfer). + if (USB0.in_ep_reg[n].diepint & USB_D_XFERCOMPL0_M) { + ESP_EARLY_LOGV(TAG, "TUSB IRQ - IN XFER complete!"); + USB0.in_ep_reg[n].diepint = USB_D_XFERCOMPL0_M; + dcd_event_xfer_complete(0, n | TUSB_DIR_IN_MASK, xfer->total_len, XFER_RESULT_SUCCESS, true); + } + + // XFER FIFO empty + if (USB0.in_ep_reg[n].diepint & USB_D_TXFEMP0_M) { + ESP_EARLY_LOGV(TAG, "TUSB IRQ - IN XFER FIFO empty!"); + USB0.in_ep_reg[n].diepint = USB_D_TXFEMP0_M; + transmit_packet(xfer, &USB0.in_ep_reg[n], n); + + // Turn off TXFE if all bytes are written. + if (xfer->queued_len == xfer->total_len) + { + USB0.dtknqr4_fifoemptymsk &= ~(1 << n); + } + } + + // XFER Timeout + if (USB0.in_ep_reg[n].diepint & USB_D_TIMEOUT0_M) { + // Clear interrupt or enpoint will hang. + USB0.in_ep_reg[n].diepint = USB_D_TIMEOUT0_M; + // Maybe retry? + } + } + } +} + + +static void _dcd_int_handler(void* arg) +{ + (void) arg; + uint8_t const rhport = 0; + + const uint32_t int_msk = USB0.gintmsk; + const uint32_t int_status = USB0.gintsts & int_msk; + + if (int_status & USB_USBRST_M) { + // start of reset + ESP_EARLY_LOGV(TAG, "dcd_int_handler - reset"); + USB0.gintsts = USB_USBRST_M; + // FIFOs will be reassigned when the endpoints are reopen + _allocated_fifos = 1; + bus_reset(); + } + + if (int_status & USB_RESETDET_M) { + ESP_EARLY_LOGV(TAG, "dcd_int_handler - reset while suspend"); + USB0.gintsts = USB_RESETDET_M; + bus_reset(); + } + + if (int_status & USB_ENUMDONE_M) { + // ENUMDNE detects speed of the link. For full-speed, we + // always expect the same value. This interrupt is considered + // the end of reset. + USB0.gintsts = USB_ENUMDONE_M; + enum_done_processing(); + dcd_event_bus_reset(rhport, TUSB_SPEED_FULL, true); + } + + if(int_status & USB_USBSUSP_M) + { + USB0.gintsts = USB_USBSUSP_M; + dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true); + } + + if(int_status & USB_WKUPINT_M) + { + USB0.gintsts = USB_WKUPINT_M; + dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true); + } + + if (int_status & USB_OTGINT_M) + { + // OTG INT bit is read-only + ESP_EARLY_LOGV(TAG, "dcd_int_handler - disconnected"); + + uint32_t const otg_int = USB0.gotgint; + + if (otg_int & USB_SESENDDET_M) + { + dcd_event_bus_signal(rhport, DCD_EVENT_UNPLUGGED, true); + } + + USB0.gotgint = otg_int; + } + + if (int_status & USB_SOF_M) { + USB0.gintsts = USB_SOF_M; + + // Disable SOF interrupt since currently only used for remote wakeup detection + USB0.gintmsk &= ~USB_SOFMSK_M; + + dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true); + } + + + if (int_status & USB_RXFLVI_M) { + // RXFLVL bit is read-only + ESP_EARLY_LOGV(TAG, "dcd_int_handler - rx!"); + + // Mask out RXFLVL while reading data from FIFO + USB0.gintmsk &= ~USB_RXFLVIMSK_M; + read_rx_fifo(); + USB0.gintmsk |= USB_RXFLVIMSK_M; + } + + // OUT endpoint interrupt handling. + if (int_status & USB_OEPINT_M) { + // OEPINT is read-only + ESP_EARLY_LOGV(TAG, "dcd_int_handler - OUT endpoint!"); + handle_epout_ints(); + } + + // IN endpoint interrupt handling. + if (int_status & USB_IEPINT_M) { + // IEPINT bit read-only + ESP_EARLY_LOGV(TAG, "dcd_int_handler - IN endpoint!"); + handle_epin_ints(); + } + + // Without handling + USB0.gintsts |= USB_CURMOD_INT_M | + USB_MODEMIS_M | + USB_OTGINT_M | + USB_NPTXFEMP_M | + USB_GINNAKEFF_M | + USB_GOUTNAKEFF | + USB_ERLYSUSP_M | + USB_USBSUSP_M | + USB_ISOOUTDROP_M | + USB_EOPF_M | + USB_EPMIS_M | + USB_INCOMPISOIN_M | + USB_INCOMPIP_M | + USB_FETSUSP_M | + USB_PTXFEMP_M; +} + +void dcd_int_enable (uint8_t rhport) +{ + (void) rhport; + esp_intr_alloc(ETS_USB_INTR_SOURCE, ESP_INTR_FLAG_LOWMED, (intr_handler_t) _dcd_int_handler, NULL, &usb_ih); +} + +void dcd_int_disable (uint8_t rhport) +{ + (void) rhport; + esp_intr_free(usb_ih); +} + +#endif // #if OPT_MCU_ESP32S2 || OPT_MCU_ESP32S3 + diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/portable/template/dcd_template.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/portable/template/dcd_template.c new file mode 100644 index 000000000..977369300 --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/portable/template/dcd_template.c @@ -0,0 +1,136 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2018, hathach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if CFG_TUSB_MCU == OPT_MCU_NONE + +#include "device/dcd.h" + +//--------------------------------------------------------------------+ +// MACRO TYPEDEF CONSTANT ENUM DECLARATION +//--------------------------------------------------------------------+ + + +/*------------------------------------------------------------------*/ +/* Device API + *------------------------------------------------------------------*/ + +// Initialize controller to device mode +void dcd_init (uint8_t rhport) +{ + (void) rhport; +} + +// Enable device interrupt +void dcd_int_enable (uint8_t rhport) +{ + (void) rhport; +} + +// Disable device interrupt +void dcd_int_disable (uint8_t rhport) +{ + (void) rhport; +} + +// Receive Set Address request, mcu port must also include status IN response +void dcd_set_address (uint8_t rhport, uint8_t dev_addr) +{ + (void) rhport; + (void) dev_addr; +} + +// Wake up host +void dcd_remote_wakeup (uint8_t rhport) +{ + (void) rhport; +} + +// Connect by enabling internal pull-up resistor on D+/D- +void dcd_connect(uint8_t rhport) +{ + (void) rhport; +} + +// Disconnect by disabling internal pull-up resistor on D+/D- +void dcd_disconnect(uint8_t rhport) +{ + (void) rhport; +} + +//--------------------------------------------------------------------+ +// Endpoint API +//--------------------------------------------------------------------+ + +// Configure endpoint's registers according to descriptor +bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * ep_desc) +{ + (void) rhport; + (void) ep_desc; + return false; +} + +void dcd_edpt_close_all (uint8_t rhport) +{ + (void) rhport; +} + +// Submit a transfer, When complete dcd_event_xfer_complete() is invoked to notify the stack +bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes) +{ + (void) rhport; + (void) ep_addr; + (void) buffer; + (void) total_bytes; + return false; +} + +// Submit a transfer where is managed by FIFO, When complete dcd_event_xfer_complete() is invoked to notify the stack - optional, however, must be listed in usbd.c +bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes) +{ + (void) rhport; + (void) ep_addr; + (void) ff; + (void) total_bytes; + return false; +} + +// Stall endpoint +void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr) +{ + (void) rhport; + (void) ep_addr; +} + +// clear stall, data toggle is also reset to DATA0 +void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr) +{ + (void) rhport; + (void) ep_addr; +} + +#endif diff --git a/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/tusb.c b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/tusb.c new file mode 100644 index 000000000..9583f509d --- /dev/null +++ b/Firmware/Targets/ESP32SX/components/tinyusb/tinyusb/src/tusb.c @@ -0,0 +1,245 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach (tinyusb.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * This file is part of the TinyUSB stack. + */ + +#include "tusb_option.h" + +#if TUSB_OPT_HOST_ENABLED || TUSB_OPT_DEVICE_ENABLED + +#include "tusb.h" + +// TODO clean up +#if TUSB_OPT_DEVICE_ENABLED +#include "device/usbd_pvt.h" +#endif + +bool tusb_init(void) +{ +#if TUSB_OPT_DEVICE_ENABLED + TU_ASSERT ( tud_init(TUD_OPT_RHPORT) ); // init device stack +#endif + +#if TUSB_OPT_HOST_ENABLED + TU_ASSERT( tuh_init(TUH_OPT_RHPORT) ); // init host stack +#endif + + return true; +} + +bool tusb_inited(void) +{ + bool ret = false; + +#if TUSB_OPT_DEVICE_ENABLED + ret = ret || tud_inited(); +#endif + +#if TUSB_OPT_HOST_ENABLED + ret = ret || tuh_inited(); +#endif + + return ret; +} + +//--------------------------------------------------------------------+ +// Internal Helper for both Host and Device stack +//--------------------------------------------------------------------+ + +bool tu_edpt_validate(tusb_desc_endpoint_t const * desc_ep, tusb_speed_t speed) +{ + uint16_t const max_packet_size = tu_edpt_packet_size(desc_ep); + TU_LOG2(" Open EP %02X with Size = %u\r\n", desc_ep->bEndpointAddress, max_packet_size); + + switch (desc_ep->bmAttributes.xfer) + { + case TUSB_XFER_ISOCHRONOUS: + { + uint16_t const spec_size = (speed == TUSB_SPEED_HIGH ? 1024 : 1023); + TU_ASSERT(max_packet_size <= spec_size); + } + break; + + case TUSB_XFER_BULK: + if (speed == TUSB_SPEED_HIGH) + { + // Bulk highspeed must be EXACTLY 512 + TU_ASSERT(max_packet_size == 512); + }else + { + // TODO Bulk fullspeed can only be 8, 16, 32, 64 + TU_ASSERT(max_packet_size <= 64); + } + break; + + case TUSB_XFER_INTERRUPT: + { + uint16_t const spec_size = (speed == TUSB_SPEED_HIGH ? 1024 : 64); + TU_ASSERT(max_packet_size <= spec_size); + } + break; + + default: return false; + } + + return true; +} + +void tu_edpt_bind_driver(uint8_t ep2drv[][2], tusb_desc_interface_t const* desc_itf, uint16_t desc_len, uint8_t driver_id) +{ + uint8_t const* p_desc = (uint8_t const*) desc_itf; + uint8_t const* desc_end = p_desc + desc_len; + + while( p_desc < desc_end ) + { + if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) ) + { + uint8_t const ep_addr = ((tusb_desc_endpoint_t const*) p_desc)->bEndpointAddress; + + TU_LOG(2, " Bind EP %02x to driver id %u\r\n", ep_addr, driver_id); + ep2drv[tu_edpt_number(ep_addr)][tu_edpt_dir(ep_addr)] = driver_id; + } + + p_desc = tu_desc_next(p_desc); + } +} + +uint16_t tu_desc_get_interface_total_len(tusb_desc_interface_t const* desc_itf, uint8_t itf_count, uint16_t max_len) +{ + uint8_t const* p_desc = (uint8_t const*) desc_itf; + uint16_t len = 0; + + while (itf_count--) + { + // Next on interface desc + len += tu_desc_len(desc_itf); + p_desc = tu_desc_next(p_desc); + + while (len < max_len) + { + // return on IAD regardless of itf count + if ( tu_desc_type(p_desc) == TUSB_DESC_INTERFACE_ASSOCIATION ) return len; + + if ( (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE) && + ((tusb_desc_interface_t const*) p_desc)->bAlternateSetting == 0 ) + { + break; + } + + len += tu_desc_len(p_desc); + p_desc = tu_desc_next(p_desc); + } + } + + return len; +} + +/*------------------------------------------------------------------*/ +/* Debug + *------------------------------------------------------------------*/ +#if CFG_TUSB_DEBUG +#include + +char const* const tusb_strerr[TUSB_ERROR_COUNT] = { ERROR_TABLE(ERROR_STRING) }; + +static void dump_str_line(uint8_t const* buf, uint16_t count) +{ + tu_printf(" |"); + + // each line is 16 bytes + for(uint16_t i=0; i= 0 ) + +// Which roothub port is configured as device +#define TUD_OPT_RHPORT ( ((CFG_TUSB_RHPORT0_MODE) & OPT_MODE_DEVICE) ? 0 : (((CFG_TUSB_RHPORT1_MODE) & OPT_MODE_DEVICE) ? 1 : -1) ) + +#if TUD_OPT_RHPORT == 0 +#define TUD_OPT_HIGH_SPEED ( (CFG_TUSB_RHPORT0_MODE) & OPT_MODE_HIGH_SPEED ) +#else +#define TUD_OPT_HIGH_SPEED ( (CFG_TUSB_RHPORT1_MODE) & OPT_MODE_HIGH_SPEED ) +#endif + +#define TUSB_OPT_DEVICE_ENABLED ( TUD_OPT_RHPORT >= 0 ) + +//--------------------------------------------------------------------+ +// COMMON OPTIONS +//--------------------------------------------------------------------+ + +// Debug enable to print out error message +#ifndef CFG_TUSB_DEBUG + #define CFG_TUSB_DEBUG 0 +#endif + +// place data in accessible RAM for usb controller +#ifndef CFG_TUSB_MEM_SECTION + #define CFG_TUSB_MEM_SECTION +#endif + +// alignment requirement of buffer used for endpoint transferring +#ifndef CFG_TUSB_MEM_ALIGN + #define CFG_TUSB_MEM_ALIGN TU_ATTR_ALIGNED(4) +#endif + +// OS selection +#ifndef CFG_TUSB_OS + #define CFG_TUSB_OS OPT_OS_NONE +#endif + +#ifndef CFG_TUSB_OS_INC_PATH + #define CFG_TUSB_OS_INC_PATH +#endif + +//-------------------------------------------------------------------- +// DEVICE OPTIONS +//-------------------------------------------------------------------- + +#ifndef CFG_TUD_ENDPOINT0_SIZE + #define CFG_TUD_ENDPOINT0_SIZE 64 +#endif + +#ifndef CFG_TUD_CDC + #define CFG_TUD_CDC 0 +#endif + +#ifndef CFG_TUD_MSC + #define CFG_TUD_MSC 0 +#endif + +#ifndef CFG_TUD_HID + #define CFG_TUD_HID 0 +#endif + +#ifndef CFG_TUD_AUDIO + #define CFG_TUD_AUDIO 0 +#endif + +#ifndef CFG_TUD_VIDEO + #define CFG_TUD_VIDEO 0 +#endif + +#ifndef CFG_TUD_MIDI + #define CFG_TUD_MIDI 0 +#endif + +#ifndef CFG_TUD_VENDOR + #define CFG_TUD_VENDOR 0 +#endif + +#ifndef CFG_TUD_USBTMC + #define CFG_TUD_USBTMC 0 +#endif + +#ifndef CFG_TUD_DFU_RUNTIME + #define CFG_TUD_DFU_RUNTIME 0 +#endif + +#ifndef CFG_TUD_DFU + #define CFG_TUD_DFU 0 +#endif + +#ifndef CFG_TUD_BTH + #define CFG_TUD_BTH 0 +#endif + +#ifndef CFG_TUD_ECM_RNDIS + #ifdef CFG_TUD_NET + #warning "CFG_TUD_NET is renamed to CFG_TUD_ECM_RNDIS" + #define CFG_TUD_ECM_RNDIS CFG_TUD_NET + #else + #define CFG_TUD_ECM_RNDIS 0 + #endif +#endif + +#ifndef CFG_TUD_NCM + #define CFG_TUD_NCM 0 +#endif + +//-------------------------------------------------------------------- +// HOST OPTIONS +//-------------------------------------------------------------------- +#if TUSB_OPT_HOST_ENABLED + #ifndef CFG_TUH_DEVICE_MAX + #define CFG_TUH_DEVICE_MAX 1 + #endif + + #ifndef CFG_TUH_ENUMERATION_BUFSIZE + #define CFG_TUH_ENUMERATION_BUFSIZE 256 + #endif +#endif // TUSB_OPT_HOST_ENABLED + +//------------- CLASS -------------// + +#ifndef CFG_TUH_HUB +#define CFG_TUH_HUB 0 +#endif + +#ifndef CFG_TUH_CDC +#define CFG_TUH_CDC 0 +#endif + +#ifndef CFG_TUH_HID +#define CFG_TUH_HID 0 +#endif + +#ifndef CFG_TUH_MIDI +#define CFG_TUH_MIDI 0 +#endif + +#ifndef CFG_TUH_MSC +#define CFG_TUH_MSC 0 +#endif + +#ifndef CFG_TUH_VENDOR +#define CFG_TUH_VENDOR 0 +#endif + +//--------------------------------------------------------------------+ +// Port Specific +// TUP stand for TinyUSB Port (can be renamed) +//--------------------------------------------------------------------+ + +//------------- Unaligned Memory -------------// + +// ARMv7+ (M3-M7, M23-M33) can access unaligned memory +#if (defined(__ARM_ARCH) && (__ARM_ARCH >= 7)) + #define TUP_ARCH_STRICT_ALIGN 0 +#else + #define TUP_ARCH_STRICT_ALIGN 1 +#endif + +// TUP_MCU_STRICT_ALIGN will overwrite TUP_ARCH_STRICT_ALIGN. +// In case TUP_MCU_STRICT_ALIGN = 1 and TUP_ARCH_STRICT_ALIGN =0, we will not reply on compiler +// to generate unaligned access code. +// LPC_IP3511 Highspeed cannot access unaligned memory on USB_RAM +#if TUD_OPT_HIGH_SPEED && (CFG_TUSB_MCU == OPT_MCU_LPC54XXX || CFG_TUSB_MCU == OPT_MCU_LPC55XX) + #define TUP_MCU_STRICT_ALIGN 1 +#else + #define TUP_MCU_STRICT_ALIGN 0 +#endif + + +//------------------------------------------------------------------ +// Configuration Validation +//------------------------------------------------------------------ +#if CFG_TUD_ENDPOINT0_SIZE > 64 + #error Control Endpoint Max Packet Size cannot be larger than 64 +#endif + +#endif /* _TUSB_OPTION_H_ */ + +/** @} */ diff --git a/Firmware/Targets/ESP32SX/firmware/generate_bin.sh b/Firmware/Targets/ESP32SX/firmware/generate_bin.sh new file mode 100755 index 000000000..6df4868ff --- /dev/null +++ b/Firmware/Targets/ESP32SX/firmware/generate_bin.sh @@ -0,0 +1,48 @@ +#!/bin/sh + +set -u +set -e + + +merge_bin(){ + esptool.py --chip ${IDF_TARGET} merge_bin -o $1 --flash_mode dio --flash_size 4MB \ + 0x1000 build/bootloader/bootloader.bin \ + 0x10000 build/openffboard.bin \ + 0x8000 build/partition_table/partition-table.bin +} + +build_and_generate_bin(){ + cd ../ + idf.py fullclean + rm -f sdkconfig sdkconfig.old + idf.py build + merge_bin firmware/openffb-${IDF_TARGET}-hw_${HW_VER}.bin + cd firmware +} + +SCRIPT_DIR=$(dirname $(readlink -f "$0")) +cd $SCRIPT_DIR +rm -f ../firmware/*.bin + +export IDF_TARGET=esp32s2 +export EXTRA_CFLAGS="-DOPENFFBOARD_ESP32S2_V1_0" +export EXTRA_CXXFLAGS="-DOPENFFBOARD_ESP32S2_V1_0" +HW_VER="v1.0" +build_and_generate_bin + +export IDF_TARGET=esp32s2 +export EXTRA_CFLAGS="-DOPENFFBOARD_ESP32S2_V1_1" +export EXTRA_CXXFLAGS="-DOPENFFBOARD_ESP32S2_V1_1" +HW_VER="v1.1" +build_and_generate_bin + +export IDF_TARGET=esp32s3 +export EXTRA_CFLAGS="-DOPENFFBOARD_ESP32S2_V1_1" +export EXTRA_CXXFLAGS="-DOPENFFBOARD_ESP32S2_V1_1" +HW_VER="v1.1" +build_and_generate_bin + + + + + diff --git a/Firmware/Targets/ESP32SX/firmware/openffb-esp32s2-hw_v1.0.bin b/Firmware/Targets/ESP32SX/firmware/openffb-esp32s2-hw_v1.0.bin new file mode 100644 index 000000000..cb626848c Binary files /dev/null and b/Firmware/Targets/ESP32SX/firmware/openffb-esp32s2-hw_v1.0.bin differ diff --git a/Firmware/Targets/ESP32SX/firmware/openffb-esp32s2-hw_v1.1.bin b/Firmware/Targets/ESP32SX/firmware/openffb-esp32s2-hw_v1.1.bin new file mode 100644 index 000000000..989b4210b Binary files /dev/null and b/Firmware/Targets/ESP32SX/firmware/openffb-esp32s2-hw_v1.1.bin differ diff --git a/Firmware/Targets/ESP32SX/firmware/openffb-esp32s3-hw_v1.1.bin b/Firmware/Targets/ESP32SX/firmware/openffb-esp32s3-hw_v1.1.bin new file mode 100644 index 000000000..41bb7aa37 Binary files /dev/null and b/Firmware/Targets/ESP32SX/firmware/openffb-esp32s3-hw_v1.1.bin differ diff --git a/Firmware/Targets/ESP32SX/main/CMakeLists.txt b/Firmware/Targets/ESP32SX/main/CMakeLists.txt new file mode 100644 index 000000000..80caad1e2 --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/CMakeLists.txt @@ -0,0 +1,9 @@ + +set(FFB_DIR ../../../FFBoard) + +idf_component_register(SRC_DIRS "." "${FFB_DIR}/Src" "${FFB_DIR}/UserExtensions/Src" + INCLUDE_DIRS "." "${FFB_DIR}/Inc" "${FFB_DIR}/UserExtensions/Inc" + ) + +target_compile_options(${COMPONENT_LIB} PRIVATE "-std=gnu++17" "-fexceptions") +target_compile_options(${COMPONENT_LIB} PRIVATE -Wfatal-errors -Wno-ignored-qualifiers -Wno-unused-value) diff --git a/Firmware/Targets/ESP32SX/main/app_main.c b/Firmware/Targets/ESP32SX/main/app_main.c new file mode 100644 index 000000000..d9765956c --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/app_main.c @@ -0,0 +1,83 @@ + +#include +#include "esp_log.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "nvs_flash.h" +#include "driver/gpio.h" +#include "esp_rom_gpio.h" +#include "hal/gpio_ll.h" +#include "hal/usb_hal.h" +#include "soc/gpio_periph.h" +#include "soc/usb_periph.h" +#include "hal/usb_hal.h" +#include "glue.h" +#include "cppmain.h" + +static const char *TAG = "app_main"; + +extern void cppmain(); + +static void configure_pins(usb_hal_context_t *usb) +{ + /* usb_periph_iopins currently configures USB_OTG as USB Device. + * Introduce additional parameters in usb_hal_context_t when adding support + * for USB Host. + */ + for (const usb_iopin_dsc_t *iopin = usb_periph_iopins; iopin->pin != -1; ++iopin) { + if ((usb->use_external_phy) || (iopin->ext_phy_only == 0)) { + esp_rom_gpio_pad_select_gpio(iopin->pin); + if (iopin->is_output) { + esp_rom_gpio_connect_out_signal(iopin->pin, iopin->func, false, false); + } else { + esp_rom_gpio_connect_in_signal(iopin->pin, iopin->func, false); + if ((iopin->pin != GPIO_FUNC_IN_LOW) && (iopin->pin != GPIO_FUNC_IN_HIGH)) { + gpio_ll_input_enable(&GPIO, (gpio_num_t)iopin->pin); + } + } + esp_rom_gpio_pad_unhold(iopin->pin); + } + } + if (!usb->use_external_phy) { + gpio_set_drive_capability((gpio_num_t)USBPHY_DM_NUM, GPIO_DRIVE_CAP_3); + gpio_set_drive_capability((gpio_num_t)USBPHY_DP_NUM, GPIO_DRIVE_CAP_3); + } +} + +void app_main(void) +{ + //Initialize NVS + esp_err_t ret = nvs_flash_init(); + if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { + ESP_ERROR_CHECK(nvs_flash_erase()); + ret = nvs_flash_init(); + } + ESP_ERROR_CHECK(ret); + + glue_board_init(); + + for (size_t i = 0; i < 2; i++) { + HAL_GPIO_WritePin(LED_SYS_GPIO_Port, LED_SYS_Pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(LED_ERR_GPIO_Port, LED_ERR_Pin, GPIO_PIN_SET); + HAL_GPIO_WritePin(LED_CLIP_GPIO_Port, LED_CLIP_Pin, GPIO_PIN_SET); + vTaskDelay(pdMS_TO_TICKS(100)); + HAL_GPIO_WritePin(LED_SYS_GPIO_Port, LED_SYS_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LED_ERR_GPIO_Port, LED_ERR_Pin, GPIO_PIN_RESET); + HAL_GPIO_WritePin(LED_CLIP_GPIO_Port, LED_CLIP_Pin, GPIO_PIN_RESET); + vTaskDelay(pdMS_TO_TICKS(100)); + } + + ESP_LOGI(TAG, "USB initialization"); + // Enable APB CLK to USB peripheral + periph_module_enable(PERIPH_USB_MODULE); + periph_module_reset(PERIPH_USB_MODULE); + // Initialize HAL layer + usb_hal_context_t hal = { + .use_external_phy = 0, + }; + usb_hal_init(&hal); + configure_pins(&hal); + + cppmain(); +} + diff --git a/Firmware/Targets/ESP32SX/main/cmsis_os.h b/Firmware/Targets/ESP32SX/main/cmsis_os.h new file mode 100644 index 000000000..9d7056b85 --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/cmsis_os.h @@ -0,0 +1 @@ +// nothing to do \ No newline at end of file diff --git a/Firmware/Targets/ESP32SX/main/cpp_target_config.cpp b/Firmware/Targets/ESP32SX/main/cpp_target_config.cpp new file mode 100644 index 000000000..e3d413702 --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/cpp_target_config.cpp @@ -0,0 +1,66 @@ +#include "cpp_target_config.h" + +#if defined(SHIFTERBUTTONS) || defined(SPIBUTTONS) +extern SPI_HandleTypeDef hspi2; +static const std::vector external_spi_cspins{OutputPin(*SPI2_NSS_GPIO_Port, SPI2_NSS_Pin), OutputPin(*SPI2_SS2_GPIO_Port, SPI2_SS2_Pin),OutputPin(*SPI2_SS3_GPIO_Port, SPI2_SS3_Pin)}; +SPIPort external_spi{hspi2,external_spi_cspins,true}; +#endif + +static const std::vector motor_spi_cspins{OutputPin(*SPI1_SS1_GPIO_Port, SPI1_SS1_Pin), OutputPin(*SPI1_SS2_GPIO_Port, SPI1_SS2_Pin),OutputPin(*SPI1_SS3_GPIO_Port, SPI1_SS3_Pin)}; +extern SPI_HandleTypeDef hspi1; +SPIPort motor_spi{hspi1,motor_spi_cspins,false}; + +#ifdef EXT3_SPI_PORT +static const std::vector ext3_spi_cspins{OutputPin(*SPI3_SS1_GPIO_Port, SPI3_SS1_Pin), OutputPin(*SPI3_SS2_GPIO_Port, SPI3_SS2_Pin),OutputPin(*SPI3_SS3_GPIO_Port, SPI3_SS3_Pin)}; +extern SPI_HandleTypeDef EXT3_SPI_PORT; +SPIPort ext3_spi{hspi3,ext3_spi_cspins,true}; +#endif + +#ifdef UART_PORT_MOTOR +extern UART_HandleTypeDef UART_PORT_MOTOR; +UARTPort motor_uart{UART_PORT_MOTOR}; +#endif + +#ifdef UART_PORT_EXT +extern UART_HandleTypeDef UART_PORT_EXT; +UARTPort external_uart{UART_PORT_EXT}; +#endif + +#ifdef I2C_PORT +extern I2C_HandleTypeDef I2C_PORT; +I2CPort i2cport{I2C_PORT}; +#endif + +#ifdef CANBUS +/* + * Can BTR register for different speed configs + * 50, 100, 125, 250, 500, 1000 kbit + */ +CANPort canport{CANPORT}; + +#endif + +#ifdef PWMDRIVER +// CCR and channels must match! +const PWMConfig pwmTimerConfig = +{ + .channel_1 = TIM_CHANNEL_1, + .channel_2 = TIM_CHANNEL_2, + .channel_3 = TIM_CHANNEL_3, + .channel_4 = TIM_CHANNEL_4, + + .pwm_chan = 1, + .dir_chan = 3, + .dir_chan_n = 4, + + .centerpwm_chan = 1, + + .rcpwm_chan = 1, + + .dualpwm1 = 1, + .dualpwm2 = 2, + + .timer = &TIM_PWM, + .timerFreq = APB_CLK_FREQ +}; +#endif diff --git a/Firmware/Targets/ESP32SX/main/glue.c b/Firmware/Targets/ESP32SX/main/glue.c new file mode 100644 index 000000000..833c7b4e7 --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/glue.c @@ -0,0 +1,700 @@ +#include +#include +#include +#include "esp_log.h" +#include "esp_wifi.h" +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "hal/pcnt_ll.h" +#include "hal/gpio_ll.h" +#include "nvs_flash.h" +#include "glue.h" +#include "target_constants.h" + +static const char *TAG = "glue"; + +ADC_HandleTypeDef hadc1; +SPI_HandleTypeDef hspi1; +#if defined(SHIFTERBUTTONS) || defined(SPIBUTTONS) +SPI_HandleTypeDef hspi2; +#endif +UART_HandleTypeDef huart1; +UART_HandleTypeDef huart3; +CAN_HandleTypeDef hcan1; +TIM_HandleTypeDef htim1; +I2C_HandleTypeDef hi2c1; + + +void HAL_GPIO_Init(GPIO_TypeDef *GPIOx, GPIO_InitTypeDef *GPIO_Init) +{ + gpio_config_t io_conf = {0}; + io_conf.intr_type = GPIO_INTR_DISABLE; + io_conf.mode = GPIO_Init->Mode; + io_conf.pin_bit_mask = (BIT64(LED_CLIP_Pin) | BIT64(LED_ERR_Pin) \ + | BIT64(LED_SYS_Pin) | BIT64(DRV_ENABLE_Pin) \ + | BIT64(DRV_BRAKE_Pin) | BIT64(CAN_S_Pin)); + io_conf.pull_down_en = 0; + io_conf.pull_up_en = 0; + gpio_config(&io_conf); +} + +GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin) +{ + return gpio_ll_get_level(&GPIO, GPIO_Pin); +} + +void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState) +{ + gpio_ll_set_level(&GPIO, GPIO_Pin, PinState); +} + +void HAL_GPIO_TogglePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin) +{ + gpio_dev_t *hw = &GPIO; + if (GPIO_Pin < 32) { + hw->out ^= (1 << GPIO_Pin); + + } else { + hw->out1.data ^= (1 << (GPIO_Pin - 32)); + } +} + +void glue_pcnt_init(void) +{ + ESP_LOGI(TAG, "%s", __FUNCTION__); + +#define PCNT_UNIT PCNT_UNIT_0 +#define PCNT_H_LIM_VAL 30000 +#define PCNT_L_LIM_VAL -30000 + /* Prepare configuration for the PCNT unit */ + pcnt_config_t pcnt_config; + + // ch0 + pcnt_config.pulse_gpio_num = ENCODER_A_Pin; + pcnt_config.ctrl_gpio_num = ENCODER_B_Pin; + pcnt_config.channel = PCNT_CHANNEL_0; + pcnt_config.pos_mode = PCNT_COUNT_INC; // Count up on the positive edge + pcnt_config.neg_mode = PCNT_COUNT_DEC; // Keep the counter value on the negative edge + + pcnt_config.lctrl_mode = PCNT_MODE_REVERSE; // Reverse counting direction if low + pcnt_config.hctrl_mode = PCNT_MODE_KEEP; // Keep the primary counter mode if high + pcnt_config.counter_h_lim = PCNT_H_LIM_VAL; + pcnt_config.counter_l_lim = PCNT_L_LIM_VAL; + pcnt_config.unit = PCNT_UNIT; + pcnt_unit_config(&pcnt_config); + + // ch1 + pcnt_config.pulse_gpio_num = ENCODER_B_Pin; + pcnt_config.ctrl_gpio_num = ENCODER_A_Pin; + pcnt_config.channel = PCNT_CHANNEL_1; + pcnt_config.pos_mode = PCNT_COUNT_DEC; // Count up on the positive edge + pcnt_config.neg_mode = PCNT_COUNT_INC; // Keep the counter value on the negative edge + pcnt_unit_config(&pcnt_config); + + + /* Configure and enable the input filter */ + pcnt_set_filter_value(PCNT_UNIT, 100); + pcnt_filter_enable(PCNT_UNIT); + + /* Enable events on zero, maximum and minimum limit values */ + pcnt_event_enable(PCNT_UNIT, PCNT_EVT_ZERO); + pcnt_event_enable(PCNT_UNIT, PCNT_EVT_H_LIM); + pcnt_event_enable(PCNT_UNIT, PCNT_EVT_L_LIM); + + /* Initialize PCNT's counter */ + pcnt_counter_pause(PCNT_UNIT); + pcnt_counter_clear(PCNT_UNIT); + + /* Everything is set up, now go to counting */ + pcnt_counter_resume(PCNT_UNIT); +} + +int16_t glue_pcnt_get_delta_value(void) +{ + int16_t count; + pcnt_get_counter_value(PCNT_UNIT, &count); + pcnt_counter_clear(PCNT_UNIT); + return count; +} + +void glue_pcnt_deinit(void) +{ + // pcnt_intr_disable(PCNT_UNIT); + pcnt_counter_pause(PCNT_UNIT); +} + + +HAL_StatusTypeDef HAL_SPI_Init(SPI_HandleTypeDef *hspi) +{ + if (hspi != &hspi1) { + ESP_LOGE(TAG, "Can't init spi2 on esp32s2, don't have enough gpio"); + return HAL_ERROR; + } + + esp_err_t ret; + spi_bus_config_t buscfg1 = { + .miso_io_num = SPI1_MISO_Pin, + .mosi_io_num = SPI1_MOSI_Pin, + .sclk_io_num = SPI1_SCK_Pin, + .quadwp_io_num = -1, + .quadhd_io_num = -1, + .max_transfer_sz = 4096, + }; + hspi->Init.BaudRatePrescaler >>= 3; + uint32_t freq = (84 * 1000 * 1000) / (uint32_t)(powf(2, hspi->Init.BaudRatePrescaler + 1)); + ESP_LOGI(TAG, "%s, freq=%d, Prescaler=%d", __FUNCTION__, freq, hspi->Init.BaudRatePrescaler); + spi_device_interface_config_t devcfg1 = { + .clock_speed_hz = freq, + .mode = ((hspi->Init.CLKPolarity) << 1) | (hspi->Init.CLKPhase), + .spics_io_num = SPI1_SS1_Pin, //CS pin + .queue_size = 7, //We want to be able to queue 7 transactions at a time + }; + + //Initialize the SPI bus + ret = spi_bus_initialize(SPI2_HOST, &buscfg1, SPI_DMA_CH_AUTO); + ESP_ERROR_CHECK(ret); + //Attach the LCD to the SPI bus + ret = spi_bus_add_device(SPI2_HOST, &devcfg1, &hspi1.Instance); + ESP_ERROR_CHECK(ret); + return HAL_OK; +} + +HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout) +{ + esp_err_t ret; + spi_transaction_t t = {0}; + t.length = 8 * Size; //Command is 8 bits + t.tx_buffer = pData; //The data is the cmd itself + ESP_LOGD(TAG, "%s, len=%d", __FUNCTION__, Size); + ret = spi_device_polling_transmit(hspi->Instance, &t); //Transmit! + assert(ret == ESP_OK); //Should have had no issues. + return HAL_OK; +} + +HAL_StatusTypeDef HAL_SPI_Transmit_DMA(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size) +{ + return HAL_SPI_Transmit(hspi, pData, Size, 500); +} + +HAL_StatusTypeDef HAL_SPI_Transmit_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_SPI_TransmitReceive_IT(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_SPI_Receive_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, uint32_t Timeout) +{ + esp_err_t ret; + spi_transaction_t t = {0}; + t.length = 8 * Size; //Command is 8 bits + t.tx_buffer = pTxData; //The data is the cmd itself + t.rx_buffer = pRxData; + ESP_LOGD(TAG, "%s, len=%d", __FUNCTION__, Size); + ret = spi_device_polling_transmit(hspi->Instance, &t); //Transmit! + assert(ret == ESP_OK); //Should have had no issues. + return HAL_OK; +} + +HAL_StatusTypeDef HAL_UART_Init(UART_HandleTypeDef *huart) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_Receive_IT(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_Transmit_DMA(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_Transmit(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size, uint32_t Timeout) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_Transmit_IT(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_Receive(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size, uint32_t Timeout) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_Receive_DMA(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_AbortReceive(UART_HandleTypeDef *huart) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} +HAL_StatusTypeDef HAL_UART_AbortTransmit(UART_HandleTypeDef *huart) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} + +HAL_StatusTypeDef HAL_I2C_Init(I2C_HandleTypeDef *hi2c) +{ + hi2c->Instance = I2C_NUM_0; + i2c_config_t conf = { + .mode = I2C_MODE_MASTER, + .sda_io_num = DIN2_Pin, + .scl_io_num = DIN3_Pin, + .sda_pullup_en = GPIO_PULLUP_ENABLE, + .scl_pullup_en = GPIO_PULLUP_ENABLE, + .master.clk_speed = hi2c->Init.ClockSpeed, + }; + + i2c_param_config(hi2c->Instance, &conf); + +#define I2C_MASTER_TX_BUF_DISABLE 0 /*!< I2C master do not need buffer */ +#define I2C_MASTER_RX_BUF_DISABLE 0 /*!< I2C master do not need buffer */ + + esp_err_t ret = i2c_driver_install(hi2c->Instance, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0); + return ESP_OK == ret ? HAL_OK : HAL_ERROR; +} + +HAL_StatusTypeDef HAL_I2C_DeInit(I2C_HandleTypeDef *hi2c) +{ + i2c_driver_delete(hi2c->Instance); + return HAL_OK; +} + +HAL_StatusTypeDef HAL_I2C_Master_Transmit(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size, uint32_t Timeout) +{ + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, DevAddress, true); + i2c_master_write(cmd, pData, Size, true); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(hi2c->Instance, cmd, pdMS_TO_TICKS(Timeout)); + i2c_cmd_link_delete(cmd); + return ESP_OK == ret ? HAL_OK : HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Master_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Master_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size) +{ + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, DevAddress, true); + i2c_master_write(cmd, pData, Size, true); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(hi2c->Instance, cmd, pdMS_TO_TICKS(1000)); + i2c_cmd_link_delete(cmd); + + extern void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef * hi2c); + HAL_I2C_MasterTxCpltCallback(hi2c); // call the callback + + return ESP_OK == ret ? HAL_OK : HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Master_Receive(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size, uint32_t Timeout) +{ + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, DevAddress, true); + if (Size > 1) { + i2c_master_read(cmd, pData, Size - 1, I2C_MASTER_ACK); + } + i2c_master_read_byte(cmd, pData + Size - 1, I2C_MASTER_NACK); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(hi2c->Instance, cmd, pdMS_TO_TICKS(Timeout)); + i2c_cmd_link_delete(cmd); + return ESP_OK == ret ? HAL_OK : HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Master_Receive_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size) +{ + i2c_cmd_handle_t cmd = i2c_cmd_link_create(); + i2c_master_start(cmd); + i2c_master_write_byte(cmd, DevAddress, true); + if (Size > 1) { + i2c_master_read(cmd, pData, Size - 1, I2C_MASTER_ACK); + } + i2c_master_read_byte(cmd, pData + Size - 1, I2C_MASTER_NACK); + i2c_master_stop(cmd); + esp_err_t ret = i2c_master_cmd_begin(hi2c->Instance, cmd, pdMS_TO_TICKS(1000)); + i2c_cmd_link_delete(cmd); + + extern void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef * hi2c); + HAL_I2C_MasterRxCpltCallback(hi2c); // call the callback + return ESP_OK == ret ? HAL_OK : HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Master_Receive_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Mem_Write(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size, uint32_t Timeout) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Mem_Write_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Mem_Read(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size, uint32_t Timeout) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} +HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_ERROR; +} + +void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); +} + +HAL_StatusTypeDef HAL_TIM_PWM_Stop(TIM_HandleTypeDef *htim, uint32_t Channel) +{ + ledc_stop(htim->speed_mode, htim->channel, 0); + return HAL_OK; +} +HAL_StatusTypeDef HAL_TIM_Base_Stop_IT(TIM_HandleTypeDef *htim) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} + + +void glue_board_init(void) +{ + ESP_LOGI(TAG, "Board name: %s", OPENFFBOARD_NAME); + gpio_config_t io_conf = {0}; + io_conf.intr_type = GPIO_INTR_DISABLE; + io_conf.mode = GPIO_MODE_OUTPUT; + io_conf.pin_bit_mask = (BIT64(LED_CLIP_Pin) | BIT64(LED_ERR_Pin) \ + | BIT64(LED_SYS_Pin) | BIT64(DRV_ENABLE_Pin) \ + | BIT64(DRV_BRAKE_Pin) | BIT64(CAN_S_Pin)); + io_conf.pull_down_en = 0; + io_conf.pull_up_en = 0; + gpio_config(&io_conf); + io_conf.mode = GPIO_MODE_INPUT; + io_conf.pull_up_en = 1; + io_conf.pin_bit_mask = BIT64(BUTTON_A_Pin) \ + | BIT64(DIN2_Pin) | BIT64(DIN1_Pin) | BIT64(DIN0_Pin); +#if DIN3_Pin < SOC_GPIO_PIN_COUNT + io_conf.pin_bit_mask |= BIT64(DIN3_Pin); +#endif + + gpio_config(&io_conf); + + HAL_GPIO_WritePin(GPIOA, CAN_S_Pin, GPIO_PIN_RESET); + glue_can_set_speed(&hcan1, 500000); + HAL_CAN_Start(&hcan1); +} + +void glue_ledc_config(ledc_channel_t channel, ledc_timer_bit_t duty_resolution, uint32_t freq_hz) +{ + ESP_LOGI(TAG, "%s, ch=%d, res=%d, f=%d", __FUNCTION__, channel, duty_resolution, freq_hz); +#define LEDC_TIMER LEDC_TIMER_0 + // Set the LEDC peripheral configuration + ledc_timer_config_t ledc_timer = { + .speed_mode = LEDC_LOW_SPEED_MODE, + .timer_num = LEDC_TIMER, + .duty_resolution = duty_resolution, + .freq_hz = freq_hz, // Set output frequency at 5 kHz + .clk_cfg = LEDC_AUTO_CLK + }; + ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); + + const int pwm_pins[4] = {PWM1_Pin, PWM2_Pin, PWM3_Pin, PWM4_Pin}; + // Prepare and then apply the LEDC PWM channel configuration + ledc_channel_config_t ledc_channel = { + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = channel, + .timer_sel = LEDC_TIMER, + .intr_type = LEDC_INTR_DISABLE, + .gpio_num = pwm_pins[channel], + .duty = 0, // Set duty to 0% + .hpoint = 0 + }; + ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel)); +} + +void glue_ledc_set_duty(ledc_channel_t channel, uint32_t duty) +{ + ESP_LOGI(TAG, "%s, ch=%d, d=%d", __FUNCTION__, channel, duty); + ledc_set_duty(LEDC_LOW_SPEED_MODE, channel, duty); + ledc_update_duty(LEDC_LOW_SPEED_MODE, channel); +} + +void glue_can_set_speed(CAN_HandleTypeDef *hcan, uint32_t rate) +{ + twai_timing_config_t t_50_config = TWAI_TIMING_CONFIG_50KBITS(); + twai_timing_config_t t_100_config = TWAI_TIMING_CONFIG_100KBITS(); + twai_timing_config_t t_125_config = TWAI_TIMING_CONFIG_125KBITS(); + twai_timing_config_t t_250_config = TWAI_TIMING_CONFIG_250KBITS(); + twai_timing_config_t t_500_config = TWAI_TIMING_CONFIG_500KBITS(); + twai_timing_config_t t_1m_config = TWAI_TIMING_CONFIG_1MBITS(); + switch (rate) { + case 50000: + hcan->t_config = t_50_config; + break; + case 100000: + hcan->t_config = t_100_config; + break; + case 125000: + hcan->t_config = t_125_config; + break; + case 250000: + hcan->t_config = t_250_config; + break; + case 500000: + hcan->t_config = t_500_config; + break; + case 1000000: + hcan->t_config = t_1m_config; + break; + + default: + ESP_LOGE(TAG, "Unsupported CAN rate"); + break; + } + ESP_LOGI(TAG, "set CAN rate to %d", rate); +} + + +static void twai_receive_task(void *arg) +{ + CAN_HandleTypeDef *hcan = (CAN_HandleTypeDef *)arg; + CAN_RxHeaderTypeDef rxHeader; + uint8_t rxBuf[8]; + while (1) { + twai_message_t rx_message; + //Receive message and print message data + twai_receive(&rx_message, portMAX_DELAY); + memcpy(rxBuf, rx_message.data, rx_message.data_length_code); + rxHeader.DLC = rx_message.data_length_code; + rxHeader.ExtId = rx_message.identifier; + rxHeader.StdId = rx_message.identifier; + rxHeader.RTR = rx_message.rtr ? CAN_RTR_REMOTE : CAN_RTR_DATA; + rxHeader.Timestamp = HAL_GetTick(); + rxHeader.IDE = rx_message.extd ? CAN_ID_EXT : CAN_ID_STD; + + // for (int i = 0; i < rx_message.data_length_code; i++) { + // printf("%X, ", rx_message.data[i] ); + // } printf("\n"); + glue_can_receive_msg(hcan, rxBuf, &rxHeader); + } + vTaskDelete(NULL); +} + + +HAL_StatusTypeDef HAL_CAN_Start(CAN_HandleTypeDef *hcan) +{ + twai_status_info_t info; + twai_get_status_info(&info); + if (TWAI_STATE_RUNNING == info.state) { + ESP_LOGW(TAG, "TWAI Driver is already running"); + return HAL_ERROR; + } + + //Filter + twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); + // + twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_Pin, CAN_RX_Pin, TWAI_MODE_NORMAL); + //Install TWAI driver + esp_err_t ret = twai_driver_install(&g_config, &hcan->t_config, &f_config); + if (ESP_ERR_INVALID_STATE == ret) { + ESP_LOGW(TAG, "TWAI Driver is already installed"); + } + ESP_LOGI(TAG, "TWAI Driver installed"); + ESP_ERROR_CHECK(twai_start()); + xTaskCreatePinnedToCore(twai_receive_task, "TWAI_rx", 4096, hcan, 6, &hcan->task, 0); + ESP_LOGI(TAG, "TWAI Driver started"); + return HAL_OK; +} + +HAL_StatusTypeDef HAL_CAN_ConfigFilter(CAN_HandleTypeDef *hcan, CAN_FilterTypeDef *sFilterConfig) +{ + ESP_LOGW(TAG, "unsupport config filter"); + return HAL_OK; +} + +HAL_StatusTypeDef HAL_CAN_Stop(CAN_HandleTypeDef *hcan) +{ + vTaskDelete(hcan->task); + ESP_ERROR_CHECK(twai_stop()); + ESP_ERROR_CHECK(twai_driver_uninstall()); + ESP_LOGI(TAG, "Driver uninstalled"); + return HAL_OK; +} + +HAL_StatusTypeDef HAL_CAN_AddTxMessage(CAN_HandleTypeDef *hcan, CAN_TxHeaderTypeDef *pHeader, uint8_t aData[], uint32_t *pTxMailbox) +{ + twai_message_t msg = {0}; + memcpy(msg.data, aData, pHeader->DLC); + msg.rtr = (pHeader->RTR == CAN_RTR_REMOTE); + msg.data_length_code = pHeader->DLC; + msg.extd = (pHeader->IDE == CAN_ID_EXT); + msg.identifier = pHeader->ExtId; + ESP_LOGD(TAG, "send id=0x%x, dlc=%d, extd=%d, rtr=%d, ss=%d", + msg.identifier, + msg.data_length_code, + (int)msg.extd, + (int)msg.rtr, + (int)msg.ss); + // printf("send data:"); + // for (int i = 0; i < msg.data_length_code; i++) { + // printf("%X, ", msg.data[i] ); + // } printf("\n"); + esp_err_t ret = twai_transmit(&msg, pdMS_TO_TICKS(1000)); + if (ret != ESP_OK) { + ESP_LOGE(TAG, "twai error %s", esp_err_to_name(ret)); + } + return ret == ESP_OK ? HAL_OK : HAL_ERROR; +} + +HAL_StatusTypeDef HAL_CAN_AbortTxRequest(CAN_HandleTypeDef *hcan, uint32_t TxMailboxes) +{ + twai_clear_transmit_queue(); + twai_clear_receive_queue(); + return HAL_OK; +} +HAL_StatusTypeDef HAL_CAN_ResetError(CAN_HandleTypeDef *hcan) +{ + ESP_LOGW(TAG, "%s: Unsupported", __FUNCTION__); + return HAL_OK; +} + +static void continuous_adc_init(uint16_t adc1_chan_mask, uint16_t adc2_chan_mask, adc_channel_t *channel, uint8_t channel_num) +{ +#define GET_UNIT(x) ((x>>3) & 0x1) + adc_digi_init_config_t adc_dma_config = { + .max_store_buf_size = channel_num * 4 * 2, + .conv_num_each_intr = channel_num * 4 * 2, + .adc1_chan_mask = adc1_chan_mask, + .adc2_chan_mask = adc2_chan_mask, + }; + ESP_ERROR_CHECK(adc_digi_initialize(&adc_dma_config)); + + adc_digi_configuration_t dig_cfg = { + .conv_limit_en = 0, + .conv_limit_num = adc_dma_config.conv_num_each_intr, + .sample_freq_hz = 1 * 1000, + .conv_mode = ADC_CONV_SINGLE_UNIT_1, +#ifdef CONFIG_IDF_TARGET_ESP32S3 + .format = ADC_DIGI_OUTPUT_FORMAT_TYPE2, +#elif defined CONFIG_IDF_TARGET_ESP32S2 + .format = ADC_DIGI_OUTPUT_FORMAT_TYPE1, +#endif + }; + + adc_digi_pattern_config_t adc_pattern[SOC_ADC_PATT_LEN_MAX] = {0}; + dig_cfg.pattern_num = channel_num; + for (int i = 0; i < channel_num; i++) { + uint8_t unit = GET_UNIT(channel[i]); + uint8_t ch = channel[i] & 0x7; + adc_pattern[i].atten = ADC_ATTEN_DB_11; + adc_pattern[i].channel = ch; + adc_pattern[i].unit = unit; + adc_pattern[i].bit_width = SOC_ADC_DIGI_MAX_BITWIDTH; + + ESP_LOGI(TAG, "adc_pattern[%d].atten is :%x", i, adc_pattern[i].atten); + ESP_LOGI(TAG, "adc_pattern[%d].channel is :%x", i, adc_pattern[i].channel); + } + dig_cfg.adc_pattern = adc_pattern; + ESP_ERROR_CHECK(adc_digi_controller_configure(&dig_cfg)); +} + +HAL_StatusTypeDef HAL_ADC_Start_DMA(ADC_HandleTypeDef *hadc, uint32_t *pData, uint32_t Length) +{ + if (3 != Length) { + ESP_LOGE(TAG, "ADC channels error"); + return HAL_ERROR; + } + + static uint16_t adc1_chan_mask = BIT(0) | BIT(1) | BIT(2); + adc_channel_t channel[3] = {ADC1_CHANNEL_0, ADC1_CHANNEL_1, ADC1_CHANNEL_2}; + continuous_adc_init(adc1_chan_mask, 0, channel, sizeof(channel) / sizeof(adc_channel_t)); + adc_digi_start(); + + // volatile uint32_t* getAnalogBuffer(ADC_HandleTypeDef* hadc,uint8_t* chans); + // while (1) + // { + // uint8_t c=0; + // uint32_t *p = getAnalogBuffer(NULL, &c); + // ESP_LOGI(TAG, "%d, %d, %d", p[0], p[1], p[2]); + // vTaskDelay(10); + // } + + return HAL_OK; +} + +void Error_Handler(void) +{ + ESP_LOGE(TAG, "Error_Handler!!!"); + assert(false); +} + + +void RebootDFU(void) +{ + ESP_LOGE(TAG, "Unsupport DFU"); +} + +/** + * @brief Returns the device revision identifier. + * @retval Device revision identifier + */ +uint32_t HAL_GetREVID(void) +{ + return 0x01; +} + +/** + * @brief Returns the device identifier. + * @retval Device identifier + */ +uint32_t HAL_GetDEVID(void) +{ + return FW_DEVID; +} + +uint32_t HAL_GetUIDw0(void) +{ + uint8_t mac[6]; + esp_wifi_get_mac(WIFI_IF_STA, mac); + uint32_t ret = (uint32_t)mac[1] << 8 | mac[0]; + return ret; +} +uint32_t HAL_GetUIDw1(void) +{ + uint8_t mac[6]; + esp_wifi_get_mac(WIFI_IF_STA, mac); + uint32_t ret = (uint32_t)mac[3] << 8 | mac[2]; + return ret; +} +uint32_t HAL_GetUIDw2(void) +{ + uint8_t mac[6]; + esp_wifi_get_mac(WIFI_IF_STA, mac); + uint32_t ret = (uint32_t)mac[5] << 8 | mac[4]; + return ret; +} diff --git a/Firmware/Targets/ESP32SX/main/glue.h b/Firmware/Targets/ESP32SX/main/glue.h new file mode 100644 index 000000000..9a052e2d1 --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/glue.h @@ -0,0 +1,138 @@ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __GLUE_H +#define __GLUE_H + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "driver/gpio.h" +#include "driver/pcnt.h" +#include "driver/ledc.h" +#include "driver/twai.h" +#include "driver/adc.h" +#include "driver/i2c.h" +#include "driver/spi_master.h" +#include "sdkconfig.h" +#include "stm32f4xx_hal.h" +#include "target_constants.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Exported macro ------------------------------------------------------------*/ +#define HAL_GetTick() (esp_timer_get_time()/1000) +#define NVIC_SystemReset() esp_restart() + +#define HAL_FLASH_Unlock() +#define HAL_FLASH_Lock() + +/** + \brief Reverse byte order (16 bit) + \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000. + \param [in] value Value to reverse + \return Reversed value + */ +static inline int16_t __REVSH(int16_t value) +{ +#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8) + return (int16_t)__builtin_bswap16(value); +#else + int16_t result; + + __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); + return result; +#endif +} + +#define TIM_CHANNEL_1 LEDC_CHANNEL_0 +#define TIM_CHANNEL_2 LEDC_CHANNEL_1 +#define TIM_CHANNEL_3 LEDC_CHANNEL_2 +#define TIM_CHANNEL_4 LEDC_CHANNEL_3 + +/* Private defines -----------------------------------------------------------*/ +#ifdef CONFIG_IDF_TARGET_ESP32S3 +#include "./openffboard_esp32s3_v1.1_pins.h" +#elif defined CONFIG_IDF_TARGET_ESP32S2 +#ifdef OPENFFBOARD_ESP32S2_V1_1 +#include "./openffboard_esp32s2_v1.1_pins.h" +#elif defined OPENFFBOARD_ESP32S2_V1_0 +#include "./openffboard_esp32s2_v1.0_pins.h" +#endif +#endif + +void HAL_GPIO_Init(GPIO_TypeDef *GPIOx, GPIO_InitTypeDef *GPIO_Init); +GPIO_PinState HAL_GPIO_ReadPin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin); +void HAL_GPIO_WritePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin, GPIO_PinState PinState); +void HAL_GPIO_TogglePin(GPIO_TypeDef *GPIOx, uint16_t GPIO_Pin); +uint32_t HAL_GetREVID(void); +uint32_t HAL_GetDEVID(void); +uint32_t HAL_GetUIDw0(void); +uint32_t HAL_GetUIDw1(void); +uint32_t HAL_GetUIDw2(void); +void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim); +HAL_StatusTypeDef HAL_TIM_PWM_Stop(TIM_HandleTypeDef *htim, uint32_t Channel); +HAL_StatusTypeDef HAL_TIM_Base_Stop_IT(TIM_HandleTypeDef *htim); +HAL_StatusTypeDef HAL_SPI_Init(SPI_HandleTypeDef *hspi); +HAL_StatusTypeDef HAL_SPI_Transmit(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_SPI_Transmit_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_SPI_TransmitReceive_IT(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size); +HAL_StatusTypeDef HAL_SPI_Receive_IT(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_SPI_Receive(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, uint16_t Size, + uint32_t Timeout); +HAL_StatusTypeDef HAL_SPI_Transmit_DMA(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_SPI_Receive_DMA(SPI_HandleTypeDef *hspi, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_SPI_TransmitReceive_DMA(SPI_HandleTypeDef *hspi, uint8_t *pTxData, uint8_t *pRxData, + uint16_t Size); + +HAL_StatusTypeDef HAL_I2C_Init(I2C_HandleTypeDef *hi2c); +HAL_StatusTypeDef HAL_I2C_DeInit(I2C_HandleTypeDef *hi2c); +HAL_StatusTypeDef HAL_I2C_Master_Transmit(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_I2C_Master_Transmit_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_I2C_Master_Transmit_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_I2C_Master_Receive(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_I2C_Master_Receive_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_I2C_Master_Receive_DMA(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_I2C_Mem_Write(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_I2C_Mem_Write_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_I2C_Mem_Read(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_I2C_Mem_Read_IT(I2C_HandleTypeDef *hi2c, uint16_t DevAddress, uint16_t MemAddress, uint16_t MemAddSize, uint8_t *pData, uint16_t Size); + +HAL_StatusTypeDef HAL_UART_Init(UART_HandleTypeDef *huart); +HAL_StatusTypeDef HAL_UART_Receive_IT(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_UART_Transmit_DMA(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_UART_Transmit(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_UART_Transmit_IT(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_UART_Receive(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size, uint32_t Timeout); +HAL_StatusTypeDef HAL_UART_Receive_DMA(UART_HandleTypeDef *huart, uint8_t *pData, uint16_t Size); +HAL_StatusTypeDef HAL_UART_AbortReceive(UART_HandleTypeDef *huart); +HAL_StatusTypeDef HAL_UART_AbortTransmit(UART_HandleTypeDef *huart); + +HAL_StatusTypeDef HAL_ADC_Start_DMA(ADC_HandleTypeDef *hadc, uint32_t *pData, uint32_t Length); + +void Error_Handler(void); +void RebootDFU(); + +void glue_board_init(void); +void glue_pcnt_init(void); +int16_t glue_pcnt_get_delta_value(void); +void glue_pcnt_deinit(void); +void glue_ledc_config(ledc_channel_t channel, ledc_timer_bit_t duty_resolution, uint32_t freq_hz); +void glue_ledc_set_duty(ledc_channel_t channel, uint32_t duty); +void glue_can_set_speed(CAN_HandleTypeDef *hcan, uint32_t rate); +void glue_can_receive_msg(CAN_HandleTypeDef *hcan, uint8_t *rxBuf, CAN_RxHeaderTypeDef *rxHeader); +HAL_StatusTypeDef HAL_CAN_Start(CAN_HandleTypeDef *hcan); +HAL_StatusTypeDef HAL_CAN_ConfigFilter(CAN_HandleTypeDef *hcan, CAN_FilterTypeDef *sFilterConfig); +HAL_StatusTypeDef HAL_CAN_Stop(CAN_HandleTypeDef *hcan); +HAL_StatusTypeDef HAL_CAN_AddTxMessage(CAN_HandleTypeDef *hcan, CAN_TxHeaderTypeDef *pHeader, uint8_t aData[], uint32_t *pTxMailbox); +HAL_StatusTypeDef HAL_CAN_AbortTxRequest(CAN_HandleTypeDef *hcan, uint32_t TxMailboxes); +HAL_StatusTypeDef HAL_CAN_ResetError(CAN_HandleTypeDef *hcan); + +#ifdef __cplusplus +} +#endif + +#endif /* __GLUE_H */ diff --git a/Firmware/Targets/ESP32SX/main/glue_stm32f4xx_hal_spi.h b/Firmware/Targets/ESP32SX/main/glue_stm32f4xx_hal_spi.h new file mode 100644 index 000000000..26b788440 --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/glue_stm32f4xx_hal_spi.h @@ -0,0 +1,304 @@ +/** + ****************************************************************************** + * @file stm32f4xx_hal_spi.h + * @author MCD Application Team + * @brief Header file of SPI HAL module. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2016 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef STM32F4xx_HAL_SPI_H +#define STM32F4xx_HAL_SPI_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "driver/spi_master.h" + +/** @addtogroup STM32F4xx_HAL_Driver + * @{ + */ + +/** @addtogroup SPI + * @{ + */ + +/* Exported types ------------------------------------------------------------*/ +/** @defgroup SPI_Exported_Types SPI Exported Types + * @{ + */ + +/** + * @brief SPI Configuration Structure definition + */ +typedef struct +{ + uint32_t Mode; /*!< Specifies the SPI operating mode. + This parameter can be a value of @ref SPI_Mode */ + + uint32_t Direction; /*!< Specifies the SPI bidirectional mode state. + This parameter can be a value of @ref SPI_Direction */ + + uint32_t DataSize; /*!< Specifies the SPI data size. + This parameter can be a value of @ref SPI_Data_Size */ + + uint32_t CLKPolarity; /*!< Specifies the serial clock steady state. + This parameter can be a value of @ref SPI_Clock_Polarity */ + + uint32_t CLKPhase; /*!< Specifies the clock active edge for the bit capture. + This parameter can be a value of @ref SPI_Clock_Phase */ + + uint32_t NSS; /*!< Specifies whether the NSS signal is managed by + hardware (NSS pin) or by software using the SSI bit. + This parameter can be a value of @ref SPI_Slave_Select_management */ + + uint32_t BaudRatePrescaler; /*!< Specifies the Baud Rate prescaler value which will be + used to configure the transmit and receive SCK clock. + This parameter can be a value of @ref SPI_BaudRate_Prescaler + @note The communication clock is derived from the master + clock. The slave clock does not need to be set. */ + + uint32_t FirstBit; /*!< Specifies whether data transfers start from MSB or LSB bit. + This parameter can be a value of @ref SPI_MSB_LSB_transmission */ + + uint32_t TIMode; /*!< Specifies if the TI mode is enabled or not. + This parameter can be a value of @ref SPI_TI_mode */ + + uint32_t CRCCalculation; /*!< Specifies if the CRC calculation is enabled or not. + This parameter can be a value of @ref SPI_CRC_Calculation */ + + uint32_t CRCPolynomial; /*!< Specifies the polynomial used for the CRC calculation. + This parameter must be an odd number between Min_Data = 1 and Max_Data = 65535 */ +} SPI_InitTypeDef; + +/** + * @brief HAL SPI State structure definition + */ +typedef enum +{ + HAL_SPI_STATE_RESET = 0x00U, /*!< Peripheral not Initialized */ + HAL_SPI_STATE_READY = 0x01U, /*!< Peripheral Initialized and ready for use */ + HAL_SPI_STATE_BUSY = 0x02U, /*!< an internal process is ongoing */ + HAL_SPI_STATE_BUSY_TX = 0x03U, /*!< Data Transmission process is ongoing */ + HAL_SPI_STATE_BUSY_RX = 0x04U, /*!< Data Reception process is ongoing */ + HAL_SPI_STATE_BUSY_TX_RX = 0x05U, /*!< Data Transmission and Reception process is ongoing */ + HAL_SPI_STATE_ERROR = 0x06U, /*!< SPI error state */ + HAL_SPI_STATE_ABORT = 0x07U /*!< SPI abort is ongoing */ +} HAL_SPI_StateTypeDef; + +/** + * @brief SPI handle Structure definition + */ +typedef struct __SPI_HandleTypeDef +{ + spi_device_handle_t Instance; + + SPI_InitTypeDef Init; /*!< SPI communication parameters */ + +#if (USE_HAL_SPI_REGISTER_CALLBACKS == 1U) + void (* TxCpltCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Tx Completed callback */ + void (* RxCpltCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Rx Completed callback */ + void (* TxRxCpltCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI TxRx Completed callback */ + void (* TxHalfCpltCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Tx Half Completed callback */ + void (* RxHalfCpltCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Rx Half Completed callback */ + void (* TxRxHalfCpltCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI TxRx Half Completed callback */ + void (* ErrorCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Error callback */ + void (* AbortCpltCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Abort callback */ + void (* MspInitCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Msp Init callback */ + void (* MspDeInitCallback)(struct __SPI_HandleTypeDef *hspi); /*!< SPI Msp DeInit callback */ + +#endif /* USE_HAL_SPI_REGISTER_CALLBACKS */ +} SPI_HandleTypeDef; + + +/******************* Bit definition for SPI_CR1 register ********************/ +#define SPI_CR1_CPHA_Pos (0U) +#define SPI_CR1_CPHA_Msk (0x1UL << SPI_CR1_CPHA_Pos) /*!< 0x00000001 */ +#define SPI_CR1_CPHA SPI_CR1_CPHA_Msk /*! 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5) + return __builtin_bswap32(value); +#else + uint32_t result; + + __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) ); + return result; +#endif +} + +#ifndef __ALIGN_END +#define __ALIGN_END __attribute__ ((aligned (4))) +#endif /* __ALIGN_END */ +#ifndef __ALIGN_BEGIN +#define __ALIGN_BEGIN +#endif /* __ALIGN_BEGIN */ + + +typedef enum { + HAL_OK = 0x00U, + HAL_ERROR = 0x01U, + HAL_BUSY = 0x02U, + HAL_TIMEOUT = 0x03U +} HAL_StatusTypeDef; +typedef enum +{ + DISABLE = 0U, + ENABLE = !DISABLE +} FunctionalState; + +#define GPIOA ((GPIO_TypeDef *) 0) +#define GPIOB ((GPIO_TypeDef *) 0) +#define GPIOC ((GPIO_TypeDef *) 0) +#define GPIOD ((GPIO_TypeDef *) 0) +#define GPIOE ((GPIO_TypeDef *) 0) +#define GPIOF ((GPIO_TypeDef *) 0) +#define GPIOG ((GPIO_TypeDef *) 0) +#define GPIOH ((GPIO_TypeDef *) 0) +#define GPIOI ((GPIO_TypeDef *) 0) + +#define GPIO_PIN_0 (0 ) +#define GPIO_PIN_1 (1 ) +#define GPIO_PIN_2 (2 ) +#define GPIO_PIN_3 (3 ) +#define GPIO_PIN_4 (4 ) +#define GPIO_PIN_5 (5 ) +#define GPIO_PIN_6 (6 ) +#define GPIO_PIN_7 (7 ) +#define GPIO_PIN_8 (8 ) +#define GPIO_PIN_9 (9 ) +#define GPIO_PIN_10 (10) +#define GPIO_PIN_11 (11) +#define GPIO_PIN_12 (12) +#define GPIO_PIN_13 (13) +#define GPIO_PIN_14 (14) +#define GPIO_PIN_15 (15) +#define GPIO_PIN_16 (16) +#define GPIO_PIN_17 (17) +#define GPIO_PIN_18 (18) +#define GPIO_PIN_19 (19) +#define GPIO_PIN_20 (20) +#define GPIO_PIN_21 (21) +#define GPIO_PIN_22 (22) +#define GPIO_PIN_23 (23) +#define GPIO_PIN_25 (25) +#define GPIO_PIN_26 (26) +#define GPIO_PIN_27 (27) +#define GPIO_PIN_28 (28) +#define GPIO_PIN_29 (29) +#define GPIO_PIN_30 (30) +#define GPIO_PIN_31 (31) +#define GPIO_PIN_32 (32) +#define GPIO_PIN_33 (33) +#define GPIO_PIN_34 (34) +#define GPIO_PIN_35 (35) +#define GPIO_PIN_36 (36) +#define GPIO_PIN_37 (37) +#define GPIO_PIN_38 (38) +#define GPIO_PIN_39 (39) +#define GPIO_PIN_40 (40) +#define GPIO_PIN_41 (41) +#define GPIO_PIN_42 (42) +#define GPIO_PIN_43 (43) +#define GPIO_PIN_44 (44) +#define GPIO_PIN_45 (45) +#define GPIO_PIN_46 (46) +#define GPIO_PIN_47 (47) +#define GPIO_PIN_48 (48) + +/** + * @brief GPIO Init structure definition + */ +typedef struct +{ + uint32_t Pin; /*!< Specifies the GPIO pins to be configured. + This parameter can be any value of @ref GPIO_pins_define */ + + uint32_t Mode; /*!< Specifies the operating mode for the selected pins. + This parameter can be a value of @ref GPIO_mode_define */ + + uint32_t Pull; /*!< Specifies the Pull-up or Pull-Down activation for the selected pins. + This parameter can be a value of @ref GPIO_pull_define */ + + uint32_t Speed; /*!< Specifies the speed for the selected pins. + This parameter can be a value of @ref GPIO_speed_define */ + + uint32_t Alternate; /*!< Peripheral to be connected to the selected pins. + This parameter can be a value of @ref GPIO_Alternate_function_selection */ +}GPIO_InitTypeDef; + +#define GPIO_MODE_INPUT GPIO_MODE_INPUT /*!< Input Floating Mode */ +#define GPIO_MODE_OUTPUT_PP GPIO_MODE_OUTPUT /*!< Output Push Pull Mode */ +#define GPIO_MODE_OUTPUT_OD GPIO_MODE_OUTPUT_OD /*!< Output Open Drain Mode */ + +#define GPIO_NOPULL GPIO_PULLUP_DISABLE /*!< No Pull-up or Pull-down activation */ +#define GPIO_PULLUP GPIO_PULLUP_ENABLE /*!< Pull-up activation */ +#define GPIO_PULLDOWN GPIO_PULLDOWN_ENABLE /*!< Pull-down activation */ + +#define GPIO_SPEED_FREQ_LOW 0x00000000U /*!< IO works at 2 MHz, please refer to the product datasheet */ +#define GPIO_SPEED_FREQ_MEDIUM 0x00000001U /*!< range 12,5 MHz to 50 MHz, please refer to the product datasheet */ +#define GPIO_SPEED_FREQ_HIGH 0x00000002U /*!< range 25 MHz to 100 MHz, please refer to the product datasheet */ +#define GPIO_SPEED_FREQ_VERY_HIGH 0x00000003U /*!< range 50 MHz to 200 MHz, please refer to the product datasheet */ + +typedef struct { + twai_timing_config_t t_config; + TaskHandle_t task; +} CAN_HandleTypeDef; + +/** + * @brief CAN filter configuration structure definition + */ +typedef struct { + uint32_t FilterIdHigh; /*!< Specifies the filter identification number (MSBs for a 32-bit + configuration, first one for a 16-bit configuration). + This parameter must be a number between Min_Data = 0x0000 and Max_Data = 0xFFFF. */ + + uint32_t FilterIdLow; /*!< Specifies the filter identification number (LSBs for a 32-bit + configuration, second one for a 16-bit configuration). + This parameter must be a number between Min_Data = 0x0000 and Max_Data = 0xFFFF. */ + + uint32_t FilterMaskIdHigh; /*!< Specifies the filter mask number or identification number, + according to the mode (MSBs for a 32-bit configuration, + first one for a 16-bit configuration). + This parameter must be a number between Min_Data = 0x0000 and Max_Data = 0xFFFF. */ + + uint32_t FilterMaskIdLow; /*!< Specifies the filter mask number or identification number, + according to the mode (LSBs for a 32-bit configuration, + second one for a 16-bit configuration). + This parameter must be a number between Min_Data = 0x0000 and Max_Data = 0xFFFF. */ + + uint32_t FilterFIFOAssignment; /*!< Specifies the FIFO (0 or 1U) which will be assigned to the filter. + This parameter can be a value of @ref CAN_filter_FIFO */ + + uint32_t FilterBank; /*!< Specifies the filter bank which will be initialized. + For single CAN instance(14 dedicated filter banks), + this parameter must be a number between Min_Data = 0 and Max_Data = 13. + For dual CAN instances(28 filter banks shared), + this parameter must be a number between Min_Data = 0 and Max_Data = 27. */ + + uint32_t FilterMode; /*!< Specifies the filter mode to be initialized. + This parameter can be a value of @ref CAN_filter_mode */ + + uint32_t FilterScale; /*!< Specifies the filter scale. + This parameter can be a value of @ref CAN_filter_scale */ + + uint32_t FilterActivation; /*!< Enable or disable the filter. + This parameter can be a value of @ref CAN_filter_activation */ + + uint32_t SlaveStartFilterBank; /*!< Select the start filter bank for the slave CAN instance. + For single CAN instances, this parameter is meaningless. + For dual CAN instances, all filter banks with lower index are assigned to master + CAN instance, whereas all filter banks with greater index are assigned to slave + CAN instance. + This parameter must be a number between Min_Data = 0 and Max_Data = 27. */ + +} CAN_FilterTypeDef; + +/** + * @brief CAN Tx message header structure definition + */ +typedef struct { + uint32_t StdId; /*!< Specifies the standard identifier. + This parameter must be a number between Min_Data = 0 and Max_Data = 0x7FF. */ + + uint32_t ExtId; /*!< Specifies the extended identifier. + This parameter must be a number between Min_Data = 0 and Max_Data = 0x1FFFFFFF. */ + + uint32_t IDE; /*!< Specifies the type of identifier for the message that will be transmitted. + This parameter can be a value of @ref CAN_identifier_type */ + + uint32_t RTR; /*!< Specifies the type of frame for the message that will be transmitted. + This parameter can be a value of @ref CAN_remote_transmission_request */ + + uint32_t DLC; /*!< Specifies the length of the frame that will be transmitted. + This parameter must be a number between Min_Data = 0 and Max_Data = 8. */ + + bool TransmitGlobalTime; /*!< Specifies whether the timestamp counter value captured on start + of frame transmission, is sent in DATA6 and DATA7 replacing pData[6] and pData[7]. + @note: Time Triggered Communication Mode must be enabled. + @note: DLC must be programmed as 8 bytes, in order these 2 bytes are sent. + This parameter can be set to ENABLE or DISABLE. */ + +} CAN_TxHeaderTypeDef; +#define CAN_RX_FIFO0 (0x00000000U) /*!< CAN receive FIFO 0 */ +#define CAN_RX_FIFO1 (0x00000001U) /*!< CAN receive FIFO 1 */ +#define CAN_FILTERMODE_IDMASK (0x00000000U) /*!< Identifier mask mode */ +#define CAN_FILTERMODE_IDLIST (0x00000001U) /*!< Identifier list mode */ +#define CAN_FILTERSCALE_16BIT (0x00000000U) /*!< Two 16-bit filters */ +#define CAN_FILTERSCALE_32BIT (0x00000001U) /*!< One 32-bit filter */ +#define CAN_ID_STD (0x00000000U) /*!< Standard Id */ +#define CAN_ID_EXT (0x00000004U) /*!< Extended Id */ +#define CAN_RTR_DATA (0x00000000U) /*!< Data frame */ +#define CAN_RTR_REMOTE (0x00000002U) /*!< Remote frame */ +/** + * @brief CAN Rx message header structure definition + */ +typedef struct { + uint32_t StdId; /*!< Specifies the standard identifier. + This parameter must be a number between Min_Data = 0 and Max_Data = 0x7FF. */ + + uint32_t ExtId; /*!< Specifies the extended identifier. + This parameter must be a number between Min_Data = 0 and Max_Data = 0x1FFFFFFF. */ + + uint32_t IDE; /*!< Specifies the type of identifier for the message that will be transmitted. + This parameter can be a value of @ref CAN_identifier_type */ + + uint32_t RTR; /*!< Specifies the type of frame for the message that will be transmitted. + This parameter can be a value of @ref CAN_remote_transmission_request */ + + uint32_t DLC; /*!< Specifies the length of the frame that will be transmitted. + This parameter must be a number between Min_Data = 0 and Max_Data = 8. */ + + uint32_t Timestamp; /*!< Specifies the timestamp counter value captured on start of frame reception. + @note: Time Triggered Communication Mode must be enabled. + This parameter must be a number between Min_Data = 0 and Max_Data = 0xFFFF. */ + + uint32_t FilterMatchIndex; /*!< Specifies the index of matching acceptance filter element. + This parameter must be a number between Min_Data = 0 and Max_Data = 0xFF. */ + +} CAN_RxHeaderTypeDef; + +typedef struct +{ + uint32_t ClockSpeed; /*!< Specifies the clock frequency. + This parameter must be set to a value lower than 400kHz */ + + uint32_t DutyCycle; /*!< Specifies the I2C fast mode duty cycle. + This parameter can be a value of @ref I2C_duty_cycle_in_fast_mode */ + + uint32_t OwnAddress1; /*!< Specifies the first device own address. + This parameter can be a 7-bit or 10-bit address. */ + + uint32_t AddressingMode; /*!< Specifies if 7-bit or 10-bit addressing mode is selected. + This parameter can be a value of @ref I2C_addressing_mode */ + + uint32_t DualAddressMode; /*!< Specifies if dual addressing mode is selected. + This parameter can be a value of @ref I2C_dual_addressing_mode */ + + uint32_t OwnAddress2; /*!< Specifies the second device own address if dual addressing mode is selected + This parameter can be a 7-bit address. */ + + uint32_t GeneralCallMode; /*!< Specifies if general call mode is selected. + This parameter can be a value of @ref I2C_general_call_addressing_mode */ + + uint32_t NoStretchMode; /*!< Specifies if nostretch mode is selected. + This parameter can be a value of @ref I2C_nostretch_mode */ + +} I2C_InitTypeDef; + +/** @defgroup I2C_duty_cycle_in_fast_mode I2C duty cycle in fast mode + * @{ + */ +#define I2C_DUTYCYCLE_2 0x00000000U +#define I2C_DUTYCYCLE_16_9 I2C_CCR_DUTY +/** + * @} + */ + +/** @defgroup I2C_addressing_mode I2C addressing mode + * @{ + */ +#define I2C_ADDRESSINGMODE_7BIT 0x00004000U +#define I2C_ADDRESSINGMODE_10BIT (I2C_OAR1_ADDMODE | 0x00004000U) +/** + * @} + */ + +/** @defgroup I2C_dual_addressing_mode I2C dual addressing mode + * @{ + */ +#define I2C_DUALADDRESS_DISABLE 0x00000000U +#define I2C_DUALADDRESS_ENABLE I2C_OAR2_ENDUAL +/** + * @} + */ + +/** @defgroup I2C_general_call_addressing_mode I2C general call addressing mode + * @{ + */ +#define I2C_GENERALCALL_DISABLE 0x00000000U +#define I2C_GENERALCALL_ENABLE I2C_CR1_ENGC +/** + * @} + */ + +/** @defgroup I2C_nostretch_mode I2C nostretch mode + * @{ + */ +#define I2C_NOSTRETCH_DISABLE 0x00000000U +#define I2C_NOSTRETCH_ENABLE I2C_CR1_NOSTRETCH +/** + * @} + */ + +/** @defgroup I2C_Memory_Address_Size I2C Memory Address Size + * @{ + */ +#define I2C_MEMADD_SIZE_8BIT 0x00000001U +#define I2C_MEMADD_SIZE_16BIT 0x00000010U +/** + * @} + */ + +/** @defgroup I2C_XferDirection_definition I2C XferDirection definition + * @{ + */ +#define I2C_DIRECTION_RECEIVE 0x00000000U +#define I2C_DIRECTION_TRANSMIT 0x00000001U + +typedef enum +{ + HAL_I2C_STATE_RESET = 0x00U, /*!< Peripheral is not yet Initialized */ + HAL_I2C_STATE_READY = 0x20U, /*!< Peripheral Initialized and ready for use */ + HAL_I2C_STATE_BUSY = 0x24U, /*!< An internal process is ongoing */ + HAL_I2C_STATE_BUSY_TX = 0x21U, /*!< Data Transmission process is ongoing */ + HAL_I2C_STATE_BUSY_RX = 0x22U, /*!< Data Reception process is ongoing */ + HAL_I2C_STATE_LISTEN = 0x28U, /*!< Address Listen Mode is ongoing */ + HAL_I2C_STATE_BUSY_TX_LISTEN = 0x29U, /*!< Address Listen Mode and Data Transmission + process is ongoing */ + HAL_I2C_STATE_BUSY_RX_LISTEN = 0x2AU, /*!< Address Listen Mode and Data Reception + process is ongoing */ + HAL_I2C_STATE_ABORT = 0x60U, /*!< Abort user request ongoing */ + HAL_I2C_STATE_TIMEOUT = 0xA0U, /*!< Timeout state */ + HAL_I2C_STATE_ERROR = 0xE0U /*!< Error */ + +} HAL_I2C_StateTypeDef; + +typedef struct +{ + uint32_t Instance; /*!< I2C NUM for ESP32Sx */ + + I2C_InitTypeDef Init; /*!< I2C communication parameters */ + + uint8_t *pBuffPtr; /*!< Pointer to I2C transfer buffer */ + + uint16_t XferSize; /*!< I2C transfer size */ + + __IO uint16_t XferCount; /*!< I2C transfer counter */ + + __IO uint32_t XferOptions; /*!< I2C transfer options */ + + __IO uint32_t PreviousState; /*!< I2C communication Previous state and mode */ + + + __IO HAL_I2C_StateTypeDef State; /*!< I2C communication state */ + + + __IO uint32_t ErrorCode; /*!< I2C Error code */ + + __IO uint32_t Devaddress; /*!< I2C Target device address */ + + __IO uint32_t Memaddress; /*!< I2C Target memory address */ + + __IO uint32_t MemaddSize; /*!< I2C Target memory address size */ + + __IO uint32_t EventCount; /*!< I2C Event counter */ + +} I2C_HandleTypeDef; + +// In order to be lazy, I refer to the header file of STM32 +#include "./glue_stm32f4xx_hal_spi.h" + +typedef struct { + uint32_t a; +} ADC_HandleTypeDef; + + +typedef enum { + GPIO_PIN_RESET = 0, + GPIO_PIN_SET +} GPIO_PinState; +typedef struct { + uint32_t a; +} GPIO_TypeDef; + +typedef struct { + uint32_t speed_mode; /**< Speed mode of the LEDC channel group */ + uint32_t channel; /**< LEDC channel (0 - LEDC_CHANNEL_MAX-1) */ +} TIM_HandleTypeDef; + +typedef struct +{ + uint32_t BaudRate; /*!< This member configures the UART communication baud rate. + The baud rate is computed using the following formula: + - IntegerDivider = ((PCLKx) / (8 * (OVR8+1) * (huart->Init.BaudRate))) + - FractionalDivider = ((IntegerDivider - ((uint32_t) IntegerDivider)) * 8 * (OVR8+1)) + 0.5 + Where OVR8 is the "oversampling by 8 mode" configuration bit in the CR1 register. */ + + uint32_t WordLength; /*!< Specifies the number of data bits transmitted or received in a frame. + This parameter can be a value of @ref UART_Word_Length */ + + uint32_t StopBits; /*!< Specifies the number of stop bits transmitted. + This parameter can be a value of @ref UART_Stop_Bits */ + + uint32_t Parity; /*!< Specifies the parity mode. + This parameter can be a value of @ref UART_Parity + @note When parity is enabled, the computed parity is inserted + at the MSB position of the transmitted data (9th bit when + the word length is set to 9 data bits; 8th bit when the + word length is set to 8 data bits). */ + + uint32_t Mode; /*!< Specifies whether the Receive or Transmit mode is enabled or disabled. + This parameter can be a value of @ref UART_Mode */ + + uint32_t HwFlowCtl; /*!< Specifies whether the hardware flow control mode is enabled or disabled. + This parameter can be a value of @ref UART_Hardware_Flow_Control */ + + uint32_t OverSampling; /*!< Specifies whether the Over sampling 8 is enabled or disabled, to achieve higher speed (up to fPCLK/8). + This parameter can be a value of @ref UART_Over_Sampling */ +} UART_InitTypeDef; + +typedef struct { + UART_InitTypeDef Init; + uint32_t a; + uint32_t ErrorCode; +} UART_HandleTypeDef; + + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F4xx_HAL_H */ diff --git a/Firmware/Targets/ESP32SX/main/target_constants.h b/Firmware/Targets/ESP32SX/main/target_constants.h new file mode 100644 index 000000000..d56099d30 --- /dev/null +++ b/Firmware/Targets/ESP32SX/main/target_constants.h @@ -0,0 +1,126 @@ +/* + * target_constants.h + * + * Created on: 01.05.2022 + * Author: Zhouli + */ + +#ifndef INC_TARGET_CONSTANTS_H_ +#define INC_TARGET_CONSTANTS_H_ + +#include "sdkconfig.h" +/* + * Add settings and peripheral maps for this specific target here + */ + +// Hardware name string +#define HW_TYPE "ESP32SX" +#define HW_TYPE_INT 2 +#define FW_DEVID 0x413 // F407 + +#if !(defined(CONFIG_IDF_TARGET_ESP32S2)) && !(defined(CONFIG_IDF_TARGET_ESP32S3)) +#error "The target chip is not supported, you should select a proper target by 'idf.py set-target esp32s2/3'" +#endif + +#define HW_ESP32SX // Defining this macro will implement the STM32 HAL library using some of the functions of ESP-IDF + +#ifdef HW_ESP32SX +#if (!defined OPENFFBOARD_ESP32S2_V1_0) && (!defined OPENFFBOARD_ESP32S2_V1_1) +#define OPENFFBOARD_ESP32S2_V1_1 // Default use a V1.1 version of the board +#endif + +#include "glue.h" + +#endif + +// Enabled features + +// Main classes +#define FFBWHEEL +// #define FFBJOYSTICK +// #define MIDI +// #define TMCDEBUG +// #define CANBRIDGE + +/* + * FFBWheel uses 2 FFB axis descriptor instead of 1 axis. + * Might improve compatibility with direct input but will report a 2 axis ffb compatible device + */ +//#define FFBWHEEL_USE_1AXIS_DESC + +// Extra features +#define LOCALBUTTONS +// #define SPIBUTTONS +// #define SHIFTERBUTTONS +// #define PCF8574BUTTONS // Requires I2C +#define ANALOGAXES +#define TMC4671DRIVER +#define PWMDRIVER +#define LOCALENCODER +#define CANBUS +#define ODRIVE +#define VESC +// #define MTENCODERSPI // requires SPI3, not support on ESP32SX now +// #define CANBUTTONS // Requires CAN, not support on ESP32SX now, besause the can filter has not been implemented in glue.c +// #define CANANALOG // Requires CAN, not support on ESP32SX now, besause the can filter has not been implemented in glue.c +// #define ADS111XANALOG // Requires I2C, not support on ESP32SX now +// #define UARTCOMMANDS // Not support on ESP32SX now, besause the uart has not been implemented in glue.c + +//#define TMCTEMP // Enable tmc temperature shutdown. replaced by hardware selection +//---------------------- + + +#define TIM_PWM htim1 + +#define UART_PORT_EXT huart1 // main uart port + +#define UART_PORT_MOTOR huart3 // motor uart port + +#define UART_BUF_SIZE 1 // How many bytes to expect via DMA + +// #define I2C_PORT hi2c1 // open it will change two GPIOs: DIN2->I2C_SDA, DIN3->I2C_SCL + +// ADC Channels +#define ADC1_CHANNELS 3 // how many analog input values to be read by dma +// #define ADC2_CHANNELS 2 // VSENSE + +// extern ADC_HandleTypeDef hadc2; +// #define VSENSE_HADC hadc2 +// #define ADC_CHAN_VINT 1 // adc buffer index of internal voltage sense +// #define ADC_CHAN_VEXT 0 // adc buffer index of supply voltage sense +// extern volatile uint32_t ADC1_BUF[ADC1_CHANNELS]; // Buffer +// #define VSENSE_ADC_BUF ADC2_BUF +#define VOLTAGE_MULT_DEFAULT 24.6 // Voltage in mV = adc*VOLTAGE_MULT (24.6 for 976k/33k divider) + +extern ADC_HandleTypeDef hadc1; +#define AIN_HADC hadc1 // main adc for analog pins +#define ADC_PINS 3 // Amount of analog channel pins +#define ADC_CHAN_FPIN 0 // First analog channel pin. last channel = fpin+ADC_PINS-1 + +#ifdef OPENFFBOARD_ESP32S2_V1_1 +#define BUTTON_PINS 4 // The ESP32SX board has only 4 button input +#else +#define BUTTON_PINS 3 +#endif + +extern SPI_HandleTypeDef hspi1; +#define HSPIDRV hspi1 + +// CAN +#ifdef CANBUS +extern CAN_HandleTypeDef hcan1; +#define CANPORT hcan1 + +#define CANSPEEDPRESET_50 0 +#define CANSPEEDPRESET_100 1 +#define CANSPEEDPRESET_125 2 +#define CANSPEEDPRESET_250 3 +#define CANSPEEDPRESET_500 4 +#define CANSPEEDPRESET_1000 5 + +#endif + +// System + + +#endif /* INC_TARGET_CONSTANTS_H_ */ diff --git a/Firmware/Targets/ESP32SX/partitions.csv b/Firmware/Targets/ESP32SX/partitions.csv new file mode 100644 index 000000000..8bc7d7c8f --- /dev/null +++ b/Firmware/Targets/ESP32SX/partitions.csv @@ -0,0 +1,5 @@ +# Name, Type, SubType, Offset, Size, Flags +# Note: if you have increased the bootloader size, make sure to update the offsets to avoid overlap +nvs, data, nvs, , 0x6000, +phy_init, data, phy, , 0x1000, +factory, app, factory, , 1500K, diff --git a/Firmware/Targets/ESP32SX/sdkconfig.defaults b/Firmware/Targets/ESP32SX/sdkconfig.defaults new file mode 100644 index 000000000..165b529da --- /dev/null +++ b/Firmware/Targets/ESP32SX/sdkconfig.defaults @@ -0,0 +1,17 @@ + +CONFIG_FREERTOS_HZ=1000 +CONFIG_ESP32S2_DEFAULT_CPU_FREQ_240=y +CONFIG_ESP32S3_DEFAULT_CPU_FREQ_240=y +CONFIG_TINYUSB=y +CONFIG_TINYUSB_CDC_ENABLED=y +CONFIG_TINYUSB_HID_ENABLED=y +CONFIG_TINYUSB_MIDI_ENABLED=y +CONFIG_TINYUSB_CDC_RX_BUFSIZE=512 +CONFIG_TINYUSB_CDC_TX_BUFSIZE=512 +CONFIG_TINYUSB_MIDI_RX_BUFSIZE=512 +CONFIG_TINYUSB_MIDI_TX_BUFSIZE=512 +CONFIG_COMPILER_CXX_RTTI=y + +CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y +CONFIG_PARTITION_TABLE_CUSTOM=y +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"