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