mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
Refactor uart (#13585)
* serial - refactor serial resource handling - port related resources (tx/rx/inverted,dma) - are stored sequentially, with space in place of ports that are not enabled (RESOURCE_UART_COUNT + RESOURCE_LPUART_COUNT + RESOURCE_SOFTSERIAL_COUNT) - RESOURCE_UART_OFFSET, RESOURCE_LPUART_OFFSET, RESOURCE_SOFTSERIAL_OFFSET - resource entries are pointing into this array (UART, LPUART, SOFTSERIAL) - both pins and DMA - inverter is supproted only for UART + LPUART (makes sense only for UART, to be removed) - softSerialPinConfig is removed, it is no longer necessary - serialResourceIndex(identifier) is used universally to get correct index into resource array - unified handling of resources where possible - serialOwnerTxRx() + serialOwnerIndex() are used for displaying resources correctly to user. - serialType(identifier) implemented - serialOwnerTxRx / serialOwnerIndex are trivial with it - large switch() statemens are greatly simplified * serial - merge code duplicated in all UART implementations - drivers/serial_uart_hw.c contains merged serialUART code. Code did not match exactly. Obvious cases are fixed, more complicated use #ifs - pin inversion unified - uartOpen is refactored a bit * serial - refactor uartDevice - use 'compressed' enum from uartDeviceIdx_e. Only enabled ports are in this enum. - uartDeviceIdx directly indexes uartDevice (no search necessary, no wasted space) - use `serialPortIdentifier_e identifier;` for uartHardware - for DMA remap, define only entries for enabled ports (uartDeviceIdx_e does not exist disabled port) * serial - apply changes to inverter New implementation is trivial * serial - HAL - rxIrq, txIrq replaces by irqn There is only one IRQ for serial * serial - serial_post.h Generated code to normalize target configuration. jinja2 template is used to generate header file. Port handling is unified a lot. SERIAL_<type><n>_USED 0/1 - always defined, value depends on target configuration SERIAL_<type>_MASK - bitmask of used ports or given type. <port>1 is BIT(0) SERIAL_<type>_COUNT - number of enabled ports of given type SERIAL_<type>_MAX - <index of highest used port> + 1, 0 when no port is enabled * targets - remove automatically calculated valued from configs serial_post.h generated it * serial - remove definitions replaced by serial_post.h * serial - change LPUART to UART_LP1 in places LPUART is mostly handled as another UART port, this change reflects it * serial - use ARRAYLEN / ARRAYEND in some places replaces constant that may not match actual array size * serial - adapt softserial to changes * serial - whitespace, comments * softserial - fix serialTimerConfigureTimebase * serial - suspicious code * serial - unittests * serial - cleanup * serial - simpler port names Is this useful ? * serial - no port count for SITL necessary * serial - fix unittests include serial_post.h, some ports are defined, so normalization will work fine * timers - remove obsolete defines from SITL and unittests * serial - cosmetic improvements comments, whitespace, minor refactoring * serial - fix serialInit bug serialPortToDisable was preventing further tests * serial - fix possible NULL pointer dereference * serial - move serialResourceIndex to drivers * serial - refactor pp/od and pulldown/pullup/nopull Centralize serial options handling, decouple it from MCU type Move some code into new drivers/serial_impl.c * serial - make port.identifier valid early in port initialization * serial - fix unittest Code is moved around a bit to make unittests implementation easier * serial - bug - fix off-by-one error uart1/softserial1/lpuart was not working properly because of this * serial - whipespace + formating + style + comments No functional changes * utils - add popcount functions Wrap __builtin_popcount for easier use. Refactor existing code using it. Update code using BITCOUNT macro in dynamic context (BITCOUNT is for compile-time use only) * serial - inverter - simplify code * pinio - prevent array access overflow * serial - refactor SERIAL_BIDIR_* SERIAL_BIDIR_OD / SERIAL_BIDIR_PP SERIAL_PULL_DEFAULT / SERIAL_PULL_NONE / SERIAL_PULL_PD * serial - simplify code minor refactoring - cleaner AVOID_UARTx_FOR_PWM_PPM (unused anyway) - serialFindPortConfiguration* - remove code duplication - use serialType in smartaudio * serial - add port names - const string is assiociated with each compiled-in port (easy to pass around) - findSerialPortByName * cli - improve serialpassthrough - support port options (in current mode argument) - support port names (`serialpassthrough uart1`) - improve error handling; check more parse errors * cli - resources - minor refactor - prevent SERIAL_TX_NONSENSE when parsing resource type - fix possible NULL pointer dereference - store resource tag only after checking all conditions - slighty better error reporting - `resource serial_tx 1` will print resource assignment * serial - remane pulldown to SERIAL_PULL_SMARTAUDIO Make sure nobody uses it by mistake. SERIAL_PULL_SMARTAUDIO_SUPER_DANGEROUS_USE_AT_YOUR_OWN_RISK_THIS_WILL_BREAK_EVERYTHING_AND_KILL_YOUR_PET_FISH would be better choice, but try shorter version first. * smartaudio - minor refactor/improvement - Fix softserial on AT32 - make it easier to handle SA on per-cpu basis * softserial - minor refactoring no functional changes * UART - move AF handling before MCU dependent code * UART - adapt APM32 / APM32F4 code - Modeled after F4 where possible - come code moved from APM32 to APM32F4, possbily not necessary, but it improves similarity with STM32F4 * UART - APM32 - remove per-pin AF * UART - APM32 - bugfix - fix pinswap #if conditions * UART - apply some improvemnts from APM32 to STM32 * UART - add todo for F4 serial speed * UART - fix typo * UART - refactor support for USE_SMARTAUDIO_NOPULLDOWN * UART - typos, comments, minor improvements * UART - move code into enableRxIrq TODO: split into mcu-specific filer (but in sepatate PR) * UART - add UART_TRAIT_xx makes #if test easier and more consistent more traits shall follow * UART - fix variable name unused branch, would trigger compilation error otherwise * UART - use tables instead of switch * UART - smartaudio minor style change + better comments * UART - unify mspPorts iteration * UART - fix spelling * UART - another typo * UART - fix serialResourceIndex offset must be added, not subtracted offset can be negative * UART - fix typo Bad day ... * UART - use const table GCC does optimize it better. Should not cause functional change * UART - use OwnerIndex for inverter - 1 based - only UART has inversion * UART - refactor serial_resource a bit Single table + helper function. New table is easier to read * UART - serial_impl header is necessary * UART - style-only changes typos unify whitespace comment improvement add some const modifiers use cfg in uartConfigureDma minor improvemnt of for loops * UART - remove LPUART inverter LPUART always supports internal inversion, code was incomplete and unused * UART - update jinja template, regenerate files * UART - enable UART module RCC clk before configuring DMA Does not change anything, UART RCCs are enabled unconditionally
This commit is contained in:
parent
a659189bf0
commit
246d04dc57
90 changed files with 2471 additions and 1931 deletions
|
@ -328,9 +328,10 @@ typedef enum {
|
|||
} rebootTarget_e;
|
||||
|
||||
typedef struct serialPassthroughPort_s {
|
||||
int id;
|
||||
serialPortIdentifier_e id;
|
||||
uint32_t baud;
|
||||
unsigned mode;
|
||||
portMode_e mode;
|
||||
portOptions_e options;
|
||||
serialPort_t *port;
|
||||
} serialPassthroughPort_t;
|
||||
|
||||
|
@ -993,7 +994,7 @@ static void cliShowParseError(const char *cmdName)
|
|||
|
||||
static void cliShowInvalidArgumentCountError(const char *cmdName)
|
||||
{
|
||||
cliPrintErrorLinef(cmdName, "INVALID ARGUMENT COUNT", cmdName);
|
||||
cliPrintErrorLinef(cmdName, "INVALID ARGUMENT COUNT");
|
||||
}
|
||||
|
||||
static void cliShowArgumentRangeError(const char *cmdName, char *name, int min, int max)
|
||||
|
@ -1278,7 +1279,7 @@ static void printSerial(dumpFlags_t dumpMask, const serialConfig_t *serialConfig
|
|||
{
|
||||
const char *format = "serial %d %d %ld %ld %ld %ld";
|
||||
headingStr = cliPrintSectionHeading(dumpMask, false, headingStr);
|
||||
for (uint32_t i = 0; i < SERIAL_PORT_COUNT; i++) {
|
||||
for (unsigned i = 0; i < ARRAYLEN(serialConfig->portConfigs); i++) {
|
||||
if (!serialIsPortAvailable(serialConfig->portConfigs[i].identifier)) {
|
||||
continue;
|
||||
};
|
||||
|
@ -1398,24 +1399,24 @@ static void cliSerial(const char *cmdName, char *cmdline)
|
|||
}
|
||||
|
||||
#if defined(USE_SERIAL_PASSTHROUGH)
|
||||
static void cbCtrlLine(void *context, uint16_t ctrl)
|
||||
static void cbCtrlLine_reset(void *context, uint16_t ctrl)
|
||||
{
|
||||
#ifdef USE_PINIO
|
||||
int contextValue = (int)(long)context;
|
||||
if (contextValue) {
|
||||
pinioSet(contextValue - 1, !(ctrl & CTRL_LINE_STATE_DTR));
|
||||
} else
|
||||
#endif /* USE_PINIO */
|
||||
UNUSED(context);
|
||||
|
||||
if (!(ctrl & CTRL_LINE_STATE_DTR)) {
|
||||
systemReset();
|
||||
}
|
||||
}
|
||||
|
||||
static int cliParseSerialMode(const char *tok)
|
||||
#ifdef USE_PINIO
|
||||
static void cbCtrlLine_pinIO(void *context, uint16_t ctrl)
|
||||
{
|
||||
int mode = 0;
|
||||
pinioSet((intptr_t)context, !(ctrl & CTRL_LINE_STATE_DTR));
|
||||
}
|
||||
#endif
|
||||
|
||||
static portMode_e cliParseSerialMode(const char *tok)
|
||||
{
|
||||
portMode_e mode = 0;
|
||||
|
||||
if (strcasestr(tok, "rx")) {
|
||||
mode |= MODE_RX;
|
||||
|
@ -1427,6 +1428,29 @@ static int cliParseSerialMode(const char *tok)
|
|||
return mode;
|
||||
}
|
||||
|
||||
static portOptions_e cliParseSerialOptions(const char *tok)
|
||||
{
|
||||
struct {
|
||||
const char* tag;
|
||||
portOptions_e val;
|
||||
} map[] = {
|
||||
{"Invert", SERIAL_INVERTED},
|
||||
{"Stop2", SERIAL_STOPBITS_2},
|
||||
{"Even", SERIAL_PARITY_EVEN},
|
||||
{"Bidir", SERIAL_BIDIR},
|
||||
{"Pushpull", SERIAL_BIDIR_PP},
|
||||
{"Saudio", SERIAL_PULL_SMARTAUDIO},
|
||||
{"Check", SERIAL_CHECK_TX},
|
||||
};
|
||||
portOptions_e options = 0;
|
||||
for (unsigned i = 0; i < ARRAYLEN(map); i++) {
|
||||
if (strstr(tok, map[i].tag) != 0) {
|
||||
options |= map[i].val;
|
||||
}
|
||||
}
|
||||
return options;
|
||||
}
|
||||
|
||||
static void cliSerialPassthrough(const char *cmdName, char *cmdline)
|
||||
{
|
||||
if (isEmpty(cmdline)) {
|
||||
|
@ -1434,64 +1458,90 @@ static void cliSerialPassthrough(const char *cmdName, char *cmdline)
|
|||
return;
|
||||
}
|
||||
|
||||
serialPassthroughPort_t ports[2] = { {SERIAL_PORT_NONE, 0, 0, NULL}, {cliPort->identifier, 0, 0, cliPort} };
|
||||
serialPassthroughPort_t ports[2] = { {SERIAL_PORT_NONE, 0, 0, 0, NULL}, {cliPort->identifier, 0, 0, 0, cliPort} };
|
||||
bool enableBaudCb = false;
|
||||
int port1PinioDtr = 0;
|
||||
bool port1ResetOnDtr = false;
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
bool escSensorPassthrough = false;
|
||||
#endif
|
||||
char *saveptr;
|
||||
char* tok = strtok_r(cmdline, " ", &saveptr);
|
||||
int index = 0;
|
||||
|
||||
while (tok != NULL) {
|
||||
switch (index) {
|
||||
case 0:
|
||||
if (strcasestr(tok, "esc_sensor")) {
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
escSensorPassthrough = true;
|
||||
#endif
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR);
|
||||
ports[0].id = portConfig->identifier;
|
||||
} else {
|
||||
ports[0].id = atoi(tok);
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
ports[0].baud = atoi(tok);
|
||||
break;
|
||||
case 2:
|
||||
ports[0].mode = cliParseSerialMode(tok);
|
||||
break;
|
||||
case 3:
|
||||
if (strncasecmp(tok, "reset", strlen(tok)) == 0) {
|
||||
port1ResetOnDtr = true;
|
||||
#ifdef USE_PINIO
|
||||
} else if (strncasecmp(tok, "none", strlen(tok)) == 0) {
|
||||
port1PinioDtr = 0;
|
||||
int port1PinioDtr = -1; // route port2 USB DTR to pinio
|
||||
#endif
|
||||
bool port1ResetOnDtr = false; // reset board with DTR
|
||||
bool escSensorPassthrough = false;
|
||||
|
||||
char* nexttok = cmdline;
|
||||
char* tok;
|
||||
int index = 0;
|
||||
while ((tok = strsep(&nexttok, " ")) != NULL) {
|
||||
if (*tok == '\0') { // skip adjacent delimiters
|
||||
continue;
|
||||
}
|
||||
unsigned portN = (index < 4) ? 0 : 1; // port1 / port2
|
||||
switch (index) {
|
||||
case 0: // port1 to open: esc_sensor, portName, port ID port1
|
||||
case 4: // port2 to use (defaults to CLI serial if no more arguments)
|
||||
{
|
||||
serialPortIdentifier_e portId;
|
||||
char* endptr;
|
||||
if (portN == 0 && strcasestr(tok, "esc_sensor") != NULL) {
|
||||
escSensorPassthrough = true;
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_ESC_SENSOR);
|
||||
portId = portConfig ? portConfig->identifier : SERIAL_PORT_NONE;
|
||||
} else if (strcasecmp(tok, "cli") == 0) {
|
||||
portId = cliPort->identifier;
|
||||
} else if ((portId = findSerialPortByName(tok, strcasecmp)) >= 0) {
|
||||
// empty
|
||||
} else if ((portId = strtol(tok, &endptr, 10)) >= 0 && *endptr == '\0') {
|
||||
// empty
|
||||
} else {
|
||||
port1PinioDtr = atoi(tok);
|
||||
if (port1PinioDtr < 0 || port1PinioDtr > PINIO_COUNT) {
|
||||
cliPrintLinef("Invalid PinIO number %d", port1PinioDtr);
|
||||
return ;
|
||||
}
|
||||
#endif /* USE_PINIO */
|
||||
cliPrintLinef("Failed parsing port%d (%s)", portN + 1, tok);
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
ports[1].id = atoi(tok);
|
||||
ports[1].port = NULL;
|
||||
break;
|
||||
case 5:
|
||||
ports[1].baud = atoi(tok);
|
||||
break;
|
||||
case 6:
|
||||
ports[1].mode = cliParseSerialMode(tok);
|
||||
if (portN == 1) { // port1 is specified, don't use CLI port
|
||||
ports[portN].port = NULL;
|
||||
}
|
||||
ports[portN].id = portId;
|
||||
break;
|
||||
}
|
||||
case 1: // baudrate
|
||||
case 5: {
|
||||
int baud = atoi(tok);
|
||||
ports[portN].baud = baud;
|
||||
break;
|
||||
}
|
||||
case 2: // port1 mode (rx/tx/rxtx) + options
|
||||
case 6: // port2 mode + options
|
||||
ports[portN].mode = cliParseSerialMode(tok);
|
||||
ports[portN].options = cliParseSerialOptions(tok);
|
||||
break;
|
||||
case 3: // DTR action
|
||||
if (strcasecmp(tok, "reset") == 0) {
|
||||
port1ResetOnDtr = true;
|
||||
break;
|
||||
}
|
||||
if (strcasecmp(tok, "none") == 0) {
|
||||
break;
|
||||
}
|
||||
#ifdef USE_PINIO
|
||||
port1PinioDtr = atoi(tok);
|
||||
if (port1PinioDtr < 0 || port1PinioDtr >= PINIO_COUNT) {
|
||||
cliPrintLinef("Invalid PinIO number %d", port1PinioDtr);
|
||||
return;
|
||||
}
|
||||
#endif /* USE_PINIO */
|
||||
break;
|
||||
default:
|
||||
cliPrintLinef("Unexpected argument %d (%s)", index + 1, tok);
|
||||
return;
|
||||
}
|
||||
index++;
|
||||
tok = strtok_r(NULL, " ", &saveptr);
|
||||
}
|
||||
|
||||
|
||||
for (unsigned i = 0; i < ARRAYLEN(ports); i++) {
|
||||
if (findSerialPortIndexByIdentifier(ports[i].id) < 0) {
|
||||
cliPrintLinef("Invalid port%d %d", i + 1, ports[i].id);
|
||||
return;
|
||||
} else {
|
||||
cliPrintLinef("Port%d: %s", i + 1, serialName(ports[i].id, "<invalid>"));
|
||||
}
|
||||
}
|
||||
|
||||
// Port checks
|
||||
|
@ -1500,74 +1550,70 @@ static void cliSerialPassthrough(const char *cmdName, char *cmdline)
|
|||
return ;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
if (findSerialPortIndexByIdentifier(ports[i].id) == -1) {
|
||||
cliPrintLinef("Invalid port%d %d", i + 1, ports[i].id);
|
||||
return ;
|
||||
} else {
|
||||
cliPrintLinef("Port%d: %d ", i + 1, ports[i].id);
|
||||
}
|
||||
}
|
||||
|
||||
if (ports[0].baud == 0 && ports[1].id == SERIAL_PORT_USB_VCP) {
|
||||
enableBaudCb = true;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
serialPort_t **port = &(ports[i].port);
|
||||
if (*port != NULL) {
|
||||
serialPassthroughPort_t* cfg = &ports[i];
|
||||
if (cfg->port != NULL) { // port already selected, don't touch it (used when port2 defaults to cli)
|
||||
continue;
|
||||
}
|
||||
|
||||
int portIndex = i + 1;
|
||||
serialPortUsage_t *portUsage = findSerialPortUsageByIdentifier(ports[i].id);
|
||||
serialPortUsage_t *portUsage = findSerialPortUsageByIdentifier(cfg->id);
|
||||
if (!portUsage || portUsage->serialPort == NULL) {
|
||||
bool isUseDefaultBaud = false;
|
||||
if (ports[i].baud == 0) {
|
||||
// serial port is not open yet
|
||||
const bool isUseDefaultBaud = cfg->baud == 0;
|
||||
if (isUseDefaultBaud) {
|
||||
// Set default baud
|
||||
ports[i].baud = 57600;
|
||||
isUseDefaultBaud = true;
|
||||
cfg->baud = 57600;
|
||||
}
|
||||
|
||||
if (!ports[i].mode) {
|
||||
ports[i].mode = MODE_RXTX;
|
||||
if (!cfg->mode) {
|
||||
cliPrintLinef("Using RXTX mode as default");
|
||||
cfg->mode = MODE_RXTX;
|
||||
}
|
||||
|
||||
*port = openSerialPort(ports[i].id, FUNCTION_NONE, NULL, NULL,
|
||||
ports[i].baud, ports[i].mode,
|
||||
SERIAL_NOT_INVERTED);
|
||||
if (!*port) {
|
||||
if (cfg->options) {
|
||||
cliPrintLinef("Port%d: using options 0x%x",
|
||||
portIndex, cfg->options);
|
||||
}
|
||||
cfg->port = openSerialPort(cfg->id, FUNCTION_NONE,
|
||||
NULL, NULL, // rxCallback
|
||||
cfg->baud, cfg->mode, cfg->options);
|
||||
if (!cfg->port) {
|
||||
cliPrintLinef("Port%d could not be opened.", portIndex);
|
||||
return;
|
||||
}
|
||||
|
||||
if (isUseDefaultBaud) {
|
||||
cliPrintf("Port%d opened, default baud = %d.\r\n", portIndex, ports[i].baud);
|
||||
} else {
|
||||
cliPrintf("Port%d opened, baud = %d.\r\n", portIndex, ports[i].baud);
|
||||
}
|
||||
cliPrintf("Port%d opened, %sbaud = %d.\r\n", portIndex, isUseDefaultBaud ? "default ":"", cfg->baud);
|
||||
} else {
|
||||
*port = portUsage->serialPort;
|
||||
cfg->port = portUsage->serialPort;
|
||||
// If the user supplied a mode, override the port's mode, otherwise
|
||||
// leave the mode unchanged. serialPassthrough() handles one-way ports.
|
||||
// Set the baud rate if specified
|
||||
if (ports[i].baud) {
|
||||
cliPrintf("Port%d is already open, setting baud = %d.\r\n", portIndex, ports[i].baud);
|
||||
serialSetBaudRate(*port, ports[i].baud);
|
||||
} else {
|
||||
cliPrintf("Port%d is already open, baud = %d.\r\n", portIndex, (*port)->baudRate);
|
||||
if (cfg->baud) {
|
||||
serialSetBaudRate(cfg->port, cfg->baud);
|
||||
}
|
||||
cliPrintLinef("Port%d is already open, %sbaud = %d.", portIndex, cfg->baud ? "new " : "", cfg->port->baudRate);
|
||||
|
||||
if (cfg->mode && cfg->port->mode != cfg->mode) {
|
||||
cliPrintLinef("Port%d mode changed from %d to %d.",
|
||||
portIndex, cfg->port->mode, cfg->mode);
|
||||
serialSetMode(cfg->port, cfg->mode);
|
||||
}
|
||||
|
||||
if (ports[i].mode && (*port)->mode != ports[i].mode) {
|
||||
cliPrintf("Port%d mode changed from %d to %d.\r\n",
|
||||
portIndex, (*port)->mode, ports[i].mode);
|
||||
serialSetMode(*port, ports[i].mode);
|
||||
if (cfg->options) {
|
||||
cliPrintLinef("Port%d is open, can't change options from 0x%x to 0x%x",
|
||||
portIndex, cfg->port->options, cfg->options);
|
||||
}
|
||||
|
||||
// If this port has a rx callback associated we need to remove it now.
|
||||
// Otherwise no data will be pushed in the serial port buffer!
|
||||
if ((*port)->rxCallback) {
|
||||
(*port)->rxCallback = NULL;
|
||||
if (cfg->port->rxCallback) {
|
||||
cliPrintLinef("Port%d: Callback removed", portIndex);
|
||||
cfg->port->rxCallback = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -1580,20 +1626,26 @@ static void cliSerialPassthrough(const char *cmdName, char *cmdline)
|
|||
serialSetBaudRateCb(ports[1].port, serialSetBaudRate, ports[0].port);
|
||||
}
|
||||
|
||||
char *resetMessage = "";
|
||||
const char *resetMessage = "";
|
||||
if (port1ResetOnDtr && ports[1].id == SERIAL_PORT_USB_VCP) {
|
||||
resetMessage = "or drop DTR ";
|
||||
}
|
||||
|
||||
cliPrintLinef("Forwarding, power cycle %sto exit.", resetMessage);
|
||||
|
||||
if ((ports[1].id == SERIAL_PORT_USB_VCP) && (port1ResetOnDtr
|
||||
if ((ports[1].id == SERIAL_PORT_USB_VCP)) {
|
||||
do {
|
||||
if (port1ResetOnDtr) {
|
||||
serialSetCtrlLineStateCb(ports[1].port, cbCtrlLine_reset, NULL);
|
||||
break;
|
||||
}
|
||||
#ifdef USE_PINIO
|
||||
|| port1PinioDtr
|
||||
if (port1PinioDtr >= 0) {
|
||||
serialSetCtrlLineStateCb(ports[1].port, cbCtrlLine_pinIO, (void *)(intptr_t)(port1PinioDtr));
|
||||
break;
|
||||
}
|
||||
#endif /* USE_PINIO */
|
||||
)) {
|
||||
// Register control line state callback
|
||||
serialSetCtrlLineStateCb(ports[0].port, cbCtrlLine, (void *)(intptr_t)(port1PinioDtr));
|
||||
} while (0);
|
||||
}
|
||||
|
||||
// XXX Review ESC pass through under refactored motor handling
|
||||
|
@ -1608,7 +1660,7 @@ static void cliSerialPassthrough(const char *cmdName, char *cmdline)
|
|||
const timerHardware_t *timerHardware = timerGetConfiguredByTag(tag);
|
||||
if (timerHardware) {
|
||||
IO_t io = IOGetByTag(tag);
|
||||
IOInit(io, OWNER_MOTOR, 0);
|
||||
IOInit(io, OWNER_MOTOR, i);
|
||||
IOConfigGPIO(io, IOCFG_OUT_PP);
|
||||
if (timerHardware->output & TIMER_OUTPUT_INVERTED) {
|
||||
IOLo(io);
|
||||
|
@ -5022,7 +5074,7 @@ typedef struct {
|
|||
|
||||
const cliResourceValue_t resourceTable[] = {
|
||||
#if defined(USE_BEEPER)
|
||||
DEFS( OWNER_BEEPER, PG_BEEPER_DEV_CONFIG, beeperDevConfig_t, ioTag) ,
|
||||
DEFS( OWNER_BEEPER, PG_BEEPER_DEV_CONFIG, beeperDevConfig_t, ioTag ),
|
||||
#endif
|
||||
DEFA( OWNER_MOTOR, PG_MOTOR_CONFIG, motorConfig_t, dev.ioTags[0], MAX_SUPPORTED_MOTORS ),
|
||||
#if defined(USE_SERVOS)
|
||||
|
@ -5041,20 +5093,21 @@ const cliResourceValue_t resourceTable[] = {
|
|||
#if defined(USE_LED_STRIP)
|
||||
DEFS( OWNER_LED_STRIP, PG_LED_STRIP_CONFIG, ledStripConfig_t, ioTag ),
|
||||
#endif
|
||||
#ifdef USE_UART
|
||||
DEFA( OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagTx[0], SERIAL_UART_COUNT ),
|
||||
DEFA( OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagRx[0], SERIAL_UART_COUNT ),
|
||||
#if defined(USE_UART)
|
||||
DEFA( OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagTx[RESOURCE_UART_OFFSET], RESOURCE_UART_COUNT ),
|
||||
DEFA( OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagRx[RESOURCE_UART_OFFSET], RESOURCE_UART_COUNT ),
|
||||
#endif
|
||||
#ifdef USE_INVERTER
|
||||
DEFA( OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagInverter[0], SERIAL_PORT_MAX_INDEX ),
|
||||
#if defined(USE_INVERTER)
|
||||
DEFA( OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagInverter[RESOURCE_UART_OFFSET], RESOURCE_UART_COUNT ),
|
||||
// LPUART and SOFTSERIAL don't need external inversion
|
||||
#endif
|
||||
#if defined(USE_SOFTSERIAL)
|
||||
DEFA( OWNER_SOFTSERIAL_TX, PG_SOFTSERIAL_PIN_CONFIG, softSerialPinConfig_t, ioTagTx[0], SOFTSERIAL_COUNT ),
|
||||
DEFA( OWNER_SOFTSERIAL_RX, PG_SOFTSERIAL_PIN_CONFIG, softSerialPinConfig_t, ioTagRx[0], SOFTSERIAL_COUNT ),
|
||||
DEFA( OWNER_SOFTSERIAL_TX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagTx[RESOURCE_SOFTSERIAL_OFFSET], RESOURCE_SOFTSERIAL_COUNT ),
|
||||
DEFA( OWNER_SOFTSERIAL_RX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagRx[RESOURCE_SOFTSERIAL_OFFSET], RESOURCE_SOFTSERIAL_COUNT ),
|
||||
#endif
|
||||
#ifdef USE_LPUART1
|
||||
DEFA( OWNER_LPUART_TX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagTx[SERIAL_UART_COUNT], SERIAL_LPUART_COUNT ),
|
||||
DEFA( OWNER_LPUART_RX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagRx[SERIAL_UART_COUNT], SERIAL_LPUART_COUNT ),
|
||||
#if defined(USE_LPUART)
|
||||
DEFA( OWNER_LPUART_TX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagTx[RESOURCE_LPUART_OFFSET], RESOURCE_LPUART_COUNT ),
|
||||
DEFA( OWNER_LPUART_RX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagRx[RESOURCE_LPUART_OFFSET], RESOURCE_LPUART_COUNT ),
|
||||
#endif
|
||||
#ifdef USE_I2C
|
||||
DEFW( OWNER_I2C_SCL, PG_I2C_CONFIG, i2cConfig_t, ioTagScl, I2CDEV_COUNT ),
|
||||
|
@ -5163,10 +5216,10 @@ const cliResourceValue_t resourceTable[] = {
|
|||
#undef DEFA
|
||||
#undef DEFW
|
||||
|
||||
static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index)
|
||||
static ioTag_t* getIoTag(const cliResourceValue_t value, uint8_t index)
|
||||
{
|
||||
const pgRegistry_t* rec = pgFind(value.pgn);
|
||||
return CONST_CAST(ioTag_t *, rec->address + value.stride * index + value.offset);
|
||||
return (ioTag_t *)(rec->address + value.stride * index + value.offset);
|
||||
}
|
||||
|
||||
static void printResource(dumpFlags_t dumpMask, const char *headingStr)
|
||||
|
@ -5321,6 +5374,7 @@ typedef struct dmaoptEntry_s {
|
|||
// DEFS : Single entry
|
||||
// DEFA : Array of uint8_t (stride = 1)
|
||||
// DEFW : Wider stride case; array of structs.
|
||||
// DEFW_OFS: array of structs, starting at offset ofs
|
||||
|
||||
#define DEFS(device, peripheral, pgn, type, member) \
|
||||
{ device, peripheral, pgn, 0, offsetof(type, member), 0, MASK_IGNORED }
|
||||
|
@ -5329,7 +5383,10 @@ typedef struct dmaoptEntry_s {
|
|||
{ device, peripheral, pgn, sizeof(uint8_t), offsetof(type, member), max, mask }
|
||||
|
||||
#define DEFW(device, peripheral, pgn, type, member, max, mask) \
|
||||
{ device, peripheral, pgn, sizeof(type), offsetof(type, member), max, mask }
|
||||
DEFW_OFS(device, peripheral, pgn, type, member, 0, max, mask)
|
||||
|
||||
#define DEFW_OFS(device, peripheral, pgn, type, member, ofs, max, mask) \
|
||||
{ device, peripheral, pgn, sizeof(type), offsetof(type, member) + (ofs) * sizeof(type), max, mask }
|
||||
|
||||
dmaoptEntry_t dmaoptEntryTable[] = {
|
||||
DEFW("SPI_SDO", DMA_PERIPH_SPI_SDO, PG_SPI_PIN_CONFIG, spiPinConfig_t, txDmaopt, SPIDEV_COUNT, MASK_IGNORED),
|
||||
|
@ -5339,8 +5396,14 @@ dmaoptEntry_t dmaoptEntryTable[] = {
|
|||
DEFW("SPI_RX", DMA_PERIPH_SPI_SDI, PG_SPI_PIN_CONFIG, spiPinConfig_t, rxDmaopt, SPIDEV_COUNT, MASK_IGNORED),
|
||||
DEFA("ADC", DMA_PERIPH_ADC, PG_ADC_CONFIG, adcConfig_t, dmaopt, ADCDEV_COUNT, MASK_IGNORED),
|
||||
DEFS("SDIO", DMA_PERIPH_SDIO, PG_SDIO_CONFIG, sdioConfig_t, dmaopt),
|
||||
DEFW("UART_TX", DMA_PERIPH_UART_TX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, txDmaopt, UARTDEV_CONFIG_MAX, MASK_IGNORED),
|
||||
DEFW("UART_RX", DMA_PERIPH_UART_RX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, rxDmaopt, UARTDEV_CONFIG_MAX, MASK_IGNORED),
|
||||
#ifdef USE_UART
|
||||
DEFW_OFS("UART_TX", DMA_PERIPH_UART_TX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, txDmaopt, RESOURCE_UART_OFFSET, RESOURCE_UART_COUNT, MASK_IGNORED),
|
||||
DEFW_OFS("UART_RX", DMA_PERIPH_UART_RX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, rxDmaopt, RESOURCE_UART_OFFSET, RESOURCE_UART_COUNT, MASK_IGNORED),
|
||||
#endif
|
||||
#ifdef USE_LPUART
|
||||
DEFW_OFS("LPUART_TX", DMA_PERIPH_UART_TX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, txDmaopt, RESOURCE_LPUART_OFFSET, RESOURCE_LPUART_COUNT, MASK_IGNORED),
|
||||
DEFW_OFS("LPUART_RX", DMA_PERIPH_UART_RX, PG_SERIAL_UART_CONFIG, serialUartConfig_t, rxDmaopt, RESOURCE_LPUART_OFFSET, RESOURCE_LPUART_COUNT, MASK_IGNORED),
|
||||
#endif
|
||||
#if defined(STM32H7) || defined(STM32G4)
|
||||
DEFW("TIMUP", DMA_PERIPH_TIMUP, PG_TIMER_UP_CONFIG, timerUpConfig_t, dmaopt, HARDWARE_TIMER_DEFINITION_COUNT, TIMUP_TIMERS),
|
||||
#endif
|
||||
|
@ -6071,7 +6134,7 @@ static void cliResource(const char *cmdName, char *cmdline)
|
|||
}
|
||||
|
||||
pch = strtok_r(NULL, " ", &saveptr);
|
||||
int index = atoi(pch);
|
||||
int index = pch ? atoi(pch) : 0;
|
||||
|
||||
if (resourceTable[resourceIndex].maxIndex > 0 || index > 0) {
|
||||
if (index <= 0 || index > RESOURCE_VALUE_MAX_INDEX(resourceTable[resourceIndex].maxIndex)) {
|
||||
|
@ -6083,11 +6146,13 @@ static void cliResource(const char *cmdName, char *cmdline)
|
|||
pch = strtok_r(NULL, " ", &saveptr);
|
||||
}
|
||||
|
||||
ioTag_t *tag = getIoTag(resourceTable[resourceIndex], index);
|
||||
ioTag_t *resourceTag = getIoTag(resourceTable[resourceIndex], index);
|
||||
|
||||
if (strlen(pch) > 0) {
|
||||
if (strToPin(pch, tag)) {
|
||||
if (*tag == IO_TAG_NONE) {
|
||||
if (pch && strlen(pch) > 0) {
|
||||
ioTag_t tag;
|
||||
if (strToPin(pch, &tag)) {
|
||||
if (!tag) {
|
||||
*resourceTag = tag;
|
||||
#ifdef MINIMAL_CLI
|
||||
cliPrintLine("Freed");
|
||||
#else
|
||||
|
@ -6095,9 +6160,10 @@ static void cliResource(const char *cmdName, char *cmdline)
|
|||
#endif
|
||||
return;
|
||||
} else {
|
||||
ioRec_t *rec = IO_Rec(IOGetByTag(*tag));
|
||||
ioRec_t *rec = IO_Rec(IOGetByTag(tag));
|
||||
if (rec) {
|
||||
resourceCheck(resourceIndex, index, *tag);
|
||||
*resourceTag = tag;
|
||||
resourceCheck(resourceIndex, index, tag);
|
||||
#ifdef MINIMAL_CLI
|
||||
cliPrintLinef(" %c%02d set", IO_GPIOPortIdx(rec) + 'A', IO_GPIOPinIdx(rec));
|
||||
#else
|
||||
|
@ -6106,12 +6172,18 @@ static void cliResource(const char *cmdName, char *cmdline)
|
|||
} else {
|
||||
cliShowParseError(cmdName);
|
||||
}
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
cliPrintErrorLinef(cmdName, "Failed to parse '%s' as pin", pch);
|
||||
}
|
||||
} else {
|
||||
ioTag_t tag = *resourceTag;
|
||||
char ioName[5];
|
||||
if (tag) {
|
||||
tfp_sprintf(ioName, "%c%02d", IO_GPIOPortIdxByTag(tag) + 'A', IO_GPIOPinIdxByTag(tag));
|
||||
}
|
||||
cliPrintLinef("# resource %s %d %s", ownerNames[resourceTable[resourceIndex].owner], RESOURCE_INDEX(index), tag ? ioName : "NONE");
|
||||
}
|
||||
|
||||
cliShowParseError(cmdName);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue