Create release.yml
[betaflight.git] / src / main / vcp / hw_config.c
bloba1cad3a9a8a6f8ce4aa1e6334735f7fa13b104e7
1 /**
2 ******************************************************************************
3 * @file hw_config.c
4 * @author MCD Application Team
5 * @version V4.0.0
6 * @date 21-January-2013
7 * @brief Hardware Configuration & Setup
8 ******************************************************************************
9 * @attention
11 * <h2><center>&copy; COPYRIGHT 2013 STMicroelectronics</center></h2>
13 * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
14 * You may not use this file except in compliance with the License.
15 * You may obtain a copy of the License at:
17 * http://www.st.com/software_license_agreement_liberty_v2
19 * Unless required by applicable law or agreed to in writing, software
20 * distributed under the License is distributed on an "AS IS" BASIS,
21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22 * See the License for the specific language governing permissions and
23 * limitations under the License.
25 ******************************************************************************
28 /* Includes ------------------------------------------------------------------*/
30 #include "stm32_it.h"
31 #include "platform.h"
32 #include "usb_lib.h"
33 #include "usb_prop.h"
34 #include "usb_desc.h"
35 #include "hw_config.h"
36 #include "usb_pwr.h"
38 #include <stdbool.h>
39 #include "drivers/system.h"
40 #include "drivers/usb_io.h"
41 #include "drivers/nvic.h"
43 #include "common/utils.h"
46 /* Private typedef -----------------------------------------------------------*/
47 /* Private define ------------------------------------------------------------*/
48 /* Private macro -------------------------------------------------------------*/
49 /* Private variables ---------------------------------------------------------*/
50 ErrorStatus HSEStartUpStatus;
51 EXTI_InitTypeDef EXTI_InitStructure;
52 __IO uint32_t packetSent; // HJI
53 extern __IO uint32_t receiveLength; // HJI
55 uint8_t receiveBuffer[64]; // HJI
56 uint32_t sendLength; // HJI
57 static void IntToUnicode(uint32_t value, uint8_t *pbuf, uint8_t len);
58 static void (*ctrlLineStateCb)(void *context, uint16_t ctrlLineState);
59 static void *ctrlLineStateCbContext;
60 static void (*baudRateCb)(void *context, uint32_t baud);
61 static void *baudRateCbContext;
63 /* Extern variables ----------------------------------------------------------*/
65 /* Private function prototypes -----------------------------------------------*/
66 /* Private functions ---------------------------------------------------------*/
67 /*******************************************************************************
68 * Function Name : Set_System
69 * Description : Configures Main system clocks & power
70 * Input : None.
71 * Return : None.
72 *******************************************************************************/
73 void Set_System(void)
75 #if !defined(STM32L1XX_MD) && !defined(STM32L1XX_HD) && !defined(STM32L1XX_MD_PLUS)
76 GPIO_InitTypeDef GPIO_InitStructure;
77 #endif /* STM32L1XX_MD && STM32L1XX_XD */
79 #if defined(USB_USE_EXTERNAL_PULLUP)
80 GPIO_InitTypeDef GPIO_InitStructure;
81 #endif /* USB_USE_EXTERNAL_PULLUP */
83 /*!< At this stage the microcontroller clock setting is already configured,
84 this is done through SystemInit() function which is called from startup
85 file (startup_stm32f10x_xx.s) before to branch to application main.
86 To reconfigure the default setting of SystemInit() function, refer to
87 system_stm32f10x.c file
89 #if defined(STM32L1XX_MD) || defined(STM32L1XX_HD)|| defined(STM32L1XX_MD_PLUS) || defined(STM32F37X) || defined(STM32F303xC)
90 /* Enable the SYSCFG module clock */
91 RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
92 #endif /* STM32L1XX_XD */
94 usbGenerateDisconnectPulse();
96 #if defined(STM32F37X) || defined(STM32F303xC)
98 /*Set PA11,12 as IN - USB_DM,DP*/
99 RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
100 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12;
101 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
102 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
103 GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
104 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
105 GPIO_Init(GPIOA, &GPIO_InitStructure);
107 /*SET PA11,12 for USB: USB_DM,DP*/
108 GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_14);
109 GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_14);
111 #endif /* STM32F37X && STM32F303xC)*/
112 #if defined(STM32F10X)
113 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
114 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12;
115 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
116 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
117 GPIO_Init(GPIOA, &GPIO_InitStructure);
118 #endif
120 // Initialise callbacks
121 ctrlLineStateCb = NULL;
122 baudRateCb = NULL;
124 /* Configure the EXTI line 18 connected internally to the USB IP */
125 EXTI_ClearITPendingBit(EXTI_Line18);
126 EXTI_InitStructure.EXTI_Line = EXTI_Line18;
127 EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
128 EXTI_InitStructure.EXTI_LineCmd = ENABLE;
129 EXTI_Init(&EXTI_InitStructure);
132 /*******************************************************************************
133 * Function Name : Set_USBClock
134 * Description : Configures USB Clock input (48MHz)
135 * Input : None.
136 * Return : None.
137 *******************************************************************************/
138 void Set_USBClock(void)
140 /* Select USBCLK source */
141 RCC_USBCLKConfig(RCC_USBCLKSource_PLLCLK_1Div5);
143 /* Enable the USB clock */
144 RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE);
147 /*******************************************************************************
148 * Function Name : Enter_LowPowerMode
149 * Description : Power-off system clocks and power while entering suspend mode
150 * Input : None.
151 * Return : None.
152 *******************************************************************************/
153 void Enter_LowPowerMode(void)
155 /* Set the device state to suspend */
156 bDeviceState = SUSPENDED;
159 /*******************************************************************************
160 * Function Name : Leave_LowPowerMode
161 * Description : Restores system clocks and power while exiting suspend mode
162 * Input : None.
163 * Return : None.
164 *******************************************************************************/
165 void Leave_LowPowerMode(void)
167 DEVICE_INFO *pInfo = &Device_Info;
169 /* Set the device state to the correct state */
170 if (pInfo->Current_Configuration != 0) {
171 /* Device configured */
172 bDeviceState = CONFIGURED;
173 } else {
174 bDeviceState = ATTACHED;
176 /*Enable SystemCoreClock*/
177 SystemInit();
180 /*******************************************************************************
181 * Function Name : USB_Interrupts_Config
182 * Description : Configures the USB interrupts
183 * Input : None.
184 * Return : None.
185 *******************************************************************************/
186 void USB_Interrupts_Config(void)
188 NVIC_InitTypeDef NVIC_InitStructure;
190 /* 2 bit for pre-emption priority, 2 bits for subpriority */
191 NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING); // is this really neccesary?
193 /* Enable the USB interrupt */
194 NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
195 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB);
196 NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB);
197 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
198 NVIC_Init(&NVIC_InitStructure);
200 /* Enable the USB Wake-up interrupt */
201 NVIC_InitStructure.NVIC_IRQChannel = USBWakeUp_IRQn;
202 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_USB_WUP);
203 NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_USB_WUP);
204 NVIC_Init(&NVIC_InitStructure);
207 #ifdef STM32F10X
209 /*******************************************************************************
210 * Function Name : USB_Interrupts_Disable
211 * Description : Disables the USB interrupts
212 * Input : None.
213 * Return : None.
214 *******************************************************************************/
215 void USB_Interrupts_Disable(void)
217 NVIC_InitTypeDef NVIC_InitStructure;
219 /* Disable the USB interrupt */
220 NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
221 NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
222 NVIC_Init(&NVIC_InitStructure);
224 /* Disable the USB Wake-up interrupt */
225 NVIC_InitStructure.NVIC_IRQChannel = USBWakeUp_IRQn;
226 NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
227 NVIC_Init(&NVIC_InitStructure);
229 #endif
231 /*******************************************************************************
232 * Function Name : USB_Cable_Config
233 * Description : Software Connection/Disconnection of USB Cable
234 * Input : None.
235 * Return : Status
236 *******************************************************************************/
237 void USB_Cable_Config(FunctionalState NewState)
239 UNUSED(NewState);
242 /*******************************************************************************
243 * Function Name : Get_SerialNum.
244 * Description : Create the serial number string descriptor.
245 * Input : None.
246 * Output : None.
247 * Return : None.
248 *******************************************************************************/
249 void Get_SerialNum(void)
251 uint32_t Device_Serial0, Device_Serial1, Device_Serial2;
253 Device_Serial0 = *(uint32_t*)ID1;
254 Device_Serial1 = *(uint32_t*)ID2;
255 Device_Serial2 = *(uint32_t*)ID3;
257 Device_Serial0 += Device_Serial2;
259 if (Device_Serial0 != 0) {
260 IntToUnicode(Device_Serial0, &Virtual_Com_Port_StringSerial[2], 8);
261 IntToUnicode(Device_Serial1, &Virtual_Com_Port_StringSerial[18], 4);
265 /*******************************************************************************
266 * Function Name : HexToChar.
267 * Description : Convert Hex 32Bits value into char.
268 * Input : None.
269 * Output : None.
270 * Return : None.
271 *******************************************************************************/
272 static void IntToUnicode(uint32_t value, uint8_t *pbuf, uint8_t len)
274 uint8_t idx = 0;
276 for (idx = 0; idx < len; idx++) {
277 if (((value >> 28)) < 0xA) {
278 pbuf[2 * idx] = (value >> 28) + '0';
279 } else {
280 pbuf[2 * idx] = (value >> 28) + 'A' - 10;
283 value = value << 4;
285 pbuf[2 * idx + 1] = 0;
289 /*******************************************************************************
290 * Function Name : Send DATA .
291 * Description : send the data received from the STM32 to the PC through USB
292 * Input : None.
293 * Output : None.
294 * Return : None.
295 *******************************************************************************/
296 uint32_t CDC_Send_DATA(const uint8_t *ptrBuffer, uint32_t sendLength)
298 /* Last transmission hasn't finished, abort */
299 if (packetSent) {
300 return 0;
303 // We can only put 64 bytes in the buffer
304 if (sendLength > 64 / 2) {
305 sendLength = 64 / 2;
308 // Try to load some bytes if we can
309 if (sendLength) {
310 UserToPMABufferCopy(ptrBuffer, ENDP1_TXADDR, sendLength);
311 SetEPTxCount(ENDP1, sendLength);
312 packetSent += sendLength;
313 SetEPTxValid(ENDP1);
316 return sendLength;
319 uint32_t CDC_Send_FreeBytes(void)
321 /* this driver is blocking, so the buffer is unlimited */
322 return 255;
325 /*******************************************************************************
326 * Function Name : Receive DATA .
327 * Description : receive the data from the PC to STM32 and send it through USB
328 * Input : None.
329 * Output : None.
330 * Return : None.
331 *******************************************************************************/
332 uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len)
334 static uint8_t offset = 0;
335 uint8_t i;
337 if (len > receiveLength) {
338 len = receiveLength;
341 for (i = 0; i < len; i++) {
342 recvBuf[i] = (uint8_t)(receiveBuffer[i + offset]);
345 receiveLength -= len;
346 offset += len;
348 /* re-enable the rx endpoint which we had set to receive 0 bytes */
349 if (receiveLength == 0) {
350 SetEPRxCount(ENDP3, 64);
351 SetEPRxStatus(ENDP3, EP_RX_VALID);
352 offset = 0;
355 return len;
358 uint32_t CDC_Receive_BytesAvailable(void)
360 return receiveLength;
363 /*******************************************************************************
364 * Function Name : usbIsConfigured.
365 * Description : Determines if USB VCP is configured or not
366 * Input : None.
367 * Output : None.
368 * Return : True if configured.
369 *******************************************************************************/
370 uint8_t usbIsConfigured(void)
372 return (bDeviceState == CONFIGURED);
375 /*******************************************************************************
376 * Function Name : usbIsConnected.
377 * Description : Determines if USB VCP is connected ot not
378 * Input : None.
379 * Output : None.
380 * Return : True if connected.
381 *******************************************************************************/
382 uint8_t usbIsConnected(void)
384 return (bDeviceState != UNCONNECTED);
388 /*******************************************************************************
389 * Function Name : CDC_BaudRate.
390 * Description : Get the current baud rate
391 * Input : None.
392 * Output : None.
393 * Return : Baud rate in bps
394 *******************************************************************************/
395 uint32_t CDC_BaudRate(void)
397 return Virtual_Com_Port_GetBaudRate();
400 /*******************************************************************************
401 * Function Name : CDC_SetBaudRateCb
402 * Description : Set a callback to call when baud rate changes
403 * Input : callback function and context.
404 * Output : None.
405 * Return : None.
406 *******************************************************************************/
407 void CDC_SetBaudRateCb(void (*cb)(void *context, uint32_t baud), void *context)
409 baudRateCbContext = context;
410 baudRateCb = cb;
413 /*******************************************************************************
414 * Function Name : CDC_SetCtrlLineStateCb
415 * Description : Set a callback to call when control line state changes
416 * Input : callback function and context.
417 * Output : None.
418 * Return : None.
419 *******************************************************************************/
420 void CDC_SetCtrlLineStateCb(void (*cb)(void *context, uint16_t ctrlLineState), void *context)
422 ctrlLineStateCbContext = context;
423 ctrlLineStateCb = cb;
426 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/