[4.4.2] Remove 15 m/s limit on estimated vario (#12788)
[betaflight.git] / src / main / vcp_hal / usbd_cdc_interface.c
blobb44576f8a1cdc105771aedd9e8bd2e6d1b363a97
1 /**
2 ******************************************************************************
3 * @file USB_Device/CDC_Standalone/Src/usbd_cdc_interface.c
4 * @author MCD Application Team
5 * @version V1.0.0
6 * @date 22-April-2016
7 * @brief Source file for USBD CDC interface
8 ******************************************************************************
9 * @attention
11 * <h2><center>&copy; Copyright (c) 2016 STMicroelectronics International N.V.
12 * All rights reserved.</center></h2>
14 * Redistribution and use in source and binary forms, with or without
15 * modification, are permitted, provided that the following conditions are met:
17 * 1. Redistribution of source code must retain the above copyright notice,
18 * this list of conditions and the following disclaimer.
19 * 2. Redistributions in binary form must reproduce the above copyright notice,
20 * this list of conditions and the following disclaimer in the documentation
21 * and/or other materials provided with the distribution.
22 * 3. Neither the name of STMicroelectronics nor the names of other
23 * contributors to this software may be used to endorse or promote products
24 * derived from this software without specific written permission.
25 * 4. This software, including modifications and/or derivative works of this
26 * software, must execute solely and exclusively on microcontroller or
27 * microprocessor devices manufactured by or for STMicroelectronics.
28 * 5. Redistribution and use of this software other than as permitted under
29 * this license is void and will automatically terminate your rights under
30 * this license.
32 * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
33 * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
34 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
35 * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
36 * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
37 * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
38 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
39 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
40 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
41 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
42 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
43 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
45 ******************************************************************************
48 /* Includes ------------------------------------------------------------------*/
50 #include <stdbool.h>
52 #include "platform.h"
54 #include "build/atomic.h"
56 #include "usbd_conf.h"
57 #include "usbd_core.h"
58 #include "usbd_desc.h"
59 #include "usbd_cdc.h"
60 #include "usbd_cdc_interface.h"
62 #include "drivers/nvic.h"
63 #include "drivers/serial_usb_vcp.h"
64 #include "drivers/time.h"
66 /* Private typedef -----------------------------------------------------------*/
67 /* Private define ------------------------------------------------------------*/
68 #define APP_RX_DATA_SIZE 2048
69 #define APP_TX_DATA_SIZE 2048
71 #define APP_TX_BLOCK_SIZE 512
73 /* Private macro -------------------------------------------------------------*/
74 /* Private variables ---------------------------------------------------------*/
75 USBD_CDC_LineCodingTypeDef LineCoding =
77 115200, /* baud rate*/
78 0x00, /* stop bits-1*/
79 0x00, /* parity - none*/
80 0x08 /* nb. of bits 8*/
83 volatile uint8_t UserRxBuffer[APP_RX_DATA_SIZE];/* Received Data over USB are stored in this buffer */
84 volatile uint8_t UserTxBuffer[APP_TX_DATA_SIZE];/* Received Data over UART (CDC interface) are stored in this buffer */
85 uint32_t BuffLength;
86 volatile uint32_t UserTxBufPtrIn = 0;/* Increment this pointer or roll it back to
87 start address when data are received over USART */
88 volatile uint32_t UserTxBufPtrOut = 0; /* Increment this pointer or roll it back to
89 start address when data are sent over USB */
91 uint32_t rxAvailable = 0;
92 uint8_t* rxBuffPtr = NULL;
94 /* TIM handler declaration */
95 TIM_HandleTypeDef TimHandle;
97 static void (*ctrlLineStateCb)(void *context, uint16_t ctrlLineState);
98 static void *ctrlLineStateCbContext;
99 static void (*baudRateCb)(void *context, uint32_t baud);
100 static void *baudRateCbContext;
102 /* Private function prototypes -----------------------------------------------*/
103 static int8_t CDC_Itf_Init(void);
104 static int8_t CDC_Itf_DeInit(void);
105 static int8_t CDC_Itf_Control(uint8_t cmd, uint8_t* pbuf, uint16_t length);
106 static int8_t CDC_Itf_Receive(uint8_t* pbuf, uint32_t *Len);
108 // The CDC_Itf_TransmitCplt field was introduced in MiddleWare that comes with
109 // H7 V1.8.0.
110 // Other MCU can be add here as the MiddleWare version advances.
111 #ifdef STM32H7
112 static int8_t CDC_Itf_TransmitCplt(uint8_t *Buf, uint32_t *Len, uint8_t epnum);
113 #endif
115 static void TIM_Config(void);
116 static void Error_Handler(void);
118 USBD_CDC_ItfTypeDef USBD_CDC_fops =
120 CDC_Itf_Init,
121 CDC_Itf_DeInit,
122 CDC_Itf_Control,
123 CDC_Itf_Receive,
124 #ifdef STM32H7
125 CDC_Itf_TransmitCplt
126 #endif
130 void TIMx_IRQHandler(void)
132 HAL_TIM_IRQHandler(&TimHandle);
135 /* Private functions ---------------------------------------------------------*/
138 * @brief CDC_Itf_Init
139 * Initializes the CDC media low layer
140 * @param None
141 * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
143 static int8_t CDC_Itf_Init(void)
145 /*##-3- Configure the TIM Base generation #################################*/
146 TIM_Config();
148 /*##-4- Start the TIM Base generation in interrupt mode ####################*/
149 /* Start Channel1 */
150 if (HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK)
152 /* Starting Error */
153 Error_Handler();
156 /*##-5- Set Application Buffers ############################################*/
157 USBD_CDC_SetTxBuffer(&USBD_Device, (uint8_t *)UserTxBuffer, 0);
158 USBD_CDC_SetRxBuffer(&USBD_Device, (uint8_t *)UserRxBuffer);
160 ctrlLineStateCb = NULL;
161 baudRateCb = NULL;
163 return (USBD_OK);
167 * @brief CDC_Itf_DeInit
168 * DeInitializes the CDC media low layer
169 * @param None
170 * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
172 static int8_t CDC_Itf_DeInit(void)
175 return (USBD_OK);
179 * @brief CDC_Itf_Control
180 * Manage the CDC class requests
181 * @param Cmd: Command code
182 * @param Buf: Buffer containing command data (request parameters)
183 * @param Len: Number of data to be sent (in bytes)
184 * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
186 static int8_t CDC_Itf_Control (uint8_t cmd, uint8_t* pbuf, uint16_t length)
188 LINE_CODING* plc = (LINE_CODING*)pbuf;
190 switch (cmd)
192 case CDC_SEND_ENCAPSULATED_COMMAND:
193 /* Add your code here */
194 break;
196 case CDC_GET_ENCAPSULATED_RESPONSE:
197 /* Add your code here */
198 break;
200 case CDC_SET_COMM_FEATURE:
201 /* Add your code here */
202 break;
204 case CDC_GET_COMM_FEATURE:
205 /* Add your code here */
206 break;
208 case CDC_CLEAR_COMM_FEATURE:
209 /* Add your code here */
210 break;
212 case CDC_SET_LINE_CODING:
213 if (pbuf && (length == sizeof(*plc))) {
214 LineCoding.bitrate = plc->bitrate;
215 LineCoding.format = plc->format;
216 LineCoding.paritytype = plc->paritytype;
217 LineCoding.datatype = plc->datatype;
219 // If a callback is provided, tell the upper driver of changes in baud rate
220 if (baudRateCb) {
221 baudRateCb(baudRateCbContext, LineCoding.bitrate);
225 break;
227 case CDC_GET_LINE_CODING:
228 if (pbuf && (length == sizeof(*plc))) {
229 plc->bitrate = LineCoding.bitrate;
230 plc->format = LineCoding.format;
231 plc->paritytype = LineCoding.paritytype;
232 plc->datatype = LineCoding.datatype;
234 break;
236 case CDC_SET_CONTROL_LINE_STATE:
237 // If a callback is provided, tell the upper driver of changes in DTR/RTS state
238 if (pbuf && (length == sizeof(uint16_t))) {
239 if (ctrlLineStateCb) {
240 ctrlLineStateCb(ctrlLineStateCbContext, *((uint16_t *)pbuf));
243 break;
245 case CDC_SEND_BREAK:
246 /* Add your code here */
247 break;
249 default:
250 break;
253 return (USBD_OK);
257 * @brief TIM period elapsed callback
258 * @param htim: TIM handle
259 * @retval None
261 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
263 if (htim->Instance != TIMusb) {
264 return;
267 uint32_t buffsize;
268 static uint32_t lastBuffsize = 0;
270 USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)USBD_Device.pCDC_ClassData;
272 if (hcdc->TxState == 0) {
273 // endpoint has finished transmitting previous block
274 if (lastBuffsize) {
275 bool needZeroLengthPacket = lastBuffsize % 64 == 0;
277 // move the ring buffer tail based on the previous succesful transmission
278 UserTxBufPtrOut += lastBuffsize;
279 if (UserTxBufPtrOut == APP_TX_DATA_SIZE) {
280 UserTxBufPtrOut = 0;
282 lastBuffsize = 0;
284 if (needZeroLengthPacket) {
285 USBD_CDC_SetTxBuffer(&USBD_Device, (uint8_t*)&UserTxBuffer[UserTxBufPtrOut], 0);
286 return;
289 if (UserTxBufPtrOut != UserTxBufPtrIn) {
290 if (UserTxBufPtrOut > UserTxBufPtrIn) { /* Roll-back */
291 buffsize = APP_TX_DATA_SIZE - UserTxBufPtrOut;
292 } else {
293 buffsize = UserTxBufPtrIn - UserTxBufPtrOut;
295 if (buffsize > APP_TX_BLOCK_SIZE) {
296 buffsize = APP_TX_BLOCK_SIZE;
299 USBD_CDC_SetTxBuffer(&USBD_Device, (uint8_t*)&UserTxBuffer[UserTxBufPtrOut], buffsize);
301 if (USBD_CDC_TransmitPacket(&USBD_Device) == USBD_OK) {
302 lastBuffsize = buffsize;
309 * @brief CDC_Itf_DataRx
310 * Data received over USB OUT endpoint are sent over CDC interface
311 * through this function.
312 * @param Buf: Buffer of data to be transmitted
313 * @param Len: Number of data received (in bytes)
314 * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
316 static int8_t CDC_Itf_Receive(uint8_t* Buf, uint32_t *Len)
318 rxAvailable = *Len;
319 rxBuffPtr = Buf;
320 if (!rxAvailable) {
321 // Received an empty packet, trigger receiving the next packet.
322 // This will happen after a packet that's exactly 64 bytes is received.
323 // The USB protocol requires that an empty (0 byte) packet immediately follow.
324 USBD_CDC_ReceivePacket(&USBD_Device);
326 return (USBD_OK);
329 #ifdef STM32H7
330 static int8_t CDC_Itf_TransmitCplt(uint8_t *Buf, uint32_t *Len, uint8_t epnum)
332 UNUSED(Buf);
333 UNUSED(Len);
334 UNUSED(epnum);
336 return (USBD_OK);
338 #endif
341 * @brief TIM_Config: Configure TIMusb timer
342 * @param None.
343 * @retval None
345 static void TIM_Config(void)
347 /* Set TIMusb instance */
348 TimHandle.Instance = TIMusb;
350 /* Initialize TIMx peripheral as follow:
351 + Period = 10000 - 1
352 + Prescaler = ((SystemCoreClock/2)/10000) - 1
353 + ClockDivision = 0
354 + Counter direction = Up
356 TimHandle.Init.Period = (CDC_POLLING_INTERVAL*1000) - 1;
357 TimHandle.Init.Prescaler = (SystemCoreClock / 2 / (1000000)) - 1;
358 TimHandle.Init.ClockDivision = 0;
359 TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
360 if (HAL_TIM_Base_Init(&TimHandle) != HAL_OK)
362 /* Initialization Error */
363 Error_Handler();
366 /*##-6- Enable TIM peripherals Clock #######################################*/
367 TIMx_CLK_ENABLE();
369 /*##-7- Configure the NVIC for TIMx ########################################*/
370 /* Set Interrupt Group Priority */
371 HAL_NVIC_SetPriority(TIMx_IRQn, 6, 0);
373 /* Enable the TIMx global Interrupt */
374 HAL_NVIC_EnableIRQ(TIMx_IRQn);
378 * @brief This function is executed in case of error occurrence.
379 * @param None
380 * @retval None
382 static void Error_Handler(void)
384 /* Add your own code here */
387 uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
389 uint32_t count = 0;
390 if ( (rxBuffPtr != NULL))
392 while ((rxAvailable > 0) && count < len)
394 recvBuf[count] = rxBuffPtr[0];
395 rxBuffPtr++;
396 rxAvailable--;
397 count++;
398 if (rxAvailable < 1)
399 USBD_CDC_ReceivePacket(&USBD_Device);
402 return count;
405 uint32_t CDC_Receive_BytesAvailable(void)
407 return rxAvailable;
410 uint32_t CDC_Send_FreeBytes(void)
413 return the bytes free in the circular buffer
415 functionally equivalent to:
416 (APP_Rx_ptr_out > APP_Rx_ptr_in ? APP_Rx_ptr_out - APP_Rx_ptr_in : APP_RX_DATA_SIZE - APP_Rx_ptr_in + APP_Rx_ptr_in)
417 but without the impact of the condition check.
419 uint32_t freeBytes;
421 ATOMIC_BLOCK(NVIC_BUILD_PRIORITY(6, 0)) {
422 freeBytes = ((UserTxBufPtrOut - UserTxBufPtrIn) + (-((int)(UserTxBufPtrOut <= UserTxBufPtrIn)) & APP_TX_DATA_SIZE)) - 1;
425 return freeBytes;
429 * @brief CDC_Send_DATA
430 * CDC received data to be send over USB IN endpoint are managed in
431 * this function.
432 * @param ptrBuffer: Buffer of data to be sent
433 * @param sendLength: Number of data to be sent (in bytes)
434 * @retval Bytes sent
436 uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
438 for (uint32_t i = 0; i < sendLength; i++) {
439 while (CDC_Send_FreeBytes() == 0) {
440 // block until there is free space in the ring buffer
441 delay(1);
443 ATOMIC_BLOCK(NVIC_BUILD_PRIORITY(6, 0)) { // Paranoia
444 UserTxBuffer[UserTxBufPtrIn] = ptrBuffer[i];
445 UserTxBufPtrIn = (UserTxBufPtrIn + 1) % APP_TX_DATA_SIZE;
448 return sendLength;
452 /*******************************************************************************
453 * Function Name : usbIsConfigured.
454 * Description : Determines if USB VCP is configured or not
455 * Input : None.
456 * Output : None.
457 * Return : True if configured.
458 *******************************************************************************/
459 uint8_t usbIsConfigured(void)
461 return (USBD_Device.dev_state == USBD_STATE_CONFIGURED);
464 /*******************************************************************************
465 * Function Name : usbIsConnected.
466 * Description : Determines if USB VCP is connected ot not
467 * Input : None.
468 * Output : None.
469 * Return : True if connected.
470 *******************************************************************************/
471 uint8_t usbIsConnected(void)
473 return (USBD_Device.dev_state != USBD_STATE_DEFAULT);
476 /*******************************************************************************
477 * Function Name : CDC_BaudRate.
478 * Description : Get the current baud rate
479 * Input : None.
480 * Output : None.
481 * Return : Baud rate in bps
482 *******************************************************************************/
483 uint32_t CDC_BaudRate(void)
485 return LineCoding.bitrate;
488 /*******************************************************************************
489 * Function Name : CDC_SetBaudRateCb
490 * Description : Set a callback to call when baud rate changes
491 * Input : callback function and context.
492 * Output : None.
493 * Return : None.
494 *******************************************************************************/
495 void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context)
497 baudRateCbContext = context;
498 baudRateCb = cb;
501 /*******************************************************************************
502 * Function Name : CDC_SetCtrlLineStateCb
503 * Description : Set a callback to call when control line state changes
504 * Input : callback function and context.
505 * Output : None.
506 * Return : None.
507 *******************************************************************************/
508 void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context)
510 ctrlLineStateCbContext = context;
511 ctrlLineStateCb = cb;
514 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/