diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index 49f647225f..1dc1a39146 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -98,7 +98,7 @@ typedef struct serialPort_s { serialIdleCallbackPtr idleCallback; - uint8_t identifier; // actually serialPortIdentifier_e; avoid circular header dependency + int8_t identifier; // actually serialPortIdentifier_e; avoid circular header dependency } serialPort_t; typedef struct serialPinConfig_s { diff --git a/src/main/drivers/serial_pinconfig.c b/src/main/drivers/serial_pinconfig.c index 9b80d0f92d..7c9eb870fc 100644 --- a/src/main/drivers/serial_pinconfig.c +++ b/src/main/drivers/serial_pinconfig.c @@ -24,7 +24,7 @@ #include "platform.h" -#ifdef USE_UART +#if defined(USE_UART) || defined(USE_LPUART) || defined(USE_SOFTSERIAL) #include "build/build_config.h" @@ -34,7 +34,6 @@ #include "pg/pg_ids.h" -#if defined(USE_UART) || defined(USE_LPUART) || defined(USE_SOFTSERIAL) typedef struct serialDefaultPin_s { serialPortIdentifier_e ident; ioTag_t rxIO, txIO, inverterIO; @@ -103,4 +102,3 @@ void pgResetFn_serialPinConfig(serialPinConfig_t *serialPinConfig) #endif -#endif diff --git a/src/main/drivers/serial_uart_pinconfig.c b/src/main/drivers/serial_uart_pinconfig.c index 502744a6fb..8d9b348354 100644 --- a/src/main/drivers/serial_uart_pinconfig.c +++ b/src/main/drivers/serial_uart_pinconfig.c @@ -56,7 +56,7 @@ void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig) #if UART_TRAIT_PINSWAP bool swap = false; #endif - for (unsigned pindex = 0 ; pindex < UARTHARDWARE_MAX_PINS ; pindex++) { + for (unsigned pindex = 0; pindex < UARTHARDWARE_MAX_PINS; pindex++) { if (cfgRx && cfgRx == hardware->rxPins[pindex].pin) { uartdev->rx = hardware->rxPins[pindex]; } diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 9816ce4624..ce8826714c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -156,8 +156,6 @@ const char* serialPortNames[SERIAL_PORT_COUNT] = { #endif }; -static uint8_t serialPortCount; - const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 400000, 460800, 500000, 921600, 1000000, 1500000, 2000000, 2470000}; // see baudRate_e @@ -179,7 +177,7 @@ serialPortConfig_t* serialFindPortConfigurationMutable(serialPortIdentifier_e id const serialPortConfig_t* serialFindPortConfiguration(serialPortIdentifier_e identifier) { - return findInPortConfigs_identifier(serialConfigMutable()->portConfigs, ARRAYLEN(serialConfig()->portConfigs), identifier); + return findInPortConfigs_identifier(serialConfig()->portConfigs, ARRAYLEN(serialConfig()->portConfigs), identifier); } PG_REGISTER_WITH_RESET_FN(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 1); @@ -379,9 +377,9 @@ bool isSerialPortShared(const serialPortConfig_t *portConfig, uint16_t functionM serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) { - for (unsigned i = 0; i < ARRAYLEN(serialConfig()->portConfigs); i++) { - const serialPortConfig_t *candidate = &serialConfig()->portConfigs[i]; - + for (const serialPortConfig_t *candidate = serialConfig()->portConfigs; + candidate < ARRAYEND(serialConfig()->portConfigs); + candidate++) { if (isSerialPortShared(candidate, functionMask, sharedWithFunction)) { const serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(candidate->identifier); if (!serialPortUsage) { @@ -437,7 +435,7 @@ bool isSerialConfigValid(serialConfig_t *serialConfigToCheck) return false; } - uint8_t bitCount = popcount(portConfig->functionMask); + uint8_t bitCount = popcount32(portConfig->functionMask); #ifdef USE_VTX_MSP if ((portConfig->functionMask & FUNCTION_VTX_MSP) && bitCount == 1) { // VTX MSP has to be shared with RX or MSP serial @@ -547,6 +545,9 @@ serialPort_t *openSerialPort( void closeSerialPort(serialPort_t *serialPort) { + if (!serialPort) { + return; + } serialPortUsage_t *serialPortUsage = findSerialPortUsageByPort(serialPort); if (!serialPortUsage) { // already closed @@ -566,7 +567,6 @@ void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisab UNUSED(softserialEnabled); #endif - serialPortCount = SERIAL_PORT_COUNT; memset(&serialPortUsageList, 0, sizeof(serialPortUsageList)); for (int index = 0; index < SERIAL_PORT_COUNT; index++) { @@ -575,7 +575,6 @@ void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisab if (serialPortToDisable != SERIAL_PORT_NONE && serialPortUsageList[index].identifier == serialPortToDisable) { serialPortUsageList[index].identifier = SERIAL_PORT_NONE; - serialPortCount--; continue; // this index is deleted } { @@ -584,7 +583,6 @@ void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisab if (resourceIndex >= 0 // resource exists && !(serialPinConfig()->ioTagTx[resourceIndex] || serialPinConfig()->ioTagRx[resourceIndex])) { serialPortUsageList[index].identifier = SERIAL_PORT_NONE; - serialPortCount--; continue; } #endif @@ -596,7 +594,6 @@ void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisab #endif ) { serialPortUsageList[index].identifier = SERIAL_PORT_NONE; - serialPortCount--; continue; } } @@ -608,15 +605,9 @@ void serialRemovePort(serialPortIdentifier_e identifier) serialPortUsage_t* usage; while ((usage = findSerialPortUsageByIdentifier(identifier)) != NULL) { usage->identifier = SERIAL_PORT_NONE; - serialPortCount--; } } -uint8_t serialGetAvailablePortCount(void) -{ - return serialPortCount; -} - bool serialIsPortAvailable(serialPortIdentifier_e identifier) { return findSerialPortUsageByIdentifier(identifier) != NULL; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 3c3bad3ffa..86cbb9afa3 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -99,7 +99,6 @@ typedef enum { SERIAL_PORT_UART9, SERIAL_PORT_USART10, SERIAL_PORT_UART10 = SERIAL_PORT_USART10, - SERIAL_PORT_USART_MAX = SERIAL_PORT_USART10, SERIAL_PORT_USB_VCP = 20, @@ -161,7 +160,6 @@ typedef void serialConsumer(uint8_t); // void serialInit(bool softserialEnabled, serialPortIdentifier_e serialPortToDisable); void serialRemovePort(serialPortIdentifier_e identifier); -uint8_t serialGetAvailablePortCount(void); bool serialIsPortAvailable(serialPortIdentifier_e identifier); bool isSerialConfigValid(serialConfig_t *serialConfig); const serialPortConfig_t *serialFindPortConfiguration(serialPortIdentifier_e identifier); diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index f0ba78feb6..2fb3fe1951 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -88,8 +88,7 @@ void mspSerialAllocatePorts(void) void mspSerialReleasePortIfAllocated(serialPort_t *serialPort) { - for (unsigned portIndex = 0; portIndex < ARRAYLEN(mspPorts); portIndex++) { - mspPort_t *candidateMspPort = &mspPorts[portIndex]; + for (mspPort_t *candidateMspPort = mspPorts; candidateMspPort < ARRAYEND(mspPorts); candidateMspPort++) { if (candidateMspPort->port == serialPort) { closeSerialPort(serialPort); memset(candidateMspPort, 0, sizeof(mspPort_t)); @@ -99,8 +98,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort) mspDescriptor_t getMspSerialPortDescriptor(const serialPortIdentifier_e portIdentifier) { - for (unsigned portIndex = 0; portIndex < ARRAYLEN(mspPorts); portIndex++) { - mspPort_t *candidateMspPort = &mspPorts[portIndex]; + for (mspPort_t *candidateMspPort = mspPorts; candidateMspPort < ARRAYEND(mspPorts); candidateMspPort++) { if (candidateMspPort->port && candidateMspPort->port->identifier == portIdentifier) { return candidateMspPort->descriptor; } @@ -111,8 +109,7 @@ mspDescriptor_t getMspSerialPortDescriptor(const serialPortIdentifier_e portIden #if defined(USE_TELEMETRY) void mspSerialReleaseSharedTelemetryPorts(void) { - for (unsigned portIndex = 0; portIndex < ARRAYLEN(mspPorts); portIndex++) { - mspPort_t *candidateMspPort = &mspPorts[portIndex]; + for (mspPort_t *candidateMspPort = mspPorts; candidateMspPort < ARRAYEND(mspPorts); candidateMspPort++) { if (candidateMspPort->sharedWithTelemetry) { closeSerialPort(candidateMspPort->port); memset(candidateMspPort, 0, sizeof(mspPort_t)); @@ -521,8 +518,7 @@ void mspProcessPacket(mspPort_t *mspPort, mspProcessCommandFnPtr mspProcessComma */ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn) { - for (unsigned portIndex = 0; portIndex < ARRAYLEN(mspPorts); portIndex++) { - mspPort_t * const mspPort = &mspPorts[portIndex]; + for (mspPort_t *mspPort = mspPorts; mspPort < ARRAYEND(mspPorts); mspPort++) { if (!mspPort->port) { continue; } @@ -577,8 +573,7 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm bool mspSerialWaiting(void) { - for (unsigned portIndex = 0; portIndex < ARRAYLEN(mspPorts); portIndex++) { - mspPort_t * const mspPort = &mspPorts[portIndex]; + for (mspPort_t *mspPort = mspPorts; mspPort < ARRAYEND(mspPorts); mspPort++) { if (!mspPort->port) { continue; } @@ -600,8 +595,7 @@ int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int d { int ret = 0; - for (unsigned portIndex = 0; ARRAYLEN(mspPorts); portIndex++) { - mspPort_t * const mspPort = &mspPorts[portIndex]; + for (mspPort_t *mspPort = mspPorts; mspPort < ARRAYEND(mspPorts); mspPort++) { // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now) if (!mspPort->port @@ -629,8 +623,7 @@ uint32_t mspSerialTxBytesFree(void) { uint32_t ret = UINT32_MAX; - for (unsigned portIndex = 0; portIndex < ARRAYLEN(mspPorts); portIndex++) { - mspPort_t * const mspPort = &mspPorts[portIndex]; + for (mspPort_t *mspPort = mspPorts; mspPort < ARRAYEND(mspPorts); mspPort++) { if (!mspPort->port) { continue; } diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 1a55375693..648b29f3f7 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -237,7 +237,7 @@ void spektrumBind(rxConfig_t *rxConfig) if (!portConfig) { return; } - +#if defined(USE_UART) || defined(USE_LPUART) || defined(USE_SOFTSERIAL) const int resourceIndex = serialResourceIndex(portConfig->identifier); const ioTag_t txPin = serialPinConfig()->ioTagTx[resourceIndex]; const ioTag_t rxPin = serialPinConfig()->ioTagRx[resourceIndex]; @@ -255,7 +255,7 @@ void spektrumBind(rxConfig_t *rxConfig) default: bindPin = rxConfig->halfDuplex ? txPin : rxPin; } - +#endif if (!bindPin) { return; } diff --git a/src/platform/APM32/usb/vcp/serial_usb_vcp.c b/src/platform/APM32/usb/vcp/serial_usb_vcp.c index d90eb4592b..26b44d12f3 100644 --- a/src/platform/APM32/usb/vcp/serial_usb_vcp.c +++ b/src/platform/APM32/usb/vcp/serial_usb_vcp.c @@ -52,7 +52,7 @@ USBD_INFO_T gUsbDevice; #define USB_TIMEOUT 50 -static vcpPort_t vcpPort; +static vcpPort_t vcpPort = {0}; static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) { @@ -203,8 +203,6 @@ static const struct serialPortVTable usbVTable[] = { serialPort_t *usbVcpOpen(void) { - vcpPort_t *s; - IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); @@ -216,10 +214,9 @@ serialPort_t *usbVcpOpen(void) /* USB device and class init */ USBD_Init(&gUsbDevice, USBD_SPEED_FS, &USBD_DESC_VCP, &USBD_CDC_CLASS, USB_DevUserHandler); - s = &vcpPort; + vcpPort_t *s = &vcpPort; s->port.vTable = usbVTable; - - return (serialPort_t *)s; + return &s->port; } uint32_t usbVcpGetBaudRate(serialPort_t *instance) diff --git a/src/platform/AT32/serial_usb_vcp_at32f4.c b/src/platform/AT32/serial_usb_vcp_at32f4.c index 552036a80a..5fa490b933 100644 --- a/src/platform/AT32/serial_usb_vcp_at32f4.c +++ b/src/platform/AT32/serial_usb_vcp_at32f4.c @@ -50,7 +50,7 @@ #define USB_TIMEOUT 50 -static vcpPort_t vcpPort; +static vcpPort_t vcpPort = {0}; otg_core_type otg_core_struct; @@ -483,8 +483,6 @@ static const struct serialPortVTable usbVTable[] = { serialPort_t *usbVcpOpen(void) { - vcpPort_t *s; - IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); usb_gpio_config(); @@ -504,12 +502,12 @@ serialPort_t *usbVcpOpen(void) USB_ID, &cdc_class_handler, &cdc_desc_handler); - s = &vcpPort; - s->port.vTable = usbVTable; TxTimerConfig(); - return (serialPort_t *)s; + vcpPort_t *s = &vcpPort; + s->port.vTable = usbVTable; + return &s->port; } uint32_t usbVcpGetBaudRate(serialPort_t *instance) diff --git a/src/platform/STM32/serial_usb_vcp.c b/src/platform/STM32/serial_usb_vcp.c index 046c28bd12..4e1e83d32e 100644 --- a/src/platform/STM32/serial_usb_vcp.c +++ b/src/platform/STM32/serial_usb_vcp.c @@ -63,7 +63,7 @@ USBD_HandleTypeDef USBD_Device; #define USB_TIMEOUT 50 -static vcpPort_t vcpPort; +static vcpPort_t vcpPort = {0}; static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) { @@ -218,8 +218,6 @@ static const struct serialPortVTable usbVTable[] = { serialPort_t *usbVcpOpen(void) { - vcpPort_t *s; - IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0); @@ -275,10 +273,9 @@ serialPort_t *usbVcpOpen(void) USB_Interrupts_Config(); #endif - s = &vcpPort; + vcpPort_t *s = &vcpPort; s->port.vTable = usbVTable; - - return (serialPort_t *)s; + return &s->port; } uint32_t usbVcpGetBaudRate(serialPort_t *instance)