Merge pull request #10558 from iNavFlight/MrD_Correct-comments-on-OSD-symbols
[inav.git] / src / main / drivers / serial_softserial.c
blob09352f4d61b303c8a0e25633f620353cb9f2452f
1 /*
2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
19 * Cleanflight (or Baseflight): original
20 * jflyper: Mono-timer and single-wire half-duplex
21 * jflyper: Ported to INAV
24 #include <stdbool.h>
25 #include <stdint.h>
27 #include "platform.h"
29 #if defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)
31 #include "build/build_config.h"
32 #include "build/atomic.h"
34 #include "build/debug.h"
36 #include "common/utils.h"
38 #include "drivers/nvic.h"
39 #include "drivers/io.h"
40 #include "drivers/timer.h"
42 #include "serial.h"
43 #include "serial_softserial.h"
45 #include "fc/config.h" //!!TODO remove this dependency
47 #define RX_TOTAL_BITS 10
48 #define TX_TOTAL_BITS 10
50 #if defined(USE_SOFTSERIAL1) && defined(USE_SOFTSERIAL2)
51 #define MAX_SOFTSERIAL_PORTS 2
52 #else
53 #define MAX_SOFTSERIAL_PORTS 1
54 #endif
56 typedef enum {
57 TIMER_MODE_SINGLE,
58 TIMER_MODE_DUAL,
59 } timerMode_e;
61 #define ICPOLARITY_RISING true
62 #define ICPOLARITY_FALLING false
64 typedef struct softSerial_s {
65 serialPort_t port;
67 IO_t rxIO;
68 IO_t txIO;
70 TCH_t * tch;
71 TCH_t * exTch;
73 timerCallbacks_t tchCallbacks;
74 timerCallbacks_t exTchCallbacks;
76 volatile uint8_t rxBuffer[SOFTSERIAL_BUFFER_SIZE];
77 volatile uint8_t txBuffer[SOFTSERIAL_BUFFER_SIZE];
79 uint8_t isSearchingForStartBit;
80 uint8_t rxBitIndex;
81 uint8_t rxLastLeadingEdgeAtBitIndex;
82 uint8_t rxEdge;
83 uint8_t rxActive;
85 uint8_t isTransmittingData;
86 int8_t bitsLeftToTransmit;
88 uint16_t internalTxBuffer; // includes start and stop bits
89 uint16_t internalRxBuffer; // includes start and stop bits
91 uint16_t transmissionErrors;
92 uint16_t receiveErrors;
94 uint8_t softSerialPortIndex;
95 timerMode_e timerMode;
96 } softSerial_t;
98 static const struct serialPortVTable softSerialVTable; // Forward
100 static softSerial_t softSerialPorts[MAX_SOFTSERIAL_PORTS];
102 void onSerialTimerOverflow(TCH_t * tch, uint32_t capture);
103 void onSerialRxPinChange(TCH_t * tch, uint32_t capture);
105 static void setTxSignal(softSerial_t *softSerial, uint8_t state)
107 if (softSerial->port.options & SERIAL_INVERTED) {
108 state = !state;
111 if (state) {
112 IOHi(softSerial->txIO);
113 } else {
114 IOLo(softSerial->txIO);
118 static void serialEnableCC(softSerial_t *softSerial)
120 timerChCaptureEnable(softSerial->tch);
123 static void serialInputPortActivate(softSerial_t *softSerial)
125 if (softSerial->port.options & SERIAL_INVERTED) {
126 const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_PD;
127 IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->tch->timHw->alternateFunction);
128 } else {
129 const uint8_t pinConfig = (softSerial->port.options & SERIAL_BIDIR_NOPULL) ? IOCFG_AF_PP : IOCFG_AF_PP_UP;
130 IOConfigGPIOAF(softSerial->rxIO, pinConfig, softSerial->tch->timHw->alternateFunction);
133 softSerial->rxActive = true;
134 softSerial->isSearchingForStartBit = true;
135 softSerial->rxBitIndex = 0;
137 // Enable input capture
138 serialEnableCC(softSerial);
141 static void serialInputPortDeActivate(softSerial_t *softSerial)
143 // Disable input capture
144 timerChCaptureDisable(softSerial->tch);
145 IOConfigGPIOAF(softSerial->rxIO, IOCFG_IN_FLOATING, softSerial->tch->timHw->alternateFunction);
146 softSerial->rxActive = false;
149 static void serialOutputPortActivate(softSerial_t *softSerial)
151 if (softSerial->exTch)
152 IOConfigGPIOAF(softSerial->txIO, IOCFG_OUT_PP, softSerial->exTch->timHw->alternateFunction);
153 else
154 IOConfigGPIO(softSerial->txIO, IOCFG_OUT_PP);
157 static void serialOutputPortDeActivate(softSerial_t *softSerial)
159 if (softSerial->exTch)
160 IOConfigGPIOAF(softSerial->txIO, IOCFG_IN_FLOATING, softSerial->exTch->timHw->alternateFunction);
161 else
162 IOConfigGPIO(softSerial->txIO, IOCFG_IN_FLOATING);
165 static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
167 return timerPeriod > 0xFFFF;
170 static void serialTimerConfigureTimebase(TCH_t * tch, uint32_t baud)
172 uint32_t baseClock = timerClock(tch->timHw->tim);
173 uint32_t clock = baseClock;
174 uint32_t timerPeriod;
176 do {
177 timerPeriod = clock / baud;
178 if (isTimerPeriodTooLarge(timerPeriod)) {
179 if (clock > 1) {
180 clock = clock / 2; // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200)
181 } else {
182 // TODO unable to continue, unable to determine clock and timerPeriods for the given baud
186 } while (isTimerPeriodTooLarge(timerPeriod));
188 timerConfigure(tch, timerPeriod, baseClock);
191 static void resetBuffers(softSerial_t *softSerial)
193 softSerial->port.rxBufferSize = SOFTSERIAL_BUFFER_SIZE;
194 softSerial->port.rxBuffer = softSerial->rxBuffer;
195 softSerial->port.rxBufferTail = 0;
196 softSerial->port.rxBufferHead = 0;
198 softSerial->port.txBuffer = softSerial->txBuffer;
199 softSerial->port.txBufferSize = SOFTSERIAL_BUFFER_SIZE;
200 softSerial->port.txBufferTail = 0;
201 softSerial->port.txBufferHead = 0;
204 serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baud, portMode_t mode, portOptions_t options)
206 softSerial_t *softSerial = &(softSerialPorts[portIndex]);
207 ioTag_t tagRx;
208 ioTag_t tagTx;
210 #ifdef USE_SOFTSERIAL1
211 if (portIndex == SOFTSERIAL1) {
212 tagRx = IO_TAG(SOFTSERIAL_1_RX_PIN);
213 tagTx = IO_TAG(SOFTSERIAL_1_TX_PIN);
215 #endif
217 #ifdef USE_SOFTSERIAL2
218 if (portIndex == SOFTSERIAL2) {
219 tagRx = IO_TAG(SOFTSERIAL_2_RX_PIN);
220 tagTx = IO_TAG(SOFTSERIAL_2_TX_PIN);
222 #endif
224 const timerHardware_t *timerRx = timerGetByTag(tagRx, TIM_USE_ANY);
225 const timerHardware_t *timerTx = timerGetByTag(tagTx, TIM_USE_ANY);
227 IO_t rxIO = IOGetByTag(tagRx);
228 IO_t txIO = IOGetByTag(tagTx);
230 if (tagRx == tagTx) {
231 if ((mode & MODE_RX) && (mode & MODE_TX)) {
232 options |= SERIAL_BIDIR;
236 if (options & SERIAL_BIDIR) {
237 // If RX and TX pins are both assigned, we CAN use either with a timer.
238 // However, for consistency with hardware UARTs, we only use TX pin,
239 // and this pin must have a timer, and it should not be N-Channel.
240 if (!timerTx || (timerTx->output & TIMER_OUTPUT_N_CHANNEL)) {
241 return NULL;
244 softSerial->tch = timerGetTCH(timerTx);
245 if (!softSerial->tch) {
246 return NULL;
249 softSerial->txIO = txIO;
250 softSerial->rxIO = txIO;
251 IOInit(txIO, OWNER_SOFTSERIAL, RESOURCE_UART_TXRX, RESOURCE_INDEX(portIndex));
252 } else {
253 if (mode & MODE_RX) {
254 // Need a pin & a timer on RX. Channel should not be N-Channel.
255 if (!timerRx || (timerRx->output & TIMER_OUTPUT_N_CHANNEL)) {
256 return NULL;
259 softSerial->tch = timerGetTCH(timerRx);
260 if (!softSerial->tch) {
261 return NULL;
264 softSerial->rxIO = rxIO;
265 IOInit(rxIO, OWNER_SOFTSERIAL, RESOURCE_UART_RX, RESOURCE_INDEX(portIndex));
268 if (mode & MODE_TX) {
269 // Need a pin on TX
270 if (!tagTx) {
271 return NULL;
274 softSerial->txIO = txIO;
276 if (!(mode & MODE_RX)) {
277 // TX Simplex, must have a timer
278 if (!timerTx) {
279 return NULL;
282 softSerial->tch = timerGetTCH(timerTx);
283 if (!softSerial->tch) {
284 return NULL;
286 } else {
287 // Duplex
288 softSerial->exTch = timerGetTCH(timerTx);
289 if (!softSerial->tch) {
290 return NULL;
293 IOInit(txIO, OWNER_SOFTSERIAL, RESOURCE_UART_TX, RESOURCE_INDEX(portIndex));
297 softSerial->port.vTable = &softSerialVTable;
298 softSerial->port.baudRate = baud;
299 softSerial->port.mode = mode;
300 softSerial->port.options = options;
301 softSerial->port.rxCallback = rxCallback;
302 softSerial->port.rxCallbackData = rxCallbackData;
304 resetBuffers(softSerial);
306 softSerial->softSerialPortIndex = portIndex;
308 softSerial->transmissionErrors = 0;
309 softSerial->receiveErrors = 0;
311 softSerial->rxActive = false;
312 softSerial->isTransmittingData = false;
314 // Configure master timer (on RX); time base and input capture
315 serialTimerConfigureTimebase(softSerial->tch, baud);
316 timerChConfigIC(softSerial->tch, options & SERIAL_INVERTED, 0);
318 // Configure bit clock interrupt & handler.
319 // If we have an extra timer (on TX), it is initialized and configured
320 // for overflow interrupt.
321 // Receiver input capture is configured when input is activated.
322 if ((mode & MODE_TX) && softSerial->exTch && softSerial->exTch->timHw->tim != softSerial->tch->timHw->tim) {
323 softSerial->timerMode = TIMER_MODE_DUAL;
324 serialTimerConfigureTimebase(softSerial->exTch, baud);
326 timerChInitCallbacks(&softSerial->tchCallbacks, softSerial, onSerialRxPinChange, NULL);
327 timerChInitCallbacks(&softSerial->exTchCallbacks, softSerial, NULL, onSerialTimerOverflow);
329 timerChConfigCallbacks(softSerial->tch, &softSerial->tchCallbacks);
330 timerChConfigCallbacks(softSerial->exTch, &softSerial->exTchCallbacks);
331 } else {
332 softSerial->timerMode = TIMER_MODE_SINGLE;
333 timerChInitCallbacks(&softSerial->tchCallbacks, softSerial, onSerialRxPinChange, onSerialTimerOverflow);
334 timerChConfigCallbacks(softSerial->tch, &softSerial->tchCallbacks);
337 if (!(options & SERIAL_BIDIR)) {
338 serialOutputPortActivate(softSerial);
339 setTxSignal(softSerial, ENABLE);
342 serialInputPortActivate(softSerial);
344 return &softSerial->port;
349 * Serial Engine
352 void processTxState(softSerial_t *softSerial)
354 uint8_t mask;
356 if (!softSerial->isTransmittingData) {
357 if (isSoftSerialTransmitBufferEmpty((serialPort_t *)softSerial)) {
358 // Transmit buffer empty.
359 // Start listening if not already in if half-duplex
360 if (!softSerial->rxActive && softSerial->port.options & SERIAL_BIDIR) {
361 serialOutputPortDeActivate(softSerial);
362 serialInputPortActivate(softSerial);
364 return;
367 // data to send
368 uint8_t byteToSend = softSerial->port.txBuffer[softSerial->port.txBufferTail++];
369 if (softSerial->port.txBufferTail >= softSerial->port.txBufferSize) {
370 softSerial->port.txBufferTail = 0;
373 // build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
374 softSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
375 softSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
376 softSerial->isTransmittingData = true;
378 if (softSerial->rxActive && (softSerial->port.options & SERIAL_BIDIR)) {
379 // Half-duplex: Deactivate receiver, activate transmitter
380 serialInputPortDeActivate(softSerial);
381 serialOutputPortActivate(softSerial);
384 if ((softSerial->port.options & SERIAL_SHORTSTOP) == 0) {
385 return;
389 if (softSerial->bitsLeftToTransmit) {
390 mask = softSerial->internalTxBuffer & 1;
391 softSerial->internalTxBuffer >>= 1;
393 setTxSignal(softSerial, mask);
394 softSerial->bitsLeftToTransmit--;
396 if (((softSerial->port.options & SERIAL_SHORTSTOP) == 0) || softSerial->bitsLeftToTransmit) {
397 return;
401 softSerial->isTransmittingData = false;
404 enum {
405 TRAILING,
406 LEADING
409 void applyChangedBits(softSerial_t *softSerial)
411 if (softSerial->rxEdge == TRAILING) {
412 uint8_t bitToSet;
413 for (bitToSet = softSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < softSerial->rxBitIndex; bitToSet++) {
414 softSerial->internalRxBuffer |= 1 << bitToSet;
419 void prepareForNextRxByte(softSerial_t *softSerial)
421 // prepare for next byte
422 softSerial->rxBitIndex = 0;
423 softSerial->isSearchingForStartBit = true;
424 if (softSerial->rxEdge == LEADING) {
425 softSerial->rxEdge = TRAILING;
426 timerChConfigIC(softSerial->tch, softSerial->port.options & SERIAL_INVERTED, 0);
427 serialEnableCC(softSerial);
431 #define STOP_BIT_MASK (1 << 0)
432 #define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
434 void extractAndStoreRxByte(softSerial_t *softSerial)
436 if ((softSerial->port.mode & MODE_RX) == 0) {
437 return;
440 uint8_t haveStartBit = (softSerial->internalRxBuffer & START_BIT_MASK) == 0;
441 uint8_t haveStopBit = (softSerial->internalRxBuffer & STOP_BIT_MASK) == 1;
443 if (!haveStartBit || !haveStopBit) {
444 softSerial->receiveErrors++;
445 return;
448 uint8_t rxByte = (softSerial->internalRxBuffer >> 1) & 0xFF;
450 if (softSerial->port.rxCallback) {
451 softSerial->port.rxCallback(rxByte, softSerial->port.rxCallbackData);
452 } else {
453 softSerial->port.rxBuffer[softSerial->port.rxBufferHead] = rxByte;
454 softSerial->port.rxBufferHead = (softSerial->port.rxBufferHead + 1) % softSerial->port.rxBufferSize;
458 void processRxState(softSerial_t *softSerial)
460 if (softSerial->isSearchingForStartBit) {
461 return;
464 softSerial->rxBitIndex++;
466 if (softSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
467 applyChangedBits(softSerial);
468 return;
471 if (softSerial->rxBitIndex == RX_TOTAL_BITS) {
473 if (softSerial->rxEdge == TRAILING) {
474 softSerial->internalRxBuffer |= STOP_BIT_MASK;
477 extractAndStoreRxByte(softSerial);
478 prepareForNextRxByte(softSerial);
482 void onSerialTimerOverflow(TCH_t * tch, uint32_t capture)
484 UNUSED(capture);
486 softSerial_t * self = (softSerial_t *)tch->cb->callbackParam;
488 if (self->port.mode & MODE_TX)
489 processTxState(self);
491 if (self->port.mode & MODE_RX)
492 processRxState(self);
495 void onSerialRxPinChange(TCH_t * tch, uint32_t capture)
497 UNUSED(capture);
499 softSerial_t * self = (softSerial_t *)tch->cb->callbackParam;
500 bool inverted = self->port.options & SERIAL_INVERTED;
502 if ((self->port.mode & MODE_RX) == 0) {
503 return;
506 if (self->isSearchingForStartBit) {
507 // Synchronize the bit timing so that it will interrupt at the center
508 // of the bit period.
510 #ifdef USE_HAL_DRIVER
511 __HAL_TIM_SET_COUNTER(self->tch->timCtx->timHandle, __HAL_TIM_GET_AUTORELOAD(self->tch->timCtx->timHandle) / 2);
512 #else
513 TIM_SetCounter(self->tch->timHw->tim, timerGetPeriod(self->tch) / 2);
514 #endif
516 // For a mono-timer full duplex configuration, this may clobber the
517 // transmission because the next callback to the onSerialTimerOverflow
518 // will happen too early causing transmission errors.
519 // For a dual-timer configuration, there is no problem.
521 if ((self->timerMode != TIMER_MODE_DUAL) && self->isTransmittingData) {
522 self->transmissionErrors++;
525 timerChConfigIC(self->tch, !inverted, 0);
526 #ifdef STM32F7
527 serialEnableCC(self);
528 #endif
529 self->rxEdge = LEADING;
531 self->rxBitIndex = 0;
532 self->rxLastLeadingEdgeAtBitIndex = 0;
533 self->internalRxBuffer = 0;
534 self->isSearchingForStartBit = false;
535 return;
538 if (self->rxEdge == LEADING) {
539 self->rxLastLeadingEdgeAtBitIndex = self->rxBitIndex;
542 applyChangedBits(self);
544 if (self->rxEdge == TRAILING) {
545 self->rxEdge = LEADING;
546 timerChConfigIC(self->tch, !inverted, 0);
547 } else {
548 self->rxEdge = TRAILING;
549 timerChConfigIC(self->tch, inverted, 0);
551 #ifdef STM32F7
552 serialEnableCC(self);
553 #endif
558 * Standard serial driver API
561 uint32_t softSerialRxBytesWaiting(const serialPort_t *instance)
563 if ((instance->mode & MODE_RX) == 0) {
564 return 0;
567 softSerial_t *s = (softSerial_t *)instance;
569 return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1);
572 uint32_t softSerialTxBytesFree(const serialPort_t *instance)
574 if ((instance->mode & MODE_TX) == 0) {
575 return 0;
578 softSerial_t *s = (softSerial_t *)instance;
580 uint32_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1);
582 return (s->port.txBufferSize - 1) - bytesUsed;
585 uint8_t softSerialReadByte(serialPort_t *instance)
587 uint8_t ch;
589 if ((instance->mode & MODE_RX) == 0) {
590 return 0;
593 if (softSerialRxBytesWaiting(instance) == 0) {
594 return 0;
597 ch = instance->rxBuffer[instance->rxBufferTail];
598 instance->rxBufferTail = (instance->rxBufferTail + 1) % instance->rxBufferSize;
599 return ch;
602 void softSerialWriteByte(serialPort_t *s, uint8_t ch)
604 if ((s->mode & MODE_TX) == 0) {
605 return;
608 s->txBuffer[s->txBufferHead] = ch;
609 s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
612 void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
614 softSerial_t *softSerial = (softSerial_t *)s;
616 softSerial->port.baudRate = baudRate;
618 serialTimerConfigureTimebase(softSerial->tch, baudRate);
621 void softSerialSetMode(serialPort_t *instance, portMode_t mode)
623 instance->mode = mode;
626 void softSerialSetOptions(serialPort_t *instance, portOptions_t options)
628 instance->options = options;
631 bool isSoftSerialTransmitBufferEmpty(const serialPort_t *instance)
633 return instance->txBufferHead == instance->txBufferTail;
636 static const struct serialPortVTable softSerialVTable = {
637 .serialWrite = softSerialWriteByte,
638 .serialTotalRxWaiting = softSerialRxBytesWaiting,
639 .serialTotalTxFree = softSerialTxBytesFree,
640 .serialRead = softSerialReadByte,
641 .serialSetBaudRate = softSerialSetBaudRate,
642 .isSerialTransmitBufferEmpty = isSoftSerialTransmitBufferEmpty,
643 .setMode = softSerialSetMode,
644 .setOptions = softSerialSetOptions,
645 .isConnected = NULL,
646 .writeBuf = NULL,
647 .beginWrite = NULL,
648 .endWrite = NULL,
649 .isIdle = NULL,
652 #endif