diff --git a/applications/sdp/mspi/src/main.c b/applications/sdp/mspi/src/main.c index ba33a841e4c..128662886ba 100644 --- a/applications/sdp/mspi/src/main.c +++ b/applications/sdp/mspi/src/main.c @@ -77,6 +77,31 @@ static volatile uint8_t response_buffer[CONFIG_SDP_MSPI_MAX_RESPONSE_SIZE]; static struct ipc_ept ep; static atomic_t ipc_atomic_sem = ATOMIC_INIT(0); +static uint16_t cnt0_top_calculate(uint32_t freq) +{ + uint16_t dividor_high = UINT16_MAX; + uint16_t dividor_low = 1; + uint16_t middle; + + if (freq == MAX_FREQUENCY) { + return 0; + } + + /* Perform binary search for the highest frequency that is smaller than the requested one. + */ + while ((dividor_high - dividor_low) > 1) { + middle = (dividor_high + dividor_low) / 2; + + if (freq < SystemCoreClock / (2 * middle)) { + dividor_low = middle; + } else { + dividor_high = middle; + } + } + + return dividor_high - 1; +} + static void adjust_tail(volatile hrt_xfer_data_t *xfer_data, uint16_t frame_width, uint32_t data_length) { @@ -174,7 +199,7 @@ static void xfer_execute(nrfe_mspi_xfer_packet_msg_t *xfer_packet) volatile nrfe_mspi_dev_config_t *device = &nrfe_mspi_devices[nrfe_mspi_xfer_config_ptr->device_index]; - xfer_params.counter_value = 4; + xfer_params.counter_value = cnt0_top_calculate(device->freq); xfer_params.ce_vio = ce_vios[device->ce_index]; xfer_params.ce_hold = nrfe_mspi_xfer_config_ptr->hold_ce; xfer_params.cpp_mode = device->cpp; @@ -258,7 +283,7 @@ void prepare_and_read_data(nrfe_mspi_xfer_packet_msg_t *xfer_packet, volatile ui &nrfe_mspi_devices[nrfe_mspi_xfer_config_ptr->device_index]; nrf_vpr_csr_vio_config_t config; - xfer_params.counter_value = 4; + xfer_params.counter_value = cnt0_top_calculate(device->freq); xfer_params.ce_vio = ce_vios[device->ce_index]; xfer_params.ce_hold = nrfe_mspi_xfer_config_ptr->hold_ce; xfer_params.ce_polarity = device->ce_polarity;