From 7f0f182bb78e6f9f5775cc7f04fd2e1b6c4c6481 Mon Sep 17 00:00:00 2001 From: mikee47 Date: Wed, 28 Oct 2020 04:56:12 +0000 Subject: [PATCH] Fix Esp32 code (not yet working - just copied Esp8266 version) --- Sming/Arch/Esp32/Core/SPI.cpp | 523 +++++++++++++++------------------- 1 file changed, 226 insertions(+), 297 deletions(-) diff --git a/Sming/Arch/Esp32/Core/SPI.cpp b/Sming/Arch/Esp32/Core/SPI.cpp index 25c2e596df..f335dc0600 100644 --- a/Sming/Arch/Esp32/Core/SPI.cpp +++ b/Sming/Arch/Esp32/Core/SPI.cpp @@ -17,7 +17,6 @@ #include "SPI.h" #include - #include "espinc/eagle_soc.h" #include "espinc/spi_register.h" #include "c_types.h" @@ -25,64 +24,186 @@ // define the static singleton SPIClass SPI; -/* @defgroup SPI hardware implementation - * @brief begin() - * - * Initializes the SPI bus using the default SPISettings - * +namespace +{ +// Used internally to calculate optimum SPI speed +struct SpiPreDiv { + unsigned freq; + unsigned prescale; + unsigned divisor; +}; + +/** + * @brief Wait until HSPI has finished any current transaction */ -void SPIClass::begin() +__forceinline void spi_wait() { - WRITE_PERI_REG(PERIPHS_IO_MUX, 0x105); //clear bit9 - // - // PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U, 2); // HSPIQ MISO == GPIO12 - // PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, 2); // HSPID MOSI == GPIO13 - // PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U, 2); // CLK == GPIO14 + while(READ_PERI_REG(SPI_CMD(SPI_NO)) & SPI_USR) { + // + } +} - prepare(this->SPIDefaultSettings); +/** + * @brief Initiate an HSPI user transaction + */ +__forceinline void spi_send() +{ + SET_PERI_REG_MASK(SPI_CMD(SPI_NO), SPI_USR); } -/* @defgroup SPI hardware implementation - * @brief beginTransaction() +/** + * @brief Configure SPI mode parameters for clock edge and clock polarity. * - * Initializes the SPI bus using the defined SPISettings + * Private method used by SPISetings * - * this methode does not initiate a transaction. So it can be used to - * setup the SPI after SPI.begin() + * @param SPI_MODE0 .. SPI_MODE4 * + * Mode Clock Polarity (CPOL) Clock Phase (CPHA) + * SPI_MODE0 0 0 + * SPI_MODE1 0 1 + * SPI_MODE2 1 0 + * SPI_MODE3 1 1 */ -void SPIClass::beginTransaction(SPISettings mySettings) +void spi_mode(uint8_t mode) { + uint8_t spi_cpha = mode & 0x0F; + uint8_t spi_cpol = mode & 0xF0; + #ifdef SPI_DEBUG - debugf("SPIhw::beginTransaction(SPISettings mySettings)"); + debugf("SPIClass::spi_mode(mode %x) spi_cpha %X,spi_cpol %X)", mode, spi_cpha, spi_cpol); #endif - // check if we need to change settings - if(this->spiSettings == mySettings) { - return; + + if(spi_cpha == spi_cpol) { + CLEAR_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_CK_OUT_EDGE); + } else { + SET_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_CK_OUT_EDGE); } - // prepare SPI settings - prepare(mySettings); + if(spi_cpol) { + SET_PERI_REG_MASK(SPI_PIN(SPI_NO), SPI_IDLE_EDGE); + } else { + CLEAR_PERI_REG_MASK(SPI_PIN(SPI_NO), SPI_IDLE_EDGE); + } +} + +/** + * @brief Setup the byte order for shifting data out of buffer + * + * Private method used by SPISetings + * + * @param MSBFIRST 1 + * Data is sent out starting with Bit31 and down to Bit0 + * LSBFIRST 0 + * Data is sent out starting with the lowest BYTE, from MSB to LSB + * 0xABCDEFGH would be sent as 0xGHEFCDAB + */ +void spi_byte_order(uint8_t byte_order) +{ +#ifdef SPI_DEBUG + debugf("SPIClass::spi_byte_order(byte_order %u)", byte_order); +#endif + + if(byte_order) { + SET_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_WR_BYTE_ORDER); + SET_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_RD_BYTE_ORDER); + } else { + CLEAR_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_WR_BYTE_ORDER); + CLEAR_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_RD_BYTE_ORDER); + } } -/* @defgroup SPI hardware implementation - * @brief transfer32() +/** + * @brief set CPI bus clock * - * private method used by transfer(byte) and transfer16(sort) - * to send/recv one uint32_t + * Private method used by setFrequency * - * SPI transfer is based on a simultaneous send and receive: - * the received data is returned in receivedVal (or receivedVal16). + * time length HIGHT level = (CPU clock / 10 / 2) ^ -1, + * time length LOW level = (CPU clock / 10 / 2) ^ -1 + * Frequency calculation: 80Mhz / predivider / divider * - * receivedVal = SPI.transfer(val) : single byte - * receivedVal16 = SPI.transfer16(val16) : single short + * @param prediv time length HIGHT level + * @param prediv time length LOW level + */ +void setClock(const SPISpeed& speed) +{ + // Clock register value is never 0, so indicates it hasn't been calculated + uint32_t regVal = speed.regVal; + if(regVal == 0) { + SPISpeed tmp = speed; + SPIClass::checkSpeed(tmp); + regVal = tmp.regVal; + } else { +#ifdef SPI_DEBUG + unsigned prescale = (speed.regVal >> SPI_CLKDIV_PRE_S) + 1; + unsigned divisor = (speed.regVal >> SPI_CLKCNT_N_S) + 1; + debugf("SPIClass::setClock(prescaler %u, divisor %u) for target %u", prescale, divisor, speed.frequency); +#endif + } + + WRITE_PERI_REG(SPI_CLOCK(SPI_NO), regVal); +} + +/** + * @brief Calculate the closest prescale value for a given frequency and clock-divider + * @param cpuFreq current CPU frequency, in Hz + * @param freq target SPI bus frequency, in Hz + * @param div divisor value to use + * @retval SpiPreDiv contains resulting frequency, prescaler and divisor values */ +SpiPreDiv calculateSpeed(unsigned cpuFreq, unsigned freq, unsigned div) +{ + SpiPreDiv prediv; + unsigned pre = cpuFreq / (freq * div); + if(pre == 0) { + pre = 1; + } + unsigned n = pre * div; + while(true) { + prediv.freq = cpuFreq / n; + if(prediv.freq <= freq) { + break; + } + ++pre; + n += div; + } + prediv.prescale = pre; + prediv.divisor = div; + +#ifdef SPI_DEBUG + debugf("SPI calculateSpeed(uint freq %u, uint pre %u, uint div %u)", f, pre, div); +#endif + + return prediv; +} + +} // namespace + +void SPIClass::begin() +{ + // CLEAR_PERI_REG_MASK(PERIPHS_IO_MUX, BIT9); + // + // PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTDI_U, 2); // HSPIQ MISO == GPIO12 + // PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTCK_U, 2); // HSPID MOSI == GPIO13 + // PIN_FUNC_SELECT(PERIPHS_IO_MUX_MTMS_U, 2); // CLK == GPIO14 + + checkSpeed(SPIDefaultSettings.speed); + prepare(SPIDefaultSettings); +} + +void SPIClass::beginTransaction(const SPISettings& mySettings) +{ +#ifdef SPI_DEBUG + debugf("SPIhw::beginTransaction(SPISettings mySettings)"); +#endif + + prepare(mySettings); +} + uint32_t SPIClass::transfer32(uint32_t data, uint8_t bits) { uint32_t regvalue = READ_PERI_REG(SPI_USER(SPI_NO)) & (SPI_WR_BYTE_ORDER | SPI_RD_BYTE_ORDER | SPI_CK_OUT_EDGE); - while(READ_PERI_REG(SPI_CMD(SPI_NO)) & SPI_USR) - ; + spi_wait(); regvalue |= SPI_USR_MOSI | SPI_DOUTDIN | SPI_CK_I_EDGE; WRITE_PERI_REG(SPI_USER(SPI_NO), regvalue); @@ -97,101 +218,84 @@ uint32_t SPIClass::transfer32(uint32_t data, uint8_t bits) WRITE_PERI_REG(SPI_W0(SPI_NO), data); } - SET_PERI_REG_MASK(SPI_CMD(SPI_NO), SPI_USR); // send - - while(READ_PERI_REG(SPI_CMD(SPI_NO)) & SPI_USR) - ; + spi_send(); + spi_wait(); // wait a while before reading the register into the buffer // delayMicroseconds(2); if(READ_PERI_REG(SPI_USER(SPI_NO)) & SPI_RD_BYTE_ORDER) { - return READ_PERI_REG(SPI_W0(SPI_NO)) >> (32 - bits); //Assuming data in is written to MSB. TBC + // Assuming data in is written to MSB. TBC + return READ_PERI_REG(SPI_W0(SPI_NO)) >> (32 - bits); } else { - return READ_PERI_REG(SPI_W0( - SPI_NO)); //Read in the same way as DOUT is sent. Note existing contents of SPI_W0 remain unless overwritten! + // Read in the same way as DOUT is sent. Note existing contents of SPI_W0 remain unless overwritten! + return READ_PERI_REG(SPI_W0(SPI_NO)); } } -/* - * used for performance tuning when doing continuous reads - * this method does not reset the registers , so make sure - * that a regular transfer(data) call was performed - */ uint8_t SPIClass::read8() { - while(READ_PERI_REG(SPI_CMD(SPI_NO)) & SPI_USR) - ; + spi_wait(); WRITE_PERI_REG(SPI_W0(SPI_NO), 0x00); - SET_PERI_REG_MASK(SPI_CMD(SPI_NO), SPI_USR); // send - - while(READ_PERI_REG(SPI_CMD(SPI_NO)) & SPI_USR) - ; + spi_send(); + spi_wait(); if(READ_PERI_REG(SPI_USER(SPI_NO)) & SPI_RD_BYTE_ORDER) { - return READ_PERI_REG(SPI_W0(SPI_NO)) >> (32 - 8); //Assuming data in is written to MSB. TBC + // Assuming data in is written to MSB. TBC + return READ_PERI_REG(SPI_W0(SPI_NO)) >> (32 - 8); } else { - return READ_PERI_REG(SPI_W0( - SPI_NO)); //Read in the same way as DOUT is sent. Note existing contents of SPI_W0 remain unless overwritten! + // Read in the same way as DOUT is sent. Note existing contents of SPI_W0 remain unless overwritten! + return READ_PERI_REG(SPI_W0(SPI_NO)); } } -/* @defgroup SPI hardware implementation - * @brief transfer(uint8_t *buffer, size_t numberBytes) - * - * SPI transfer is based on a simultaneous send and receive: - * The buffered transfers does split up the conversation internaly into 64 byte blocks. - * The received data is stored in the buffer passed by reference. - * (the data past in is replaced with the data received). - * - * SPI.transfer(buffer, size) : memory buffer of length size - */ void SPIClass::transfer(uint8_t* buffer, size_t numberBytes) { -#define BLOCKSIZE 64 // the max length of the ESP SPI_W0 registers +#define BLOCKSIZE 64U // the max length of the ESP SPI_W0 registers - uint16 bufIndx = 0; + unsigned bufIndx = 0; - int blocks = ((numberBytes - 1) / BLOCKSIZE) + 1; + unsigned blocks = ((numberBytes - 1) / BLOCKSIZE) + 1; #ifdef SPI_DEBUG - int total = blocks; + unsigned total = blocks; #endif // loop number of blocks while(blocks--) { // get full BLOCKSIZE or number of remaining bytes - uint8_t bufLength = std::min(numberBytes - bufIndx, (unsigned int)BLOCKSIZE); + auto bufLength = std::min(numberBytes - bufIndx, BLOCKSIZE); #ifdef SPI_DEBUG - debugf("Write/Read Block %d total %d bytes", total - blocks, bufLength); + debugf("Write/Read Block %u total %u bytes", total - blocks, bufLength); #endif // compute the number of bits to clock - uint8_t num_bits = bufLength * 8; + auto num_bits = bufLength * 8; uint32_t regvalue = READ_PERI_REG(SPI_USER(SPI_NO)) & (SPI_WR_BYTE_ORDER | SPI_RD_BYTE_ORDER | SPI_CK_OUT_EDGE); - while(READ_PERI_REG(SPI_CMD(SPI_NO)) & SPI_USR) - ; + spi_wait(); regvalue |= SPI_USR_MOSI | SPI_DOUTDIN | SPI_CK_I_EDGE; WRITE_PERI_REG(SPI_USER(SPI_NO), regvalue); - // setup bit lenght + // setup bit length WRITE_PERI_REG(SPI_USER1(SPI_NO), (((num_bits - 1) & SPI_USR_MOSI_BITLEN) << SPI_USR_MOSI_BITLEN_S) | (((num_bits - 1) & SPI_USR_MISO_BITLEN) << SPI_USR_MISO_BITLEN_S)); // copy the registers starting from last index position - memcpy((void*)SPI_W0(SPI_NO), &buffer[bufIndx], bufLength); - - // Begin SPI Transaction - SET_PERI_REG_MASK(SPI_CMD(SPI_NO), SPI_USR); + if(IS_ALIGNED(buffer)) { + memcpy((void*)SPI_W0(SPI_NO), &buffer[bufIndx], ALIGNUP4(bufLength)); + } else { + uint32_t wordBuffer[BLOCKSIZE / 4]; + memcpy(wordBuffer, &buffer[bufIndx], bufLength); + memcpy((void*)SPI_W0(SPI_NO), wordBuffer, ALIGNUP4(bufLength)); + } - // wait for SPI bus to be ready - while(READ_PERI_REG(SPI_CMD(SPI_NO)) & SPI_USR) - ; + spi_send(); + spi_wait(); // wait a while before reading the register into the buffer // delayMicroseconds(8); @@ -204,245 +308,70 @@ void SPIClass::transfer(uint8_t* buffer, size_t numberBytes) } }; -/** @defgroup SPI hardware implementation - * @brief prepare apply SPI bus settings - * - * Private method used by beginTransaction and begin (init) - * - * @param SPISettings include frequency, byte order and SPI mode - */ -void SPIClass::prepare(SPISettings mySettings) +void SPIClass::prepare(const SPISettings& settings) { #ifdef SPI_DEBUG - debugf("SPIClass::prepare(SPISettings mySettings)"); - mySettings.print("mySettings"); + debugf("SPIClass::prepare(SPISettings)"); + settings.print("settings"); #endif - // check if we need to change settings - if(initialised && spiSettings == mySettings) - return; - // setup clock - setFrequency(mySettings.speed); + setClock(settings.speed); // set byte order - this->spi_byte_order(mySettings.byteOrder); + spi_byte_order(settings.byteOrder); // set spi mode - spi_mode(mySettings.dataMode); - -#ifdef SPI_DEBUG - debugf("SPIhw::prepare(SPISettings mySettings) -> updated settings"); -#endif - - spiSettings = mySettings; - initialised = true; + spi_mode(settings.dataMode); }; -/** @brief spi_mode Configures SPI mode parameters for clock edge and clock polarity. - * - * Private method used by SPISetings - * - * @param SPI_MODE0 .. SPI_MODE4 - * - * Mode Clock Polarity (CPOL) Clock Phase (CPHA) - * SPI_MODE0 0 0 - * SPI_MODE1 0 1 - * SPI_MODE2 1 0 - * SPI_MODE3 1 1 - */ -void SPIClass::spi_mode(uint8_t mode) -{ - uint8_t spi_cpha = mode & 0x0F; - uint8_t spi_cpol = mode & 0xF0; - -#ifdef SPI_DEBUG - debugf("SPIClass::spi_mode(mode %x) spi_cpha %X,spi_cpol %X)", mode, spi_cpha, spi_cpol); -#endif - - if(spi_cpha == spi_cpol) { - CLEAR_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_CK_OUT_EDGE); - } else { - SET_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_CK_OUT_EDGE); - } - - if(spi_cpol) { - SET_PERI_REG_MASK(SPI_PIN(SPI_NO), SPI_IDLE_EDGE); - } else { - CLEAR_PERI_REG_MASK(SPI_PIN(SPI_NO), SPI_IDLE_EDGE); - } -} - -/** @brief spi_byte_order Setup the byte order for shifting data out of buffer - * - * Private method used by SPISetings - * - * @param MSBFIRST 1 - * Data is sent out starting with Bit31 and down to Bit0 - * LSBFIRST 0 - * Data is sent out starting with the lowest BYTE, from MSB to LSB - * 0xABCDEFGH would be sent as 0xGHEFCDAB - */ -void SPIClass::spi_byte_order(uint8_t byte_order) -{ -#ifdef SPI_DEBUG - debugf("SPIClass::spi_byte_order(byte_order %d)", byte_order); -#endif - - if(byte_order) { - SET_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_WR_BYTE_ORDER); - SET_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_RD_BYTE_ORDER); - } else { - CLEAR_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_WR_BYTE_ORDER); - CLEAR_PERI_REG_MASK(SPI_USER(SPI_NO), SPI_RD_BYTE_ORDER); - } -} - -/** @brief setClock set CPI bus clock - * - * Private method used by setFrequency - * - * time length HIGHT level = (CPU clock / 10 / 2) ^ -1, - * time length LOW level = (CPU clock / 10 / 2) ^ -1 - * Frequency calculation: 80Mhz / predivider / divider - * - * @param prediv time length HIGHT level - * @param prediv time length LOW level - * - */ -void SPIClass::setClock(uint8_t prediv, uint8_t cntdiv) -{ -#ifdef SPI_DEBUG - debugf("SPIClass::setClock(prediv %d, cntdiv %d) for target %d", prediv, cntdiv, spiSettings.speed); -#endif - debugf("SPIClass::setClock(prediv %d, cntdiv %d) for target %d", prediv, cntdiv, spiSettings.speed); - - if((prediv == 0) | (cntdiv == 0)) { - // go full speed = SYSTEMCLOCK - WRITE_PERI_REG(SPI_CLOCK(SPI_NO), SPI_CLK_EQU_SYSCLK); - } else { - WRITE_PERI_REG(SPI_CLOCK(SPI_NO), (((prediv - 1) & SPI_CLKDIV_PRE) << SPI_CLKDIV_PRE_S) | - (((cntdiv - 1) & SPI_CLKCNT_N) << SPI_CLKCNT_N_S) | - (((cntdiv >> 1) & SPI_CLKCNT_H) << SPI_CLKCNT_H_S) | - ((0 & SPI_CLKCNT_L) << SPI_CLKCNT_L_S)); - } -} - -/** @brief gnu lib div implementation - * TODO: check whether there is a implementation in SMING already - */ -div_t div(int numer, int denom) -{ - div_t result; - result.quot = numer / denom; - result.rem = numer % denom; - if(numer >= 0 && result.rem < 0) { - ++result.quot; - result.rem -= denom; - } - return result; -} - -/** @brief getFrequency computes the closest pre devider for a given clock-devider and frequency - * - * Private method used by setFrequency - * - * @param freq target SPI bus frequency - * @param &pre return the computed pre devider (pass by reference)reqHelp Help message shown by CLI "help" command - * @param clk clock devider - */ -uint32_t SPIClass::getFrequency(int freq, int& pre, int clk) -{ - int _CPU_freq = system_get_cpu_freq() * 1000000UL; -#ifdef SPI_DEBUG - debugf("SPIClass::getFrequency -> current cpu frequency %d", _CPU_freq); -#endif - - int divider = _CPU_freq / freq; - - div_t divresult = div(divider, clk); - pre = divresult.quot; - - int f = _CPU_freq / pre / clk; - while(f > freq) { - pre++; - f = _CPU_freq / pre / clk; - } -#ifdef SPI_DEBUG - debugf("SPIClass::getFrequency(int freq %d, int &pre %d, int clk %d)", f, pre, clk); -#endif - return f; -} - -/** @brief set the max SPI bus frequency - * - * Private method used by SPISetings - * - * The algorithm is testing with colock deviders 2,3 and 5 to find the best pre-devider +/** @brief Check speed settings and perform any pre-calculation required + * @param speed IN: requested bus frequency, OUT: Modified settings with prescale values + * @note + * The algorithm is testing with clock dividers 2,3 and 5 to find the best pre-divider * The resulting clock frequency is not 100% accurate but delivers result within 5% * * It is guaranteed that the frequency will not exceed the given target * * Make sure that the ESP clock frequency is set before initializing the SPI bus. * Changes on the ESP clock are not recognised once initialized - * - * @param freq Max SPI bus frequency - * @{ */ -void SPIClass::setFrequency(int freq) +void SPIClass::checkSpeed(SPISpeed& speed) { + unsigned cpuFreq = system_get_cpu_freq() * 1000000UL; #ifdef SPI_DEBUG - debugf("SPIClass::setFrequency(uint32_t %d)", freq); + debugf("SPIClass::calculateSpeed() -> current cpu frequency %u", cpuFreq); #endif - int _CPU_freq = system_get_cpu_freq() * 10000000UL; + SpiPreDiv prediv; - // dont run code if there are no changes - if(initialised && freq == spiSettings.speed) - return; + // If we're not running at max then need to determine appropriate prescale values + if(speed.frequency >= cpuFreq) { + // Use maximum speed + prediv.freq = cpuFreq; + prediv.divisor = 0; + speed.regVal = SPI_CLK_EQU_SYSCLK; + } else { + prediv = calculateSpeed(cpuFreq, speed.frequency, 2); + if(prediv.freq != speed.frequency) { + // Use whichever divisor gives the highest frequency + SpiPreDiv pd3 = calculateSpeed(cpuFreq, speed.frequency, 3); + SpiPreDiv pd5 = calculateSpeed(cpuFreq, speed.frequency, 5); + if(pd3.freq > prediv.freq || pd5.freq > prediv.freq) { + prediv = (pd3.freq > pd5.freq) ? pd3 : pd5; + } + } - if(freq == _CPU_freq) { - spiSettings.speed = freq; - setClock(0, 0); - return; + // We have prescale and divisor values, now get regVal so we don't need to do this every time prepare() is called + speed.regVal = (((prediv.prescale - 1) & SPI_CLKDIV_PRE) << SPI_CLKDIV_PRE_S) | + (((prediv.divisor - 1) & SPI_CLKCNT_N) << SPI_CLKCNT_N_S) | + (((prediv.divisor >> 1) & SPI_CLKCNT_H) << SPI_CLKCNT_H_S) | + ((0 & SPI_CLKCNT_L) << SPI_CLKCNT_L_S); } - freq = std::min(freq, _CPU_freq / 2); - spiSettings.speed = freq; + //#ifdef SPI_DEBUG + debug_e("-> Using clock divider %u -> target freq %u -> result %u", prediv.divisor, speed.frequency, prediv.freq); + //#endif - int pre2; - int f2 = getFrequency(freq, pre2, 2); - if(f2 == freq) { -#ifdef SPI_DEBUG - debugf("-> Hit!! -> target freq %d -> result %d", freq, _CPU_freq / pre2 / 2); -#endif - setClock(pre2, 2); - return; - } - - int pre3; - int f3 = getFrequency(freq, pre3, 3); - int pre5; - int f5 = getFrequency(freq, pre5, 5); - if(f3 <= f2 && f2 >= f5) { -#ifdef SPI_DEBUG - debugf("-> Using clock divider 2 -> target freq %d -> result %d", freq, _CPU_freq / pre2 / 2); -#endif - setClock(pre2, 2); - return; - } else { - if(f5 <= f3) { -#ifdef SPI_DEBUG - debugf("-> Using clock divider 3 -> target freq %d -> result %d", freq, _CPU_freq / pre3 / 3); -#endif - setClock(pre3, 3); - return; - } else { -#ifdef SPI_DEBUG - debugf("-> Using clock divider 5 -> target freq %d -> result %d", freq, _CPU_freq / pre5 / 5); -#endif - setClock(pre5, 5); - return; - } - } + speed.frequency = prediv.freq; }