src/main/drivers/dshot_bitbang.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdint.h>
  22 #include <math.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_DSHOT_BITBANG
  28
  29 #include "build/debug.h"
  30 #include "build/debug_pin.h"
  31
  32 #include "drivers/io.h"
  33 #include "drivers/io_impl.h"
  34 #include "drivers/dma.h"
  35 #include "drivers/dma_reqmap.h"
  36 #include "drivers/dshot.h"
  37 #include "drivers/dshot_bitbang.h"
  38 #include "drivers/dshot_bitbang_impl.h"
  39 #include "drivers/dshot_command.h"
  40 #include "drivers/motor.h"
  41 #include "drivers/nvic.h"
  42 #include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
  43 #include "drivers/dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring
  44 #include "drivers/dshot_bitbang_decode.h"
  45 #include "drivers/time.h"
  46 #include "drivers/timer.h"
  47
  48 #include "pg/motor.h"
  49
  50 FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS];  // TIM1 or TIM8
  51 FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
  52
  53 FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS];
  54 FAST_DATA_ZERO_INIT int usedMotorPorts;
  55
  56 FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS];
  57
  58 static FAST_DATA_ZERO_INIT int motorCount;
  59 dshotBitbangStatus_e bbStatus;
  60
  61 // For MCUs that use MPU to control DMA coherency, there might be a performance hit
  62 // on manipulating input buffer content especially if it is read multiple times,
  63 // as the buffer region is attributed as not cachable.
  64 // If this is not desirable, we should use manual cache invalidation.
  65 #ifdef USE_DSHOT_CACHE_MGMT
  66 #define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32)))
  67 #define BB_INPUT_BUFFER_ATTRIBUTE  DMA_RW_AXI __attribute__((aligned(32)))
  68 #else
  69 #if defined(STM32F4)
  70 #define BB_OUTPUT_BUFFER_ATTRIBUTE
  71 #define BB_INPUT_BUFFER_ATTRIBUTE
  72 #elif defined(STM32F7)
  73 #define BB_OUTPUT_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT
  74 #define BB_INPUT_BUFFER_ATTRIBUTE  FAST_DATA_ZERO_INIT
  75 #elif defined(STM32H7)
  76 #define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RAM
  77 #define BB_INPUT_BUFFER_ATTRIBUTE  DMA_RAM
  78 #elif defined(STM32G4)
  79 #define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RAM_W
  80 #define BB_INPUT_BUFFER_ATTRIBUTE  DMA_RAM_R
  81 #endif
  82 #endif // USE_DSHOT_CACHE_MGMT
  83
  84 BB_OUTPUT_BUFFER_ATTRIBUTE uint32_t bbOutputBuffer[MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS];
  85 BB_INPUT_BUFFER_ATTRIBUTE uint16_t bbInputBuffer[DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS];
  86
  87 uint8_t bbPuPdMode;
  88 FAST_DATA_ZERO_INIT timeUs_t dshotFrameUs;
  89
  90
  91 const timerHardware_t bbTimerHardware[] = {
  92 #if defined(STM32F4) || defined(STM32F7)
  93 #if !defined(STM32F411xE)
  94     DEF_TIM(TIM8,  CH1, NONE,  TIM_USE_NONE, 0, 1),
  95     DEF_TIM(TIM8,  CH2, NONE,  TIM_USE_NONE, 0, 1),
  96     DEF_TIM(TIM8,  CH3, NONE,  TIM_USE_NONE, 0, 1),
  97     DEF_TIM(TIM8,  CH4, NONE,  TIM_USE_NONE, 0, 0),
  98 #endif
  99     DEF_TIM(TIM1,  CH1, NONE,  TIM_USE_NONE, 0, 1),
 100     DEF_TIM(TIM1,  CH1, NONE,  TIM_USE_NONE, 0, 2),
 101     DEF_TIM(TIM1,  CH2, NONE,  TIM_USE_NONE, 0, 1),
 102     DEF_TIM(TIM1,  CH3, NONE,  TIM_USE_NONE, 0, 1),
 103     DEF_TIM(TIM1,  CH4, NONE,  TIM_USE_NONE, 0, 0),
 104
 105 #elif defined(STM32G4) || defined(STM32H7)
 106     // XXX TODO: STM32G4 and STM32H7 can use any timer for pacing
 107
 108     // DMA request numbers are duplicated for TIM1 and TIM8:
 109     //   - Any pacer can serve a GPIO port.
 110     //   - For quads (or less), 4 pacers can cover the worst case scenario of
 111     //     4 motors scattered across 4 different GPIO ports.
 112     //   - For hexas (and larger), more channels may become necessary,
 113     //     in which case the DMA request numbers should be modified.
 114     DEF_TIM(TIM8,  CH1, NONE,  TIM_USE_NONE, 0, 0, 0),
 115     DEF_TIM(TIM8,  CH2, NONE,  TIM_USE_NONE, 0, 1, 0),
 116     DEF_TIM(TIM8,  CH3, NONE,  TIM_USE_NONE, 0, 2, 0),
 117     DEF_TIM(TIM8,  CH4, NONE,  TIM_USE_NONE, 0, 3, 0),
 118     DEF_TIM(TIM1,  CH1, NONE,  TIM_USE_NONE, 0, 0, 0),
 119     DEF_TIM(TIM1,  CH2, NONE,  TIM_USE_NONE, 0, 1, 0),
 120     DEF_TIM(TIM1,  CH3, NONE,  TIM_USE_NONE, 0, 2, 0),
 121     DEF_TIM(TIM1,  CH4, NONE,  TIM_USE_NONE, 0, 3, 0),
 122
 123 #else
 124 #error MCU dependent code required
 125 #endif
 126 };
 127
 128 static FAST_DATA_ZERO_INIT motorDevice_t bbDevice;
 129 static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
 130
 131 static motorPwmProtocolTypes_e motorPwmProtocol;
 132
 133 // DMA GPIO output buffer formatting
 134
 135 static void bbOutputDataInit(uint32_t *buffer, uint16_t portMask, bool inverted)
 136 {
 137     uint32_t resetMask;
 138     uint32_t setMask;
 139
 140     if (inverted) {
 141         resetMask = portMask;
 142         setMask = (portMask << 16);
 143     } else {
 144         resetMask = (portMask << 16);
 145         setMask = portMask;
 146     }
 147
 148     int symbol_index;
 149
 150     for (symbol_index = 0; symbol_index < MOTOR_DSHOT_FRAME_BITS; symbol_index++) {
 151         buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 0] |= setMask ; // Always set all ports
 152         buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 1] = 0;          // Reset bits are port dependent
 153         buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 2] |= resetMask; // Always reset all ports
 154     }
 155
 156     //
 157     // output one more 'bit' that keeps the line level at idle to allow the ESC to sample the last bit
 158     //
 159     // Avoid CRC errors in the case of bi-directional d-shot.  CRC errors can occur if the output is
 160     // transitioned to an input before the signal has been sampled by the ESC as the sampled voltage
 161     // may be somewhere between logic-high and logic-low depending on how the motor output line is
 162     // driven or floating.  On some MCUs it's observed that the voltage momentarily drops low on transition
 163     // to input.
 164
 165     int hold_bit_index = MOTOR_DSHOT_FRAME_BITS * MOTOR_DSHOT_STATE_PER_SYMBOL;
 166     buffer[hold_bit_index + 0] |= resetMask; // Always reset all ports
 167     buffer[hold_bit_index + 1] = 0;          // Never any change
 168     buffer[hold_bit_index + 2] = 0;          // Never any change
 169 }
 170
 171 static void bbOutputDataSet(uint32_t *buffer, int pinNumber, uint16_t value, bool inverted)
 172 {
 173     uint32_t middleBit;
 174
 175     if (inverted) {
 176         middleBit = (1 << (pinNumber + 0));
 177     } else {
 178         middleBit = (1 << (pinNumber + 16));
 179     }
 180
 181     for (int pos = 0; pos < 16; pos++) {
 182         if (!(value & 0x8000)) {
 183             buffer[pos * 3 + 1] |= middleBit;
 184         }
 185         value <<= 1;
 186     }
 187 }
 188
 189 static void bbOutputDataClear(uint32_t *buffer)
 190 {
 191     // Middle position to no change
 192     for (int bitpos = 0; bitpos < 16; bitpos++) {
 193         buffer[bitpos * 3 + 1] = 0;
 194     }
 195 }
 196
 197 // bbPacer management
 198
 199 static bbPacer_t *bbFindMotorPacer(TIM_TypeDef *tim)
 200 {
 201     for (int i = 0; i < MAX_MOTOR_PACERS; i++) {
 202
 203         bbPacer_t *bbPacer = &bbPacers[i];
 204
 205         if (bbPacer->tim == NULL) {
 206             bbPacer->tim = tim;
 207             ++usedMotorPacers;
 208             return bbPacer;
 209         }
 210
 211         if (bbPacer->tim == tim) {
 212             return bbPacer;
 213         }
 214     }
 215
 216     return NULL;
 217 }
 218
 219 // bbPort management
 220
 221 static bbPort_t *bbFindMotorPort(int portIndex)
 222 {
 223     for (int i = 0; i < usedMotorPorts; i++) {
 224         if (bbPorts[i].portIndex == portIndex) {
 225             return &bbPorts[i];
 226         }
 227     }
 228     return NULL;
 229 }
 230
 231 static bbPort_t *bbAllocateMotorPort(int portIndex)
 232 {
 233     if (usedMotorPorts >= MAX_SUPPORTED_MOTOR_PORTS) {
 234         bbStatus = DSHOT_BITBANG_STATUS_TOO_MANY_PORTS;
 235         return NULL;
 236     }
 237
 238     bbPort_t *bbPort = &bbPorts[usedMotorPorts];
 239
 240     if (!bbPort->timhw) {
 241         // No more pacer channel available
 242         bbStatus = DSHOT_BITBANG_STATUS_NO_PACER;
 243         return NULL;
 244     }
 245
 246     bbPort->portIndex = portIndex;
 247     bbPort->owner.owner = OWNER_DSHOT_BITBANG;
 248     bbPort->owner.resourceIndex = RESOURCE_INDEX(portIndex);
 249
 250     ++usedMotorPorts;
 251
 252     return bbPort;
 253 }
 254
 255 const timerHardware_t *dshotBitbangTimerGetAllocatedByNumberAndChannel(int8_t timerNumber, uint16_t timerChannel)
 256 {
 257     for (int index = 0; index < usedMotorPorts; index++) {
 258         const timerHardware_t *bitbangTimer = bbPorts[index].timhw;
 259         if (bitbangTimer && timerGetTIMNumber(bitbangTimer->tim) == timerNumber && bitbangTimer->channel == timerChannel && bbPorts[index].owner.owner) {
 260             return bitbangTimer;
 261         }
 262     }
 263
 264     return NULL;
 265 }
 266
 267 const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer)
 268 {
 269     for (int index = 0; index < usedMotorPorts; index++) {
 270         const timerHardware_t *bitbangTimer = bbPorts[index].timhw;
 271         if (bitbangTimer && bitbangTimer == timer) {
 272             return &bbPorts[index].owner;
 273         }
 274     }
 275
 276     return &freeOwner;
 277 }
 278
 279 // Return frequency of smallest change [state/sec]
 280
 281 static uint32_t getDshotBaseFrequency(motorPwmProtocolTypes_e pwmProtocolType)
 282 {
 283     switch (pwmProtocolType) {
 284     case(PWM_TYPE_DSHOT600):
 285         return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
 286     case(PWM_TYPE_DSHOT300):
 287         return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
 288     default:
 289     case(PWM_TYPE_DSHOT150):
 290         return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
 291     }
 292 }
 293
 294 static void bbSetupDma(bbPort_t *bbPort)
 295 {
 296     const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(bbPort->dmaResource);
 297     dmaEnable(dmaIdentifier);
 298     bbPort->dmaSource = timerDmaSource(bbPort->timhw->channel);
 299
 300     bbPacer_t *bbPacer = bbFindMotorPacer(bbPort->timhw->tim);
 301     bbPacer->dmaSources |= bbPort->dmaSource;
 302
 303     dmaSetHandler(dmaIdentifier, bbDMAIrqHandler, NVIC_BUILD_PRIORITY(2, 1), (uint32_t)bbPort);
 304
 305     bbDMA_ITConfig(bbPort);
 306 }
 307
 308 FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
 309 {
 310     dbgPinHi(0);
 311
 312     bbPort_t *bbPort = (bbPort_t *)descriptor->userParam;
 313
 314     bbDMA_Cmd(bbPort, DISABLE);
 315
 316     bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, DISABLE);
 317
 318     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) {
 319         while (1) {};
 320     }
 321
 322     DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 323
 324 #ifdef USE_DSHOT_TELEMETRY
 325     if (useDshotTelemetry) {
 326         if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
 327 #ifdef DEBUG_COUNT_INTERRUPT
 328             bbPort->inputIrq++;
 329 #endif
 330         } else {
 331 #ifdef DEBUG_COUNT_INTERRUPT
 332             bbPort->outputIrq++;
 333 #endif
 334
 335             // Switch to input
 336
 337             bbSwitchToInput(bbPort);
 338
 339             bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE);
 340         }
 341     }
 342 #endif
 343     dbgPinLo(0);
 344 }
 345
 346 // Setup bbPorts array elements so that they each have a TIM1 or TIM8 channel
 347 // in timerHardware array for BB-DShot.
 348
 349 static void bbFindPacerTimer(void)
 350 {
 351     for (int bbPortIndex = 0; bbPortIndex < MAX_SUPPORTED_MOTOR_PORTS; bbPortIndex++) {
 352         for (unsigned timerIndex = 0; timerIndex < ARRAYLEN(bbTimerHardware); timerIndex++) {
 353             const timerHardware_t *timer = &bbTimerHardware[timerIndex];
 354             int timNumber = timerGetTIMNumber(timer->tim);
 355             if ((motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM1 && timNumber != 1)
 356                 || (motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM8 && timNumber != 8)) {
 357                 continue;
 358             }
 359             bool timerConflict = false;
 360             for (int channel = 0; channel < CC_CHANNELS_PER_TIMER; channel++) {
 361                 const timerHardware_t *timer = timerGetAllocatedByNumberAndChannel(timNumber, CC_CHANNEL_FROM_INDEX(channel));
 362                 const resourceOwner_e timerOwner = timerGetOwner(timer)->owner;
 363                 if (timerOwner != OWNER_FREE && timerOwner != OWNER_DSHOT_BITBANG) {
 364                     timerConflict = true;
 365                     break;
 366                 }
 367             }
 368
 369             for (int index = 0; index < bbPortIndex; index++) {
 370                 const timerHardware_t* t = bbPorts[index].timhw;
 371                 if (timerGetTIMNumber(t->tim) == timNumber && timer->channel == t->channel) {
 372                     timerConflict = true;
 373                     break;
 374                 }
 375             }
 376
 377             if (timerConflict) {
 378                 continue;
 379             }
 380
 381 #ifdef USE_DMA_SPEC
 382             dmaoptValue_t dmaopt = dmaGetOptionByTimer(timer);
 383             const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timer->tim, timer->channel, dmaopt);
 384             dmaResource_t *dma = dmaChannelSpec->ref;
 385 #else
 386             dmaResource_t *dma = timer->dmaRef;
 387 #endif
 388             dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dma);
 389             if (dmaGetOwner(dmaIdentifier)->owner == OWNER_FREE) {
 390                 bbPorts[bbPortIndex].timhw = timer;
 391
 392                 break;
 393             }
 394         }
 395     }
 396 }
 397
 398 static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProtocolType)
 399 {
 400     uint32_t timerclock = timerClock(bbPort->timhw->tim);
 401
 402     uint32_t outputFreq = getDshotBaseFrequency(dshotProtocolType);
 403     dshotFrameUs = 1000000 * 17 * 3 / outputFreq;
 404     bbPort->outputARR = timerclock / outputFreq - 1;
 405
 406     // XXX Explain this formula
 407     uint32_t inputFreq = outputFreq * 5 * 2 * DSHOT_BITBANG_TELEMETRY_OVER_SAMPLE / 24;
 408     bbPort->inputARR = timerclock / inputFreq - 1;
 409 }
 410
 411 //
 412 // bb only use pin info associated with timerHardware entry designated as TIM_USE_MOTOR;
 413 // it does not use the timer channel associated with the pin.
 414 //
 415
 416 static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 417 {
 418     int pinIndex = IO_GPIOPinIdx(io);
 419     int portIndex = IO_GPIOPortIdx(io);
 420
 421     bbPort_t *bbPort = bbFindMotorPort(portIndex);
 422
 423     if (!bbPort) {
 424
 425         // New port group
 426
 427         bbPort = bbAllocateMotorPort(portIndex);
 428
 429         if (bbPort) {
 430             const timerHardware_t *timhw = bbPort->timhw;
 431
 432 #ifdef USE_DMA_SPEC
 433             const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timhw->tim, timhw->channel, dmaGetOptionByTimer(timhw));
 434             bbPort->dmaResource = dmaChannelSpec->ref;
 435             bbPort->dmaChannel = dmaChannelSpec->channel;
 436 #else
 437             bbPort->dmaResource = timhw->dmaRef;
 438             bbPort->dmaChannel = timhw->dmaChannel;
 439 #endif
 440         }
 441
 442         if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
 443             bbDevice.vTable.write = motorWriteNull;
 444             bbDevice.vTable.updateStart = motorUpdateStartNull;
 445             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
 446
 447             return false;
 448         }
 449
 450         bbPort->gpio = IO_GPIO(io);
 451
 452         bbPort->portOutputCount = MOTOR_DSHOT_BUF_LENGTH;
 453         bbPort->portOutputBuffer = &bbOutputBuffer[(bbPort - bbPorts) * MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH];
 454
 455         bbPort->portInputCount = DSHOT_BB_PORT_IP_BUF_LENGTH;
 456         bbPort->portInputBuffer = &bbInputBuffer[(bbPort - bbPorts) * DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH];
 457
 458         bbTimebaseSetup(bbPort, pwmProtocolType);
 459         bbTIM_TimeBaseInit(bbPort, bbPort->outputARR);
 460         bbTimerChannelInit(bbPort);
 461
 462         bbSetupDma(bbPort);
 463         bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_OUTPUT);
 464         bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_INPUT);
 465
 466         bbDMA_ITConfig(bbPort);
 467     }
 468
 469     bbMotors[motorIndex].pinIndex = pinIndex;
 470     bbMotors[motorIndex].io = io;
 471     bbMotors[motorIndex].output = output;
 472     bbMotors[motorIndex].bbPort = bbPort;
 473
 474     IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 475
 476     // Setup GPIO_MODER and GPIO_ODR register manipulation values
 477
 478     bbGpioSetup(&bbMotors[motorIndex]);
 479
 480 #ifdef USE_DSHOT_TELEMETRY
 481     if (useDshotTelemetry) {
 482         bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_INVERTED);
 483     } else
 484 #endif
 485     {
 486         bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_NONINVERTED);
 487     }
 488
 489     bbSwitchToOutput(bbPort);
 490
 491     bbMotors[motorIndex].configured = true;
 492
 493     return true;
 494 }
 495
 496 static bool bbUpdateStart(void)
 497 {
 498 #ifdef USE_DSHOT_TELEMETRY
 499     if (useDshotTelemetry) {
 500 #ifdef USE_DSHOT_TELEMETRY_STATS
 501         const timeMs_t currentTimeMs = millis();
 502 #endif
 503         timeUs_t currentUs = micros();
 504         // don't send while telemetry frames might still be incoming
 505         if (cmpTimeUs(currentUs, lastSendUs) < (timeDelta_t)(40 + 2 * dshotFrameUs)) {
 506             return false;
 507         }
 508
 509         for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 510 #ifdef USE_DSHOT_CACHE_MGMT
 511             // Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
 512             bool invalidated = false;
 513             for (int i = 0; i < motorIndex; i++) {
 514                 if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
 515                     invalidated = true;
 516                 }
 517             }
 518             if (!invalidated) {
 519                 SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
 520                                              DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
 521             }
 522 #endif
 523
 524 #ifdef STM32F4
 525             uint32_t value = decode_bb_bitband(
 526                 bbMotors[motorIndex].bbPort->portInputBuffer,
 527                 bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
 528                 bbMotors[motorIndex].pinIndex);
 529 #else
 530             uint32_t value = decode_bb(
 531                 bbMotors[motorIndex].bbPort->portInputBuffer,
 532                 bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
 533                 bbMotors[motorIndex].pinIndex);
 534 #endif
 535             if (value == BB_NOEDGE) {
 536                 continue;
 537             }
 538             dshotTelemetryState.readCount++;
 539
 540             if (value != BB_INVALID) {
 541                 dshotTelemetryState.motorState[motorIndex].telemetryValue = value;
 542                 dshotTelemetryState.motorState[motorIndex].telemetryActive = true;
 543                 if (motorIndex < 4) {
 544                     DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, value);
 545                 }
 546             } else {
 547                 dshotTelemetryState.invalidPacketCount++;
 548             }
 549 #ifdef USE_DSHOT_TELEMETRY_STATS
 550             updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], value != BB_INVALID, currentTimeMs);
 551 #endif
 552         }
 553     }
 554 #endif
 555     for (int i = 0; i < usedMotorPorts; i++) {
 556         bbDMA_Cmd(&bbPorts[i], DISABLE);
 557         bbOutputDataClear(bbPorts[i].portOutputBuffer);
 558     }
 559
 560     return true;
 561 }
 562
 563 static void bbWriteInt(uint8_t motorIndex, uint16_t value)
 564 {
 565     bbMotor_t *const bbmotor = &bbMotors[motorIndex];
 566
 567     if (!bbmotor->configured) {
 568         return;
 569     }
 570
 571     // fetch requestTelemetry from motors. Needs to be refactored.
 572     motorDmaOutput_t * const motor = getMotorDmaOutput(motorIndex);
 573     bbmotor->protocolControl.requestTelemetry = motor->protocolControl.requestTelemetry;
 574     motor->protocolControl.requestTelemetry = false;
 575
 576     // If there is a command ready to go overwrite the value and send that instead
 577     if (dshotCommandIsProcessing()) {
 578         value = dshotCommandGetCurrent(motorIndex);
 579         if (value) {
 580             bbmotor->protocolControl.requestTelemetry = true;
 581         }
 582     }
 583
 584     bbmotor->protocolControl.value = value;
 585
 586     uint16_t packet = prepareDshotPacket(&bbmotor->protocolControl);
 587
 588     bbPort_t *bbPort = bbmotor->bbPort;
 589
 590 #ifdef USE_DSHOT_TELEMETRY
 591     if (useDshotTelemetry) {
 592         bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_INVERTED);
 593     } else
 594 #endif
 595     {
 596         bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_NONINVERTED);
 597     }
 598 }
 599
 600 static void bbWrite(uint8_t motorIndex, float value)
 601 {
 602     bbWriteInt(motorIndex, lrintf(value));
 603 }
 604
 605 static void bbUpdateComplete(void)
 606 {
 607     // If there is a dshot command loaded up, time it correctly with motor update
 608
 609     if (!dshotCommandQueueEmpty()) {
 610         if (!dshotCommandOutputIsEnabled(bbDevice.count)) {
 611             return;
 612         }
 613     }
 614
 615 #ifdef USE_DSHOT_CACHE_MGMT
 616     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 617         // Only clean each buffer once. If all motors are on a common port they'll share a buffer.
 618         bool clean = false;
 619         for (int i = 0; i < motorIndex; i++) {
 620             if (bbMotors[motorIndex].bbPort->portOutputBuffer == bbMotors[i].bbPort->portOutputBuffer) {
 621                 clean = true;
 622             }
 623         }
 624         if (!clean) {
 625             SCB_CleanDCache_by_Addr(bbMotors[motorIndex].bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
 626         }
 627     }
 628 #endif
 629
 630     for (int i = 0; i < usedMotorPorts; i++) {
 631         bbPort_t *bbPort = &bbPorts[i];
 632
 633 #ifdef USE_DSHOT_TELEMETRY
 634         if (useDshotTelemetry) {
 635             if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
 636                 bbPort->inputActive = false;
 637                 bbSwitchToOutput(bbPort);
 638             }
 639         } else
 640 #endif
 641         {
 642 #if defined(STM32G4)
 643             // Using circular mode resets the counter one short, so explicitly reload
 644             bbSwitchToOutput(bbPort);
 645 #endif
 646         }
 647
 648         bbDMA_Cmd(bbPort, ENABLE);
 649     }
 650
 651     lastSendUs = micros();
 652     for (int i = 0; i < usedMotorPacers; i++) {
 653         bbPacer_t *bbPacer = &bbPacers[i];
 654         bbTIM_DMACmd(bbPacer->tim, bbPacer->dmaSources, ENABLE);
 655     }
 656 }
 657
 658 static bool bbEnableMotors(void)
 659 {
 660     for (int i = 0; i < motorCount; i++) {
 661         if (bbMotors[i].configured) {
 662             IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg);
 663         }
 664     }
 665     return true;
 666 }
 667
 668 static void bbDisableMotors(void)
 669 {
 670     return;
 671 }
 672
 673 static void bbShutdown(void)
 674 {
 675     return;
 676 }
 677
 678 static bool bbIsMotorEnabled(uint8_t index)
 679 {
 680     return bbMotors[index].enabled;
 681 }
 682
 683 static void bbPostInit()
 684 {
 685     bbFindPacerTimer();
 686
 687     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 688
 689         if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) {
 690             return NULL;
 691         }
 692
 693
 694         bbMotors[motorIndex].enabled = true;
 695
 696         // Fill in motors structure for 4way access (XXX Should be refactored)
 697
 698         motors[motorIndex].enabled = true;
 699     }
 700 }
 701
 702 static motorVTable_t bbVTable = {
 703     .postInit = bbPostInit,
 704     .enable = bbEnableMotors,
 705     .disable = bbDisableMotors,
 706     .isMotorEnabled = bbIsMotorEnabled,
 707     .updateStart = bbUpdateStart,
 708     .write = bbWrite,
 709     .writeInt = bbWriteInt,
 710     .updateComplete = bbUpdateComplete,
 711     .convertExternalToMotor = dshotConvertFromExternal,
 712     .convertMotorToExternal = dshotConvertToExternal,
 713     .shutdown = bbShutdown,
 714 };
 715
 716 dshotBitbangStatus_e dshotBitbangGetStatus()
 717 {
 718     return bbStatus;
 719 }
 720
 721 motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count)
 722 {
 723     dbgPinLo(0);
 724     dbgPinLo(1);
 725
 726     motorPwmProtocol = motorConfig->motorPwmProtocol;
 727     bbDevice.vTable = bbVTable;
 728     motorCount = count;
 729     bbStatus = DSHOT_BITBANG_STATUS_OK;
 730
 731 #ifdef USE_DSHOT_TELEMETRY
 732     useDshotTelemetry = motorConfig->useDshotTelemetry;
 733 #endif
 734
 735     memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer));
 736
 737     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 738         const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
 739         const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]);
 740         const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
 741
 742         uint8_t output = motorConfig->motorPwmInversion ?  timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output;
 743         bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP;
 744
 745 #ifdef USE_DSHOT_TELEMETRY
 746         if (useDshotTelemetry) {
 747             output ^= TIMER_OUTPUT_INVERTED;
 748         }
 749 #endif
 750
 751         if (!IOIsFreeOrPreinit(io)) {
 752             /* not enough motors initialised for the mixer or a break in the motors */
 753             bbDevice.vTable.write = motorWriteNull;
 754             bbDevice.vTable.updateStart = motorUpdateStartNull;
 755             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
 756             bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
 757             return NULL;
 758         }
 759
 760         int pinIndex = IO_GPIOPinIdx(io);
 761
 762         bbMotors[motorIndex].pinIndex = pinIndex;
 763         bbMotors[motorIndex].io = io;
 764         bbMotors[motorIndex].output = output;
 765 #if defined(STM32F4) || defined(STM32F3)
 766         bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_50MHz, GPIO_OType_PP, bbPuPdMode);
 767 #elif defined(STM32F7) || defined(STM32G4) || defined(STM32H7)
 768         bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_MODE_OUTPUT_PP, GPIO_SPEED_FREQ_LOW, bbPuPdMode);
 769 #endif
 770
 771         IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 772         IOConfigGPIO(io, bbMotors[motorIndex].iocfg);
 773         if (output & TIMER_OUTPUT_INVERTED) {
 774             IOLo(io);
 775         } else {
 776             IOHi(io);
 777         }
 778
 779         // Fill in motors structure for 4way access (XXX Should be refactored)
 780         motors[motorIndex].io = bbMotors[motorIndex].io;
 781     }
 782
 783     return &bbDevice;
 784 }
 785
 786 #endif // USE_DSHOT_BB