diff --git a/mcux/middleware/wifi_nxp/CMakeLists.txt b/mcux/middleware/wifi_nxp/CMakeLists.txt index f14859a45c..68b871eb7f 100644 --- a/mcux/middleware/wifi_nxp/CMakeLists.txt +++ b/mcux/middleware/wifi_nxp/CMakeLists.txt @@ -12,6 +12,22 @@ else() set(MCUX_SDK_DIR ${ZEPHYR_HAL_NXP_MODULE_DIR}/mcux/mcux-sdk-ng) endif() +if(CONFIG_NXP_WIFI_CSI_AMI) +add_library(libcsi STATIC IMPORTED) +if(CONFIG_NXP_LIBCSI_CM33) + set_target_properties(libcsi PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/libcsi/zephyr/cm33/libcsi.a) +elseif(CONFIG_NXP_LIBCSI_CM7_20M) + set_target_properties(libcsi PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/libcsi/zephyr/cm7/cm7f_sta_20_only/libcsi.a) +else() + set_target_properties(libcsi PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/libcsi/zephyr/cm7/cm7f/libcsi.a) +endif() +target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/libcsi/incl) +zephyr_include_directories( + libcsi/incl +) +target_link_libraries(app PRIVATE libcsi) +endif() + zephyr_compile_definitions_ifdef(CONFIG_NXP_88W8987 SD8987 ) diff --git a/mcux/middleware/wifi_nxp/incl/nxp_wifi.h b/mcux/middleware/wifi_nxp/incl/nxp_wifi.h index fab1215c2c..2b693f16c6 100644 --- a/mcux/middleware/wifi_nxp/incl/nxp_wifi.h +++ b/mcux/middleware/wifi_nxp/incl/nxp_wifi.h @@ -118,6 +118,10 @@ extern "C" { #define CONFIG_CSI 1 #endif +#if CONFIG_NXP_WIFI_CSI_AMI +#define CONFIG_CSI_AMI 1 +#endif + #if CONFIG_NXP_WIFI_RESET #define CONFIG_WIFI_RESET 1 #endif diff --git a/mcux/middleware/wifi_nxp/incl/wifidriver/wifi_events.h b/mcux/middleware/wifi_nxp/incl/wifidriver/wifi_events.h index a14f5ee73a..2f6139f8c5 100644 --- a/mcux/middleware/wifi_nxp/incl/wifidriver/wifi_events.h +++ b/mcux/middleware/wifi_nxp/incl/wifidriver/wifi_events.h @@ -170,6 +170,9 @@ enum wifi_event /* Recv csi data */ WIFI_EVENT_CSI, WIFI_EVENT_CSI_STATUS, +#if CONFIG_CSI_AMI + WIFI_EVENT_CSI_PROC, +#endif #endif #if (CONFIG_11MC) || (CONFIG_11AZ) /* Event to trigger or stop ftm*/ diff --git a/mcux/middleware/wifi_nxp/incl/wlcmgr/wlan.h b/mcux/middleware/wifi_nxp/incl/wlcmgr/wlan.h index 785a266dac..6b1043a0d5 100644 --- a/mcux/middleware/wifi_nxp/incl/wlcmgr/wlan.h +++ b/mcux/middleware/wifi_nxp/incl/wlcmgr/wlan.h @@ -519,6 +519,22 @@ enum wlan_csi_opt CSI_FILTER_OPT_CLEAR, CSI_FILTER_OPT_DUMP, }; + +#if CONFIG_CSI_AMI +typedef struct _wlan_csi_proc_cfg +{ + /** peer mac address */ + uint8_t peer_mac[MLAN_MAC_ADDR_LENGTH]; + /** Number of CSI to process */ + uint8_t num_csi; + /** CSI bandwidth: 20/40/80 */ + uint8_t packet_bandwidth; + /** CSI format: legacy/HT/VHT/HE */ + uint8_t packet_format; + /** Reference Update */ + uint8_t reference_update; +} wlan_csi_proc_cfg; +#endif #endif #if CONFIG_NET_MONITOR @@ -6777,6 +6793,23 @@ int wlan_register_csi_user_callback(int (*csi_data_recv_callback)(void *buffer, * \return WM_SUCCESS if successful */ int wlan_unregister_csi_user_callback(void); + + +#if CONFIG_CSI_AMI +/** This function set Ambient Motion Index configuration. + * + * \param[in] cfg: Ambient Motion Index configuration.. + */ +void wlan_set_ami_cfg(wlan_csi_proc_cfg *cfg); + +/** Use this API to start or stop caculate Ambient Motion Index. + * + * \param[in] start: start/stop + * 1: start + * 0: stop + */ +void wlan_start_stop_ami(uint8_t start); +#endif #endif #if (CONFIG_11K) || (CONFIG_11V) || (CONFIG_11R) || (CONFIG_ROAMING) diff --git a/mcux/middleware/wifi_nxp/libcsi/incl/event.h b/mcux/middleware/wifi_nxp/libcsi/incl/event.h new file mode 100644 index 0000000000..36724a0200 --- /dev/null +++ b/mcux/middleware/wifi_nxp/libcsi/incl/event.h @@ -0,0 +1,58 @@ +/* + * Copyright 2025 NXP + * + * SPDX-License-Identifier: BSD-3-Clause + * + */ + +/*! \file event.h + * \brief This file provides essential macro definitions and data structures for processing CSI event data. + */ + +#ifndef _LIB_CSI_EVENT_H_ +#define _LIB_CSI_EVENT_H_ + +#include +#include +#include "wls_structure_defs.h" + +#define PI_ALPHA_FACTOR 0.1f +#define KALMAN_N0 0.2f +#define KALMAN_P0 0.5f +#define KALMAN_ALPHA 0.005f + +typedef enum { + AMI_FILTER_NOT_SET = 0, // Filter not set + AMI_FILTER_SET, // Filter manually set via configuration + AMI_FILTER_AUTO_SET, // Filter auto set from the first packet +} ami_filter_set_status_t; + +typedef enum { + AMI_REF_UNINIT = 0, // AMI reference not initialized + AMI_REF_INITIALIZED // AMI reference initialized +} ami_reference_status_t; + +typedef enum { + AMI_STOP = 0, // AMI calculation stopped + AMI_START // AMI calculation started +} ami_start_status_t; + +/** Structure for CSI config data*/ +typedef struct wls_csi_cfg + { + /** Channel number for FTM session*/ + t_u8 channel; + /** Indicate whether CSI filter has been set */ + ami_filter_set_status_t csiFilterSet; + /** Indicate whether start to caculate Ambient Motion Index. + * AMI_STOP - stop. AMI_START - start */ + ami_start_status_t start; + /** Indicates whether AMI reference has been initialized */ + ami_reference_status_t ami_reference_init; + /**CSI processing config*/ + hal_wls_processing_input_params_t wls_processing_input; + /**CSI filter parameters*/ + csi_filter_param_t gcsi_filter_param; + } wls_csi_cfg_t, ami_cfg_t; + +#endif /* _LIB_CSI_EVENT_H */ \ No newline at end of file diff --git a/mcux/middleware/wifi_nxp/libcsi/incl/range_kalman.h b/mcux/middleware/wifi_nxp/libcsi/incl/range_kalman.h new file mode 100644 index 0000000000..3fbfecfafd --- /dev/null +++ b/mcux/middleware/wifi_nxp/libcsi/incl/range_kalman.h @@ -0,0 +1,55 @@ +/* + * Copyright 2025 NXP + * + * SPDX-License-Identifier: BSD-3-Clause + * + */ + +/*! \file range_kalman.h + * \brief This file contains Kalman filter for WLS range measurements. + */ + +#ifndef RANGE_KALMAN +#define RANGE_KALMAN + +#define RANGE_RUN_FLT +#ifdef RANGE_RUN_FLT +// range is in meters, range_rate in meters/second +// time is in seconds +typedef struct { + // input + unsigned long long time; + float range_measurement; + // state + float last_range; // also output <-- + float last_range_rate; + float R0_11, R0_22, R0_12; + unsigned long long last_time; + // model parameters + float measurement_var; + float drive_var; +} range_kalman_state; + +void range_kalman_init(range_kalman_state* in, float range, unsigned long long time, float range_drive_var, float range_measurement_var, float range_rate_init); +#else +// range format u16.8 in meters +// time format is u64.0 in milliseconds +typedef struct { + // input + unsigned long long time; + unsigned int range_measurement; + // state + unsigned short last_range; // also output <-- + signed short last_range_rate; + unsigned long long last_time; + unsigned short R0_11, R0_22; + signed int R0_12; + // model parameters + unsigned int measurement_var; + unsigned int drive_var; +} range_kalman_state; +#endif + +int range_kalman(range_kalman_state* in); + +#endif diff --git a/mcux/middleware/wifi_nxp/libcsi/incl/wls_api.h b/mcux/middleware/wifi_nxp/libcsi/incl/wls_api.h new file mode 100644 index 0000000000..28365b37ab --- /dev/null +++ b/mcux/middleware/wifi_nxp/libcsi/incl/wls_api.h @@ -0,0 +1,30 @@ +/* + * Copyright 2025 NXP + * + * SPDX-License-Identifier: BSD-3-Clause + * + */ + +/*! \file wls_api.h + * \brief This file contains header file file for WLS processing fixed-point version API. + */ + +/************************************************************************ +* Header file for WLS processing fixed-point version API +************************************************************************/ +#ifndef WLS_API_H +#define WLS_API_H + +#include "wls_structure_defs.h" +#include "event.h" +#ifdef DFW_CSI_PROC +#include "dsp_cmd.h" +#endif + +int wls_process_csi(unsigned int *bufferMemory, unsigned int *fftInBuffer, hal_wls_packet_params_t *packetparams, hal_wls_processing_input_params_t *inputVals, unsigned int *resArray); +int wls_unpack_csi(unsigned int *bufferMemory, unsigned int *outBuffer, hal_wls_packet_params_t *packetparams, hal_wls_processing_input_params_t *inputVals, unsigned int *totalpower, unsigned int *pktInfoPtr); +int wls_calculate_toa(unsigned int *pktInfoPtr, int bufferSpacing, unsigned int *fftInBuffer, unsigned int *fftBuffer, unsigned int *totalpower, hal_wls_packet_params_t *packetparams, hal_wls_processing_input_params_t *inputVals); + +void wls_intialize_reference(unsigned int *headerBuffer, unsigned int *pktInfoPtr, csi_filter_param_t *csi_filter_param_ptr, int bufferSpacing, unsigned int *fftInBuffer, float *fftRefBuffer); +float wls_update_cross_corr_ami_calc(unsigned int *headerBuffer, unsigned int *pktInfoPtr, csi_filter_param_t *csi_filter_param_ptr, int bufferSpacing, unsigned int *fftInBuffer, float *fftRefBuffer, unsigned int *tempBuffer); +#endif diff --git a/mcux/middleware/wifi_nxp/wls/wls_param_defines.h b/mcux/middleware/wifi_nxp/libcsi/incl/wls_param_defines.h similarity index 85% rename from mcux/middleware/wifi_nxp/wls/wls_param_defines.h rename to mcux/middleware/wifi_nxp/libcsi/incl/wls_param_defines.h index d558005249..27a7644b52 100644 --- a/mcux/middleware/wifi_nxp/wls/wls_param_defines.h +++ b/mcux/middleware/wifi_nxp/libcsi/incl/wls_param_defines.h @@ -1,175 +1,186 @@ -#if CONFIG_WLS_CSI_PROC -/** @file wls_param_defines.h - * - * @brief This file contains global header file for configuring fft processing of CSI. - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * DFW Global header file for configuring fft processing of CSI. - ************************************************************************/ - -#ifndef WLS_PARAM_DEFINES_H -#define WLS_PARAM_DEFINES_H - -#define _BIN 0 -#define _TXT 1 -#define _TTL 2 - -#define INPUT_FILE _TXT -// #define SMAC_BFINFO - -#ifdef SMAC_BFINFO -#define CSI_SIGNATURE 0x0 -#define HEADER_LEN 0x15 -#define CSI_BUFFER_SIZE_DW (1024 * 2 + 64) -#define CSI_NUM_BUFFER 32 -#else -#define CSI_SIGNATURE 0xabcd0000 -#define HEADER_LEN 28 -#define CSI_BUFFER_SIZE_DW (512 + 64) // 1536 // 512 // -#define CSI_NUM_BUFFER 32 // 16 // -#endif - -#define ENABLE_SUBSPACE_FTIMING -// #define ENABLE_AOA -// #define FFT_PARALLEL -// #define FFT_INPLACE -// #define TDDE_FIRSTPATH -#define STA_20_ONLY - -#define MAX_RX 1 -#define MAX_TX 4 - -#if defined(FFT_PARALLEL) -#define NUM_PARALLEL 4 -#else -#define NUM_PARALLEL 1 -#endif - -#if defined(ENABLE_SUBSPACE_FTIMING) && defined(STA_20_ONLY) -#define NUM_PROC_BUF (11 + NUM_PARALLEL) -#elif defined(ENABLE_SUBSPACE_FTIMING) -#define NUM_PROC_BUF (2 + NUM_PARALLEL) -#else -#define NUM_PROC_BUF (NUM_PARALLEL) -#endif - -#define DUMP_MULTP_PEAKS 3 - -#define _SC4B0_1760_20MHz 0 -#define _SC4B0_0026_20MHz 1 -#define _SC4B0_F69C_20MHz 2 -#define _SC4B0_F798_20MHz 3 -#define _SC4B0_0354_20MHz 4 -#define _SC4B0_12C7_20MHz 5 -#define _SC4B0_127D_20MHz 6 -#define _SC4B0_0929_20MHz 7 -#define _SC4B0_0F0B_20MHz 8 - -#define _USE_DEFAULT_CAL 0xFF - -#define RX_A 0 -#define RX_B 1 -#define RX_C 2 -#define RX_D 3 - -#define BAND_2P4GHZ 0 -#define BAND_5GHZ 1 - -#define REF_ANTENNA RX_D - -#define ANTENNA_SPACING_MM 27 -#define DEFAULT_CHANNEL 36 -#define DEFAULT_FREQ_BAND BAND_5GHZ // needs to match DEFAULT_CHANNEL -#define DEFAULT_TSF 0 -#define DEFAULT_FTM_INIT 0 -#define DEFAULT_FTM_FORMAT 0xD // vht80 -#define ENABLE_DELAY_PEAKS 1 - -#define DEFAULT_CAL_LOW_A 0x3de // 0x3d1 -#define DEFAULT_CAL_LOW_B 0x23d // 0x222 -#define DEFAULT_CAL_LOW_C 0x37f // 0x399 - -#define DEFAULT_CAL_HIGH_A 0x386 // 0x37f -#define DEFAULT_CAL_HIGH_B 0x1ff // 0x1e6 -#define DEFAULT_CAL_HIGH_C 0x362 // 0x37c - -#define SCREEN_OUT -#ifdef SCREEN_OUT -// #define SMAC_LOG printf("\n");printf -#define HAL_LOG(...) printf(__VA_ARGS__); // printf("\n") -#else -#define HAL_LOG(...) -#endif - -#define TOA_MIN_DEFAULT 0 -#define SUBSPACE_DEFAULT 1 -#define AOA_DEFAULT 0 - -#ifndef ARM -// #define FLOATING_POINT -#endif - -// #define DEBUG_OUT_FD - -#define MAX_TSF_DIFF_POS 10 -#define MAX_TSF_DIFF_NEG -120 - -#define CAL_DATA _USE_DEFAULT_CAL - -#define IFFT_OSF_SHIFT 2 -// #define IFFT_OSF (1< -#if (CONFIG_11MC) || (CONFIG_11AZ) -#if CONFIG_WLS_CSI_PROC -#include -#include -#include -#include +#if (CONFIG_11MC) || (CONFIG_11AZ) || (CONFIG_CSI) +#if (CONFIG_WLS_CSI_PROC) || (CONFIG_CSI_AMI) +#include "wls_param_defines.h" +#include "wls_api.h" +#include "wls_structure_defs.h" +#include "range_kalman.h" +#include "event.h" #endif #endif @@ -47,8 +48,10 @@ static const char driver_version[] = "702.1.0"; static unsigned int mgmt_ie_index_bitmap = 0x0000000F; +#if (CONFIG_11MC) || (CONFIG_11AZ) || (CONFIG_CSI) #if (CONFIG_11MC) || (CONFIG_11AZ) ftm_start_param ftm_param; +#endif #if CONFIG_WLS_CSI_PROC #define NL_MAX_PAYLOAD (3 * 1024) unsigned int csi_res_array[8]; @@ -58,11 +61,38 @@ range_kalman_state range_input_str = {0}; #define RANGE_MEASUREMENT_VAR 4e-2f // in meter^2 #define RANGE_RATE_INIT 1e-3f // in (meter/s)^2 #define CSI_TSF_LEN 6 * sizeof(uint32_t) +#endif +#if (CONFIG_WLS_CSI_PROC) || CONFIG_CSI_AMI #define FFT_INBUFFER_LEN_DW (MAX_RX * MAX_TX + NUM_PROC_BUF) * (MAX_IFFT_SIZE_CSI) +#endif +#if CONFIG_WLS_CSI_PROC uint32_t fftInBuffer_t[FFT_INBUFFER_LEN_DW]; #endif #endif +#if (CONFIG_CSI) && ((CONFIG_CSI_AMI) || (CONFIG_WLS_CSI_PROC)) +#if defined __ICCARM__ +#define NO_INIT_AMI __no_init +#else +#define NO_INIT_AMI +#endif +extern ami_cfg_t g_ami_cfg; +NO_INIT_AMI float referenceBuffer[2 * (MAX_RX * MAX_TX) * MAX_IFFT_SIZE_CSI]; +NO_INIT_AMI unsigned int fftInBuffer[FFT_INBUFFER_LEN_DW]; +#if STA_20_ONLY +#define SCR_INBUFFER_LEN (3 * MAX_RX * MAX_TX + NUM_PROC_BUF) * (MAX_IFFT_SIZE_CSI) +#else +#define SCR_INBUFFER_LEN FFT_INBUFFER_LEN_DW +#endif +NO_INIT_AMI unsigned int scratchBuffer1[SCR_INBUFFER_LEN]; +#define LEG_RATE 0 +#define HT_RATE 1 +#define VHT_RATE 2 +#define HE_RATE 3 +t_u8 convertPktInfo[8] = {LEG_RATE, HT_RATE, HT_RATE, VHT_RATE, HE_RATE, LEG_RATE, LEG_RATE, LEG_RATE}; +#define AMI_CSI_RAW_DATA_OFFSET 8 /* interface header size + event type size */ +#endif + /* This were static functions in mlan file */ mlan_status wlan_cmd_802_11_deauthenticate(IN pmlan_private pmpriv, IN HostCmd_DS_COMMAND *cmd, IN t_void *pdata_buf); mlan_status wlan_cmd_reg_access(IN HostCmd_DS_COMMAND *cmd, IN t_u16 cmd_action, IN t_void *pdata_buf); @@ -6350,6 +6380,255 @@ int wifi_csi_cfg(wifi_csi_config_params_t *csi_params) return wifi_wait_for_cmdresp(NULL); } + +#if CONFIG_CSI_AMI +static void set_csi_proc_filter(unsigned int *headerBuffer, hal_pktinfo_t *pktinfo, csi_filter_param_t *csi_filter_param_ptr) +{ + hal_csirxinfo_t *csirxinfo = (hal_csirxinfo_t*)headerBuffer; + t_u16 addr2_lo = csirxinfo->addr2_lo; + t_u32 addr2_hi = csirxinfo->addr2_hi; + + // set sig format and BW + csi_filter_param_ptr->packet_bandwidth = pktinfo->sigBw; + csi_filter_param_ptr->num_rx = pktinfo->nRx; + csi_filter_param_ptr->num_tx = pktinfo->nTx; + csi_filter_param_ptr->packet_format = convertPktInfo[pktinfo->packetType]; + // set MAC address + (void)memcpy(&(csi_filter_param_ptr->peer_mac[0]), &addr2_lo, sizeof(t_u16)); + (void)memcpy(&(csi_filter_param_ptr->peer_mac[2]), &addr2_hi, sizeof(t_u32)); + + (void)PRINTF("Compare CSI filter set MAC: %02x.%02x.%02x.%02x.%02x.%02x, sig BW/format %d|%d\n", + csi_filter_param_ptr->peer_mac[0], csi_filter_param_ptr->peer_mac[1], + csi_filter_param_ptr->peer_mac[2], csi_filter_param_ptr->peer_mac[3], + csi_filter_param_ptr->peer_mac[4], csi_filter_param_ptr->peer_mac[5], + csi_filter_param_ptr->packet_bandwidth, csi_filter_param_ptr->packet_format); +} + +static int check_csi_filter_partial(unsigned int *headerBuffer, hal_pktinfo_t *pktinfo, csi_filter_param_t *csi_filter_param_ptr) +{ + hal_csirxinfo_t *csirxinfo = (hal_csirxinfo_t*)headerBuffer; + t_u16 addr2_lo = csirxinfo->addr2_lo; + t_u32 addr2_hi = csirxinfo->addr2_hi; + t_u8 broadcast_mac[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; + + // check sig format and BW + if ((csi_filter_param_ptr->packet_bandwidth != 0xff) + && (csi_filter_param_ptr->packet_bandwidth != pktinfo->sigBw)) + { + return -WM_FAIL; + } + + if ((csi_filter_param_ptr->packet_format != 0xff) + && (csi_filter_param_ptr->packet_format != convertPktInfo[pktinfo->packetType])) + { + return -WM_FAIL; + } + + // check MIMO dimension + if ((csi_filter_param_ptr->num_rx != 0xff) && (csi_filter_param_ptr->num_rx != pktinfo->nRx)) + { + return -WM_FAIL; + } + + if ((csi_filter_param_ptr->num_tx != 0xff) && (csi_filter_param_ptr->num_tx != pktinfo->nTx)) + { + return -WM_FAIL; + } + + // check MAC address + if ( memcmp(&(broadcast_mac[0]), &(csi_filter_param_ptr->peer_mac[0]), sizeof(broadcast_mac)) + && (memcmp(&addr2_lo, &(csi_filter_param_ptr->peer_mac[0]), sizeof(t_u16)) + || memcmp(&addr2_hi, &(csi_filter_param_ptr->peer_mac[2]), sizeof(t_u32))) ) + { + return -WM_FAIL; + } + + return WM_SUCCESS; +} + +static int check_csi_filter(unsigned int *headerBuffer, hal_pktinfo_t *pktinfo, csi_filter_param_t *csi_filter_param_ptr) +{ + hal_csirxinfo_t *csirxinfo = (hal_csirxinfo_t*)headerBuffer; + t_u16 addr2_lo = csirxinfo->addr2_lo; + t_u32 addr2_hi = csirxinfo->addr2_hi; + + // check sig format and BW + if ((csi_filter_param_ptr->packet_bandwidth != pktinfo->sigBw) + || (csi_filter_param_ptr->packet_format != convertPktInfo[pktinfo->packetType])) + { + return -WM_FAIL; + } + + // check MIMO dimension + if ((csi_filter_param_ptr->num_rx != pktinfo->nRx) + || (csi_filter_param_ptr->num_tx != pktinfo->nTx)) + { + return -WM_FAIL; + } + + // set MAC address + if (memcmp(&addr2_lo, &(csi_filter_param_ptr->peer_mac[0]), sizeof(t_u16)) + || memcmp(&addr2_hi, &(csi_filter_param_ptr->peer_mac[2]), sizeof(t_u32))) + { + return -WM_FAIL; + } + + return WM_SUCCESS; +} + +/* +* Function that handles CSI event processing for Ambient Motion Index calculation. +*/ +static void proc_csi_event(void *p_data) +{ + unsigned char *rdPtr; + unsigned int *csiBuffer = NULL; + unsigned int csi_len; + int firstPathDelay, bufferSpacing; + float ambientMotionVal_dB= 0.0f; + unsigned int headerBuffer[HEADER_LEN]; + unsigned int totalpower[MAX_RX * MAX_TX + 1]; + unsigned int pktInfo[2]; + hal_csirxinfo_t *csirxinfo; + + (void)memset(headerBuffer, 0x00, sizeof(headerBuffer)); + (void)memset(totalpower, 0x00, sizeof(totalpower)); + (void)memset(pktInfo, 0x00, sizeof(pktInfo)); + + hal_pktinfo_t *pktInfoPtr = (hal_pktinfo_t *)(void *)pktInfo; + + hal_wls_packet_params_t packetparams; + + rdPtr = (unsigned char *)p_data; + + (void)memcpy(headerBuffer, rdPtr, HEADER_LEN * sizeof(unsigned int)); + csirxinfo = (hal_csirxinfo_t*)(void *)headerBuffer; + + csi_len = csirxinfo->data_length; +#if !CONFIG_MEM_POOLS + csiBuffer = (unsigned int *)OSA_MemoryAllocate(sizeof(unsigned int) * csi_len); +#else + csiBuffer = (unsigned int *)OSA_MemoryPoolAllocate(buf_1024_MemoryPool); +#endif + + if(!csiBuffer) + { + wifi_e("%s: Failed to alloc csiBuffer",__func__); + return; + } + + (void)memcpy(csiBuffer, rdPtr, sizeof(unsigned int) * csi_len); + + (void)memset(fftInBuffer, 0x00, sizeof(fftInBuffer)); + (void)memset(scratchBuffer1, 0x00, sizeof(scratchBuffer1)); + (void)memset(&packetparams, 0x00, sizeof(hal_wls_packet_params_t)); + + packetparams.chNum = csirxinfo->chan; + + bufferSpacing = wls_unpack_csi(csiBuffer, fftInBuffer, &packetparams, &g_ami_cfg.wls_processing_input, + totalpower, pktInfo); + + if (g_ami_cfg.ami_reference_init == AMI_REF_UNINIT) + { + // initialize + if(g_ami_cfg.csiFilterSet == AMI_FILTER_NOT_SET) + { + wifi_w("Missing AMI filter set, setting filter following the first CSI/AMI packet"); + set_csi_proc_filter(headerBuffer, pktInfoPtr, &g_ami_cfg.gcsi_filter_param); + g_ami_cfg.csiFilterSet = AMI_FILTER_AUTO_SET; + } + + if(check_csi_filter_partial(headerBuffer, pktInfoPtr, &g_ami_cfg.gcsi_filter_param) == WM_SUCCESS) + { + firstPathDelay = wls_calculate_toa(pktInfo, bufferSpacing, fftInBuffer, scratchBuffer1, totalpower, + &packetparams, &g_ami_cfg.wls_processing_input); + + if(g_ami_cfg.csiFilterSet == AMI_FILTER_SET) + { + set_csi_proc_filter(headerBuffer, pktInfoPtr, &g_ami_cfg.gcsi_filter_param); + } + wls_intialize_reference(headerBuffer, pktInfo, &g_ami_cfg.gcsi_filter_param, bufferSpacing, + fftInBuffer, referenceBuffer); + g_ami_cfg.ami_reference_init = AMI_REF_INITIALIZED; + } + } + else if (check_csi_filter(headerBuffer, pktInfoPtr, &g_ami_cfg.gcsi_filter_param) == WM_SUCCESS) + { + firstPathDelay = wls_calculate_toa(pktInfo, bufferSpacing, fftInBuffer, scratchBuffer1, totalpower, + &packetparams, &g_ami_cfg.wls_processing_input); + + ambientMotionVal_dB= wls_update_cross_corr_ami_calc(headerBuffer, pktInfo, &g_ami_cfg.gcsi_filter_param, + bufferSpacing, fftInBuffer, referenceBuffer, scratchBuffer1); + + { + char myStr[4] = {'V','H','T','\0'}; + int l_BW = 20 << pktInfoPtr->sigBw; + int nRx = pktInfoPtr->nRx + 1; + int nTx = pktInfoPtr->nTx + 1; + t_u8 packet_format = convertPktInfo[pktInfoPtr->packetType]; + // record TSF + UINT64 TSF = (((UINT64)headerBuffer[4]) << 32) + headerBuffer[3]; + // calculate ToA in ns + float toa_ns = 1.e3f * firstPathDelay / (1 << 16); + if (packet_format == LEG_RATE) + { + myStr[0] = 'l'; + myStr[1] = 'e'; + myStr[2] = 'g'; + } + else if ((packet_format == HT_RATE) || (packet_format == HE_RATE)) + { + myStr[0] = 'H'; + myStr[1] = (packet_format == HE_RATE)? 'E' : 'T'; + myStr[2] = '\0'; + } + else + { + /* do nothing */ + } + mlan_adap->ami_num++; + PRINTF("NUM %d CSI Processing Results: %s(%d), RX/TX %d/%d, %-8.2f TSF %llx, Ambient Motion Index %0.1f dB\r\n", + mlan_adap->ami_num, myStr, l_BW, nRx, nTx, (double)toa_ns, TSF, (double)ambientMotionVal_dB); + + if (g_ami_cfg.gcsi_filter_param.num_csi) + { + if (g_ami_cfg.gcsi_filter_param.num_csi > 1) + { + g_ami_cfg.gcsi_filter_param.num_csi--; + } + else + { + g_ami_cfg.gcsi_filter_param.num_csi--; + g_ami_cfg.start = AMI_STOP; + g_ami_cfg.ami_reference_init = AMI_REF_UNINIT; + mlan_adap->ami_num = 0; + } + } + } + } + else + { + /* do nothing */ + } + +#if !CONFIG_MEM_POOLS + OSA_MemoryFree(csiBuffer); +#else + OSA_MemoryPoolFree(buf_1024_MemoryPool, csiBuffer); +#endif + + return; +} + +void wifi_process_csi_data(void *p_data) +{ + mlan_adap->ami_ongoing = 1; + proc_csi_event(((t_u8 *)p_data + AMI_CSI_RAW_DATA_OFFSET)); + mlan_adap->ami_ongoing = 0; + return; +} + +#endif #endif #if CONFIG_WIFI_CHANNEL_LOAD @@ -6797,7 +7076,7 @@ static int send_csi_ack(unsigned int *resArray) return ret; } -static void proc_csi_event(void *event, unsigned int *resArray) +static void proc_csi_event_wls(void *event, unsigned int *resArray) { uint8_t *csiBuffer = (uint8_t *)(event); hal_wls_packet_params_t packetparams; @@ -6825,7 +7104,7 @@ static void proc_csi_event(void *event, unsigned int *resArray) resArray[2] = 0xffffffff; resArray[3] = 0xffffffff; - wls_process_csi((unsigned int *)csiBuffer, (unsigned int *)fftInBuffer_t, &packetparams, &inputVals, resArray); + wls_process_csi((unsigned int *)csiBuffer, (unsigned int *)fftInBuffer, &packetparams, &inputVals, resArray); // record TSF resArray[3] = tsf; @@ -6838,7 +7117,7 @@ int wifi_process_wls_csi_event(void *p_data) { int ret; - proc_csi_event(((t_u8 *)p_data + sizeof(csi_event_t)), csi_res_array); + proc_csi_event_wls(((t_u8 *)p_data + sizeof(csi_event_t)), csi_res_array); // wifi_put_wls_csi_sem(); // After processing CSI raw data, release csi sem for next CSI event. ret = send_csi_ack(csi_res_array); return ret; diff --git a/mcux/middleware/wifi_nxp/wifidriver/mlan_glue.c b/mcux/middleware/wifi_nxp/wifidriver/mlan_glue.c index 0b66762f7b..9584506546 100644 --- a/mcux/middleware/wifi_nxp/wifidriver/mlan_glue.c +++ b/mcux/middleware/wifi_nxp/wifidriver/mlan_glue.c @@ -71,6 +71,11 @@ extern uint8_t wls_data[WLS_CSI_DATA_LEN]; #endif #endif +#if CONFIG_CSI_AMI +#define CSI_PROC_DATA_SIZE 1000 +uint8_t csi_proc_data[CSI_PROC_DATA_SIZE] = {0}; +#endif + #if CONFIG_WPA2_ENTP bool scan_enable_wpa2_enterprise_ap_only; #endif @@ -6722,6 +6727,15 @@ int wifi_handle_fw_event(struct bus_message *msg) PRINTM(MEVENT, "EVENT: EVENT_CSI\n"); #if CONFIG_CSI csi_deliver_data_to_user(); +#if CONFIG_CSI_AMI + /** AMI is used to reflect real-time environmental disturbances. + Caching CSI event data and delaying AMI computation is not meaningful. */ + if(!mlan_adap->ami_ongoing) + { + (void)memcpy(csi_proc_data, (t_u8 *)msg->data, CSI_PROC_DATA_SIZE); + wifi_event_completion(WIFI_EVENT_CSI_PROC, WIFI_EVENT_REASON_SUCCESS, csi_proc_data); + } +#endif #endif #if (CONFIG_11AZ) || (CONFIG_11MC) #if CONFIG_WLS_CSI_PROC diff --git a/mcux/middleware/wifi_nxp/wlcmgr/wlan.c b/mcux/middleware/wifi_nxp/wlcmgr/wlan.c index 53f498a0ee..7a87d51716 100644 --- a/mcux/middleware/wifi_nxp/wlcmgr/wlan.c +++ b/mcux/middleware/wifi_nxp/wlcmgr/wlan.c @@ -43,6 +43,10 @@ #include "wifi_ping.h" #endif +#if (CONFIG_CSI) && (CONFIG_CSI_AMI) +#include "event.h" +#endif + #if CONFIG_HOST_SLEEP #ifdef RW610 #include "fsl_power.h" @@ -141,6 +145,11 @@ static bool wlan_uap_scan_chan_list_set; #endif +#if (CONFIG_CSI) && (CONFIG_CSI_AMI) +ami_cfg_t g_ami_cfg; +static void wlan_init_ami_cfg(void); +#endif + #if CONFIG_MEF_CFG wlan_flt_cfg_t g_flt_cfg; #endif @@ -3194,6 +3203,92 @@ void wlcm_process_csi_status_report(struct wifi_message *msg) #endif } } + +#if CONFIG_CSI_AMI +void wlan_set_ami_cfg(wlan_csi_proc_cfg *cfg) +{ + (void)memcpy(g_ami_cfg.gcsi_filter_param.peer_mac, cfg->peer_mac, MLAN_MAC_ADDR_LENGTH); + g_ami_cfg.gcsi_filter_param.num_csi = cfg->num_csi; + g_ami_cfg.gcsi_filter_param.packet_bandwidth = cfg->packet_bandwidth; + g_ami_cfg.gcsi_filter_param.packet_format = cfg->packet_format; + g_ami_cfg.gcsi_filter_param.reference_update = cfg->reference_update; + + g_ami_cfg.csiFilterSet = AMI_FILTER_SET; + + return; +} + +void wlan_start_stop_ami(uint8_t start) +{ + g_ami_cfg.start = start; + + if(!start) + { + g_ami_cfg.gcsi_filter_param.num_csi = 0; + mlan_adap->ami_num = 0; + g_ami_cfg.start = AMI_STOP; + + if (g_ami_cfg.csiFilterSet == AMI_FILTER_AUTO_SET) + { + g_ami_cfg.csiFilterSet = AMI_FILTER_NOT_SET; + } + + g_ami_cfg.ami_reference_init = AMI_REF_UNINIT; + } + + return; +} + +static void wlcm_process_csi_data(void *p_data) +{ + wifi_process_csi_data(p_data); +} + +static void wlan_init_ami_cfg(void) +{ + (void)memset(&g_ami_cfg, 0x00, sizeof(ami_cfg_t)); + + g_ami_cfg.channel = 0; + g_ami_cfg.start = AMI_STOP; + + g_ami_cfg.wls_processing_input.enableCsi = 1; // turn on CSI processing + g_ami_cfg.wls_processing_input.enableAoA = AOA_DEFAULT; // turn on AoA (req. enableCsi==1) + g_ami_cfg.wls_processing_input.nTx = MAX_TX; // limit # tx streams to process + g_ami_cfg.wls_processing_input.nRx = MAX_RX; // limit # rx to process + g_ami_cfg.wls_processing_input.selCal = 0; // choose cal values + g_ami_cfg.wls_processing_input.dumpMul = 0; // dump extra peaks in AoA + g_ami_cfg.wls_processing_input.enableAntCycling = 0; // enable antenna cycling + g_ami_cfg.wls_processing_input.dumpRawAngle = 0; // Dump Raw Angle + g_ami_cfg.wls_processing_input.useToaMin = TOA_MIN_DEFAULT; // 1: use min combining, 0: power combining; + g_ami_cfg.wls_processing_input.useSubspace = SUBSPACE_DEFAULT; // 1: use subspace algo; 0: no; + g_ami_cfg.wls_processing_input.useFindAngleDelayPeaks = ENABLE_DELAY_PEAKS; // use this algorithm for AoA + + g_ami_cfg.gcsi_filter_param.num_csi = 0; + g_ami_cfg.gcsi_filter_param.packet_bandwidth = 0xff; + g_ami_cfg.gcsi_filter_param.packet_format = 0xff; + g_ami_cfg.gcsi_filter_param.reference_update = 0; + g_ami_cfg.gcsi_filter_param.IIR_alpha = PI_ALPHA_FACTOR; + g_ami_cfg.gcsi_filter_param.kalman_p0 = KALMAN_P0; + g_ami_cfg.gcsi_filter_param.kalman_alpha = KALMAN_ALPHA; + g_ami_cfg.gcsi_filter_param.kalman_N0 = KALMAN_N0; + g_ami_cfg.gcsi_filter_param.num_rx = 0xff; + g_ami_cfg.gcsi_filter_param.num_tx = 0xff; + g_ami_cfg.gcsi_filter_param.peer_mac[0] = 0xff; + g_ami_cfg.gcsi_filter_param.peer_mac[1] = 0xff; + g_ami_cfg.gcsi_filter_param.peer_mac[2] = 0xff; + g_ami_cfg.gcsi_filter_param.peer_mac[3] = 0xff; + g_ami_cfg.gcsi_filter_param.peer_mac[4] = 0xff; + g_ami_cfg.gcsi_filter_param.peer_mac[5] = 0xff; + + g_ami_cfg.csiFilterSet = AMI_FILTER_NOT_SET; + g_ami_cfg.ami_reference_init = AMI_REF_UNINIT; + + mlan_adap->ami_num = 0; + mlan_adap->ami_ongoing = 0; + + return; +} +#endif /* CONFIG_CSI_AMI */ #endif #if CONFIG_WPA_SUPP @@ -7124,6 +7219,15 @@ static enum cm_sta_state handle_message(struct wifi_message *msg) wlcm_d("got event: csi status report"); wlcm_process_csi_status_report(msg); break; +#if CONFIG_CSI_AMI + case WIFI_EVENT_CSI_PROC: + if(g_ami_cfg.start == AMI_START) + { + wlcm_d("got event: csi data process"); + wlcm_process_csi_data(msg->data); + } + break; +#endif #endif case WIFI_EVENT_SLEEP: @@ -7668,6 +7772,11 @@ int wlan_init(const uint8_t *fw_start_addr, const size_t size) else wifi_d("USB init callback is not registered"); #endif + +#if (CONFIG_CSI) && (CONFIG_CSI_AMI) + wlan_init_ami_cfg(); +#endif + return ret; } diff --git a/mcux/middleware/wifi_nxp/wlcmgr/wlan_tests.c b/mcux/middleware/wifi_nxp/wlcmgr/wlan_tests.c index 4a87c672d3..f618cc9943 100644 --- a/mcux/middleware/wifi_nxp/wlcmgr/wlan_tests.c +++ b/mcux/middleware/wifi_nxp/wlcmgr/wlan_tests.c @@ -5057,6 +5057,237 @@ static void test_wlan_csi_cfg(int argc, char **argv) (void)PRINTF("Failed to send csi cfg\r\n"); } } + +#if CONFIG_CSI_AMI +static void dump_wlan_set_ami_cfg_usage(void) +{ + (void)PRINTF("Usage : wlan-set-ami-cfg mac type [packet_type] bw [band_width]\r\n"); + (void)PRINTF(" ref [update_ref] num [CSI_number]\r\n"); + (void)PRINTF("Mandatory parameters: < xxxx >\r\n"); + (void)PRINTF(" For example: mac \r\n"); + (void)PRINTF("Optional parameters: [ xxxx ]\r\n"); + (void)PRINTF(" For example: bw [bandwidth]\r\n"); + (void)PRINTF("mac : Source address of CSI data being processed. \r\n"); + (void)PRINTF("type : Packet type: \r\n"); + (void)PRINTF(" 0: legacy - 11a/g \r\n"); + (void)PRINTF(" 1: HT - 11n \r\n"); + (void)PRINTF(" 2: VHT - 11ac \r\n"); + (void)PRINTF(" 3: HE - 11ax \r\n"); + (void)PRINTF(" Default value: 0 \r\n"); + (void)PRINTF("bw : Bandwidth:\r\n"); + (void)PRINTF(" 0: 20MHz \r\n"); + (void)PRINTF(" 1: 40MHz \r\n"); + (void)PRINTF(" 2: 80MHz \r\n"); + (void)PRINTF(" Default value: 0 \r\n"); + (void)PRINTF("ref : Reference update: \r\n"); + (void)PRINTF(" 0: static - first, no updates. \r\n"); + (void)PRINTF(" 1: IIR filter - first CSI is reference, update with IIR fitler coefficient alpha. \r\n"); + (void)PRINTF(" 2: Kalman filter - first CSI is reference, update with Kalaman fitler. \r\n"); + (void)PRINTF(" Default value: 0 \r\n"); + (void)PRINTF("num : The number of CSI data to be processed.\r\n"); + (void)PRINTF(" 0: Process CSI data until stop it.\r\n"); + (void)PRINTF(" [1 - 255]: Number of CSI data to be processed.\r\n"); + (void)PRINTF(" Default value: 0 \r\n"); +} + +static void dump_wlan_set_ami_cfg_setting(const wlan_csi_proc_cfg * cfg) +{ + (void)PRINTF("The current AMI configuration is:\r\n"); + (void)PRINTF(" MAC Address : %02X:%02X:%02X:%02X:%02X:%02X\r\n", + cfg->peer_mac[0], cfg->peer_mac[1], cfg->peer_mac[2], + cfg->peer_mac[3], cfg->peer_mac[4], cfg->peer_mac[5]); + (void)PRINTF(" Packet Type : %d\r\n", cfg->packet_format); + (void)PRINTF(" Bandwidth : %d\r\n", cfg->packet_bandwidth); + (void)PRINTF(" Reference Update : %d\r\n", cfg->reference_update); + (void)PRINTF(" Number of Packets : %d\r\n", cfg->num_csi); +} + +static void test_wlan_set_ami_cfg(int argc, char **argv) +{ + wlan_csi_proc_cfg cfg; + int arg = 0, ret = 0; + unsigned int value = 0; + char raw_mac[MLAN_MAC_ADDR_LENGTH] = {0}; + struct + { + unsigned mac : 1; + unsigned type : 1; + unsigned bw : 1; + unsigned ref : 1; + unsigned num : 1; + } info; + + (void)memset(&info, 0, sizeof(info)); + + (void)memset(&cfg, 0, sizeof(cfg)); + + if (argc < 5 && argc > 13) + { + (void)PRINTF("Error: invalid number of arguments\r\n"); + dump_wlan_set_ami_cfg_usage(); + return; + } + arg++; + + do + { + if(info.mac == 0 && string_equal("mac", argv[arg])) + { + ret = get_mac(argv[arg + 1], raw_mac, ':'); + if (ret != 0) + { + (void)PRINTF("Error: invalid 'mac' setting\r\n"); + return; + } + + info.mac = 1; + (void)memcpy(cfg.peer_mac, raw_mac, MLAN_MAC_ADDR_LENGTH); + arg += 2; + } + else if(info.type == 0 && string_equal("type", argv[arg])) + { + if(get_uint(argv[arg + 1], &value, strlen(argv[arg + 1]))) + { + (void)PRINTF("Error: invalid 'packet type' setting.\r\n"); + return; + } + + if(value > 3) + { + (void)PRINTF("Error: invalid 'packet type' setting.\r\n"); + dump_wlan_set_ami_cfg_usage(); + return; + } + + info.type = 1; + cfg.packet_format = value & 0xFF; + arg += 2; + } + else if(info.bw == 0 && string_equal("bw", argv[arg])) + { + if(get_uint(argv[arg + 1], &value, strlen(argv[arg + 1]))) + { + (void)PRINTF("Error: invalid 'bandwidth' setting.\r\n"); + return; + } + + if(value > 2) + { + (void)PRINTF("Error: invalid 'bandwidth' setting.\r\n"); + dump_wlan_set_ami_cfg_usage(); + return; + } + + info.bw = 1; + cfg.packet_bandwidth = value & 0xFF; + arg += 2; + } + else if(info.ref == 0 && string_equal("ref", argv[arg])) + { + if(get_uint(argv[arg + 1], &value, strlen(argv[arg + 1]))) + { + (void)PRINTF("Error: invalid 'update ref' setting.\r\n"); + return; + } + + if(value > 2) + { + (void)PRINTF("Error: invalid 'update ref' setting.\r\n"); + dump_wlan_set_ami_cfg_usage(); + return; + } + + info.ref = 1; + cfg.reference_update = value & 0xFF; + arg += 2; + } + else if(info.num == 0 && string_equal("num", argv[arg])) + { + if(get_uint(argv[arg + 1], &value, strlen(argv[arg + 1]))) + { + (void)PRINTF("Error: invalid 'CSI number' setting.\r\n"); + return; + } + + if(value > 255) + { + (void)PRINTF("Error: invalid 'CSI number' setting.\r\n"); + dump_wlan_set_ami_cfg_usage(); + return; + } + + info.num = 1; + cfg.num_csi = value & 0xFF; + arg += 2; + } + else + { + PRINTF("UNKNOW setting.\r\n"); + dump_wlan_set_ami_cfg_usage(); + return; + } + } while (arg < argc); + + if(info.mac == 0) + { + PRINTF("Please check whether mac or channel is set.\r\n"); + dump_wlan_set_ami_cfg_usage(); + return; + } + + dump_wlan_set_ami_cfg_setting(&cfg); + + wlan_set_ami_cfg(&cfg); + + return; +} + +static void dump_test_wlan_start_stop_ami_usage(void) +{ + (void)PRINTF("wlan-start-stop-ami \r\n"); + (void)PRINTF(" 1 - start to caculate Ambient Motion Index.\r\n"); + (void)PRINTF(" 0 - stop to caculate Ambient Motion Index.\r\n"); +} + +static void test_wlan_start_stop_ami(int argc, char **argv) +{ + unsigned int value = 0; + uint8_t start = 0; + + if(argc != 2) + { + (void)PRINTF("Invalid configuration number\r\n"); + dump_test_wlan_start_stop_ami_usage(); + return; + } + + if(get_uint(argv[1], &value, strlen(argv[1]))) + { + (void)PRINTF("Invalid setting\r\n"); + dump_test_wlan_start_stop_ami_usage(); + return; + } + + start = value & 0xFF; + if(start != 0 && start != 1) + { + (void)PRINTF("Invalid start/stop setting\r\n"); + dump_test_wlan_start_stop_ami_usage(); + return; + } + + if(start) + { + wlan_unregister_csi_user_callback(); + } + + wlan_start_stop_ami(start); + + return; + +} + +#endif /* CONFIG_CSI_AMI */ #endif #if (CONFIG_11K) || (CONFIG_11V) || (CONFIG_11R) || (CONFIG_ROAMING) @@ -8933,6 +9164,12 @@ static struct cli_command tests[] = { " ", test_wlan_set_csi_param_header}, {"wlan-set-csi-filter", " ", test_wlan_set_csi_filter}, +#if CONFIG_CSI_AMI + {"wlan-set-ami-cfg", + " mac type bw ref num ", + test_wlan_set_ami_cfg}, + {"wlan-start-stop-ami", "", test_wlan_start_stop_ami}, +#endif #endif #if CONFIG_TX_RX_HISTOGRAM {"wlan-txrx-histogram", " ", test_wlan_txrx_histogram}, diff --git a/mcux/middleware/wifi_nxp/wls/range_kalman.c b/mcux/middleware/wifi_nxp/wls/range_kalman.c deleted file mode 100644 index 6be6063e60..0000000000 --- a/mcux/middleware/wifi_nxp/wls/range_kalman.c +++ /dev/null @@ -1,143 +0,0 @@ -/** @file range_kalman.c - * - * @brief This file contains Kalman filter for WLS range measurements - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -#include -#if CONFIG_WLS_CSI_PROC -#include "range_kalman.h" -#include -#include - -#ifdef RANGE_RUN_FLT -int range_kalman(range_kalman_state *in) -{ - float range_hat, range_rate_hat; - float nu, S1; - float R1_11, R1_12, R1_22, W1_1, W1_2; - float delta_T = (float)(in->time - in->last_time) / 1000; // now seconds - float delta_T2 = delta_T * delta_T; - float delta_T3 = delta_T2 * delta_T; - - // state propagation - // A = | 1 T | - // | 0 1 | - range_hat = in->last_range + delta_T * in->last_range_rate; - range_rate_hat = in->last_range_rate; - // residual - nu = in->range_measurement - range_hat; // H = [1; 0] - - // propagated cov = A*R11*A' + Q - R1_11 = in->R0_11 + delta_T * 2 * in->R0_12 + delta_T2 * in->R0_22; - R1_12 = in->R0_12 + delta_T * in->R0_22; - R1_22 = in->R0_22; - // Q = | T^3/3 T^2/2 | * drive_var - // | T^2/2 T | - R1_11 += in->drive_var * delta_T3 / 3; - R1_12 += in->drive_var * delta_T2 / 2; - R1_22 += in->drive_var * delta_T; - - // inovation cov S1 = y_err + H*R1*H' - S1 = in->measurement_var + R1_11; - // filter gain W1 = (R1*H')/S1 - W1_1 = R1_11 / S1; - W1_2 = R1_12 / S1; - - // updated covariance R11 = R1 - W1*S1*W1' - R1_11 -= W1_1 * S1 * W1_1; - in->R0_11 = (R1_11 > 0) ? R1_11 : 0; - R1_12 -= W1_1 * S1 * W1_2; - in->R0_12 = R1_12; - R1_22 -= W1_2 * S1 * W1_2; - in->R0_22 = (R1_22 > 0) ? R1_22 : 0; - // updated state - range_hat += W1_1 * nu; - in->last_range = (range_hat > 0) ? range_hat : 0; - range_rate_hat += W1_2 * nu; - in->last_range_rate = range_rate_hat; - in->last_time = in->time; - - PRINTF("Kalman update R mat: %f m, %f m/s; d-time: %d ms; range rate %f m/s\r\n", (double)sqrtf(in->R0_11), - (double)sqrtf(in->R0_22), (int)(delta_T * 1000), (double)in->last_range_rate); - return 0; // no errors -} - -void range_kalman_init(range_kalman_state *in, - float range, - unsigned long long time, - float range_drive_var, - float range_measurement_var, - float range_rate_init) -{ - // initialize state - in->last_range = range; - in->last_range_rate = 0; - in->last_time = time; - - in->R0_11 = range_measurement_var; - in->R0_12 = 0; - in->R0_22 = range_rate_init; - - // initialize parameters - in->drive_var = range_drive_var; - in->measurement_var = range_measurement_var; -} - -#else -int range_kalman(range_kalman_state *in) -{ - int range_hat, range_rate_hat; - int nu, S1; - int R1_11, R1_12, R1_22, W1_1, W1_2; - unsigned int delta_T = in->time - in->last_time; // format ?? - unsigned int delta_T2 = delta_T * delta_T; - unsigned int delta_T3 = delta_T2 * delta_T; - - // state propagation - // A = | 1 T | - // | 0 1 | - range_hat = in->last_range + delta_T * in->last_range_rate; - range_rate_hat = in->last_range_rate; - // residual - nu = in->range_measurement - range_hat; // H = [1; 0] - - // propagated cov = A*R11*A' + Q - R1_11 = in->R0_11 + delta_T * 2 * in->R0_12 + delta_T2 * in->R0_22; - R1_12 = in->R0_12 + delta_T * in->R0_22; - R1_22 = in->R0_22; - // Q = | T^3/3 T^2/2 | * drive_var - // | T^2/2 T | - R1_11 += in->drive_var * delta_T3 / 3; - R1_12 += in->drive_var * delta_T2 / 2; - R1_22 += in->drive_var * delta_T; - - // inovation cov S1 = y_err + H*R1*H' - S1 = in->measurement_var + R1_11; - // filter gain W1 = (R1*H')/S1 - W1_1 = R1_11 / S1; - W1_2 = R1_12 / S1; - - // updated covariance R11 = R1 - W1*S1*W1' - R1_11 -= W1_1 * S1 * W1_1; - in->R0_11 = (R1_11 > 0) ? (unsigned short)R1_11 : 0; - R1_12 -= W1_1 * S1 * W1_2; - in->R0_12 = R1_12; - R1_22 -= W1_2 * S1 * W1_2; - in->R0_22 = (R1_22 > 0) ? (unsigned short)R1_22 : 0; - // updated state - range_hat += W1_1 * nu; - in->last_range = (range_hat > 0) ? (unsigned short)range_hat : 0; - range_rate_hat += W1_2 * nu; - in->last_range_rate = (signed short)range_rate_hat; - in->last_time = in->time; - - return 0; // no errors -} -#endif - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/range_kalman.h b/mcux/middleware/wifi_nxp/wls/range_kalman.h deleted file mode 100644 index 29047e1ed4..0000000000 --- a/mcux/middleware/wifi_nxp/wls/range_kalman.h +++ /dev/null @@ -1,64 +0,0 @@ -#if CONFIG_WLS_CSI_PROC -/** @file range_kalman.h - * - * @brief This file contains Kalman filter for WLS range measurements - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -#ifndef RANGE_KALMAN -#define RANGE_KALMAN - -#define RANGE_RUN_FLT -#ifdef RANGE_RUN_FLT -// range is in meters, range_rate in meters/second -// time is in seconds -typedef struct -{ - // input - unsigned long long time; - float range_measurement; - // state - float last_range; // also output <-- - float last_range_rate; - float R0_11, R0_22, R0_12; - unsigned long long last_time; - // model parameters - float measurement_var; - float drive_var; -} range_kalman_state; - -void range_kalman_init(range_kalman_state *in, - float range, - unsigned long long time, - float range_drive_var, - float range_measurement_var, - float range_rate_init); -#else -// range format u16.8 in meters -// time format is u64.0 in milliseconds -typedef struct -{ - // input - unsigned long long time; - unsigned int range_measurement; - // state - unsigned short last_range; // also output <-- - signed short last_range_rate; - unsigned long long last_time; - unsigned short R0_11, R0_22; - signed int R0_12; - // model parameters - unsigned int measurement_var; - unsigned int drive_var; -} range_kalman_state; -#endif - -int range_kalman(range_kalman_state *in); - -#endif - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_QR_algorithm.c b/mcux/middleware/wifi_nxp/wls/wls_QR_algorithm.c deleted file mode 100644 index c1fe830e86..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_QR_algorithm.c +++ /dev/null @@ -1,1540 +0,0 @@ -/** @file wls_QR_algorithm.c - * - * @brief This file contains the QR algorithm processing code. - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * DFW code that defines the QR algorithm processing. - ************************************************************************/ - -#include -#if CONFIG_WLS_CSI_PROC - -// Standard includes. -#include -// #include -// #include -#ifdef ARM_DS5 -#include -#else -#include -// 5 sqrtf, 10 fabsf -> use hardware -#endif - -// Specific includes. -#include "wls_QR_algorithm.h" - -#define TOL_HH_SIG 1e-8f -#define TOL_QR_STEP 1e-5f -#define TOL_QR_STEP_LOW 1e-3f -#define WILK_IT_MAX 15 - -// #define DEBUG_OUTPUT - -int house_holder_vec(float *x, float *v, float *betaPtr, float *muPtr, int len) -{ - int ii; - float sigma = 0; - float mu, xOne, vOne, vOneSqr; -#ifdef ARM_DS5 - float32x2_t sigmaVec, xVecf32; - sigmaVec = vdup_n_f32(0.0f); -#endif - xOne = x[0]; - v[0] = 1.0f; - for (ii = 1; ii < len - 1; ii += 2) - { -#ifdef ARM_DS5 - xVecf32 = vld1_f32((float32_t const *)(x + ii)); - sigmaVec = vmla_f32(sigmaVec, xVecf32, xVecf32); - vst1_f32((float32_t *)(v + ii), xVecf32); -#else - sigma += x[ii] * x[ii]; - sigma += x[ii + 1] * x[ii + 1]; - v[ii] = x[ii]; - v[ii + 1] = x[ii + 1]; -#endif - } -#ifdef ARM_DS5 - sigmaVec = vpadd_f32(sigmaVec, sigmaVec); - sigma = vget_lane_f32(sigmaVec, 0); -#endif - if (ii < len) - { - sigma += x[ii] * x[ii]; - v[ii] = x[ii]; - } - if (sigma < TOL_HH_SIG) - { - *muPtr = xOne; - *betaPtr = 0; - return 1; - } - - mu = SQRTF(xOne * xOne + sigma); - if (xOne <= 0) - { - vOne = xOne - mu; - } - else - { - vOne = -sigma / (xOne + mu); - } - vOneSqr = vOne * vOne; - *betaPtr = 2 * vOneSqr / (sigma + vOneSqr); - *muPtr = mu; - for (ii = 1; ii < len; ii++) - { - v[ii] = v[ii] / vOne; - } - - return 0; -} - -int givens_rot(float a, float b, float *cosPtr, float *lenPtr) -{ - float r, temp, tempInv; - - if (b == 0) - { - cosPtr[0] = 1; - cosPtr[1] = 0; - return 1; - } - if (FABSF(b) > FABSF(a)) - { - r = -a / b; - temp = SQRTF(1 + r * r); - tempInv = 1 / temp; - cosPtr[1] = tempInv; - cosPtr[0] = tempInv * r; - *lenPtr = -b * temp; - } - else - { - r = -b / a; - temp = SQRTF(1 + r * r); - tempInv = 1 / temp; - - cosPtr[0] = tempInv; - cosPtr[1] = tempInv * r; - *lenPtr = a * temp; - } - - return 0; -} - -int QR_step_Wilkinson(float *diagA, float *offDiagA, float *Qmat, int matSizeN, int kk) -{ -#ifdef ARM_DS5 - int ii, jj; - float a, b, c, d; - float bSqr, dSqr; - float mu, temp; - float x, z; - float lenVal, cosArray[2]; - float offDiagA2 = 0; - float *ldPtr; - - float32x2_t cosVec, cosVec2, diagAVec, offDiagAVec; - float32x2_t tempVec1, tempVec2, tempVec3; - float32x2_t colVec0, colVec1, resVec0, resVec1; - float32x2x2_t trnVec; - - a = diagA[kk - 1]; // second last diagonal - b = offDiagA[kk]; - c = diagA[kk]; // last diagonal - d = (a - c) / 2; - - bSqr = b * b; - dSqr = d * d; - temp = SQRTF(dSqr + bSqr); - temp = (d > 0) ? (d + temp) : (d - temp); - mu = c - bSqr / temp; - - // first iteration - x = diagA[0] - mu; - z = offDiagA[1]; - - givens_rot(x, z, &cosArray[0], &lenVal); - - cosVec = vld1_f32((float32_t const *)cosArray); // VLD1.32 {d0}, [r0] - - tempVec3 = vrev64_f32(cosVec); // VREV64.32 d0,d0 - tempVec3 = vneg_f32(tempVec3); - trnVec = vtrn_f32(tempVec3, cosVec); // VTRN.32 d0,d0 - cosVec2 = trnVec.val[0]; - - diagAVec = vld1_f32((float32_t const *)diagA); - offDiagAVec = vld1_f32((float32_t const *)(offDiagA + 1)); - - tempVec1 = vmul_lane_f32(cosVec, diagAVec, 0); - tempVec1 = vmla_lane_f32(tempVec1, cosVec2, offDiagAVec, 0); - - tempVec2 = vmul_lane_f32(cosVec, offDiagAVec, 0); - tempVec2 = vmla_lane_f32(tempVec2, cosVec2, diagAVec, 1); - - resVec0 = vmul_f32(tempVec1, cosVec); - resVec0 = vmla_f32(resVec0, tempVec2, cosVec2); - - tempVec3 = vmul_lane_f32(cosVec2, offDiagAVec, 1); - - vst1_f32((float32_t *)diagA, resVec0); - - resVec1 = vmul_lane_f32(tempVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, tempVec2, cosVec, 0); - - offDiagA[1] = vget_lane_f32(resVec1, 0); - - // update first two rows of Q - ldPtr = Qmat; - for (jj = 0; jj < matSizeN; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); - - resVec0 = vmul_lane_f32(colVec0, cosVec, 0); - resVec1 = vmul_lane_f32(colVec1, cosVec, 0); - - resVec0 = vmls_lane_f32(resVec0, colVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, colVec0, cosVec, 1); - - vst1_f32((float32_t *)ldPtr, resVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), resVec1); - ldPtr += 2; - } - - if (kk > 1) - { - offDiagA[2] = vget_lane_f32(tempVec3, 1); // VMOV.32 r0, d0[0] - offDiagA2 = vget_lane_f32(tempVec3, 0); - - for (ii = 2; ii < kk; ii++) - { - x = offDiagA[ii - 1]; - z = offDiagA2; - givens_rot(x, z, &cosArray[0], offDiagA + (ii - 1)); - // offDiagA[ii-1] = lenVal; - cosVec = vld1_f32((float32_t const *)cosArray); // VLD1.32 {d0}, [r0] - - tempVec3 = vrev64_f32(cosVec); // VREV64.32 d0,d0 - tempVec3 = vneg_f32(tempVec3); - trnVec = vtrn_f32(tempVec3, cosVec); // VTRN.32 d0,d0 - cosVec2 = trnVec.val[0]; - - diagAVec = vld1_f32((float32_t const *)(diagA + ii - 1)); - offDiagAVec = vld1_f32((float32_t const *)(offDiagA + ii)); - - tempVec1 = vmul_lane_f32(cosVec, diagAVec, 0); - tempVec1 = vmla_lane_f32(tempVec1, cosVec2, offDiagAVec, 0); - - tempVec2 = vmul_lane_f32(cosVec, offDiagAVec, 0); - tempVec2 = vmla_lane_f32(tempVec2, cosVec2, diagAVec, 1); - - resVec0 = vmul_f32(tempVec1, cosVec); - resVec0 = vmla_f32(resVec0, tempVec2, cosVec2); - - tempVec3 = vmul_lane_f32(cosVec2, offDiagAVec, 1); - - vst1_f32((float32_t *)(diagA + ii - 1), resVec0); - - resVec1 = vmul_lane_f32(tempVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, tempVec2, cosVec, 0); - - offDiagA[ii] = vget_lane_f32(resVec1, 0); - - offDiagA[ii + 1] = vget_lane_f32(tempVec3, 1); - offDiagA2 = vget_lane_f32(tempVec3, 0); - - // update two rows of Q - ldPtr = Qmat + (ii - 1) * MAX_MAT_SIZE; - for (jj = 0; jj < matSizeN; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); - - resVec0 = vmul_lane_f32(colVec0, cosVec, 0); - resVec1 = vmul_lane_f32(colVec1, cosVec, 0); - - resVec0 = vmls_lane_f32(resVec0, colVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, colVec0, cosVec, 1); - - vst1_f32((float32_t *)ldPtr, resVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), resVec1); - ldPtr += 2; - } - } - - // last iteration , ii=kk - x = offDiagA[kk - 1]; - z = offDiagA2; - givens_rot(x, z, &cosArray[0], offDiagA + (kk - 1)); - // offDiagA[kk-1] = lenVal; - cosVec = vld1_f32((float32_t const *)cosArray); // VLD1.32 {d0}, [r0] - - tempVec3 = vrev64_f32(cosVec); // VREV64.32 d0,d0 - tempVec3 = vneg_f32(tempVec3); - trnVec = vtrn_f32(tempVec3, cosVec); // VTRN.32 d0,d0 - cosVec2 = trnVec.val[0]; - - diagAVec = vld1_f32((float32_t const *)(diagA + kk - 1)); - offDiagAVec = vld1_f32((float32_t const *)(offDiagA + kk)); - - tempVec1 = vmul_lane_f32(cosVec, diagAVec, 0); - tempVec1 = vmla_lane_f32(tempVec1, cosVec2, offDiagAVec, 0); - - tempVec2 = vmul_lane_f32(cosVec, offDiagAVec, 0); - tempVec2 = vmla_lane_f32(tempVec2, cosVec2, diagAVec, 1); - - resVec0 = vmul_f32(tempVec1, cosVec); - resVec0 = vmla_f32(resVec0, tempVec2, cosVec2); - - vst1_f32((float32_t *)(diagA + kk - 1), resVec0); - - resVec1 = vmul_lane_f32(tempVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, tempVec2, cosVec, 0); - - offDiagA[kk] = vget_lane_f32(resVec1, 0); - - // update two rows of Q - ldPtr = Qmat + (kk - 1) * MAX_MAT_SIZE; - for (jj = 0; jj < matSizeN; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); - - resVec0 = vmul_lane_f32(colVec0, cosVec, 0); - resVec1 = vmul_lane_f32(colVec1, cosVec, 0); - - resVec0 = vmls_lane_f32(resVec0, colVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, colVec0, cosVec, 1); - - vst1_f32((float32_t *)ldPtr, resVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), resVec1); - ldPtr += 2; - } - } - - return 0; -#else - int ii, jj; - float a, b, c, d; - float bSqr, dSqr; - float mu, temp; - float x, z; - float cosVal, sinVal, lenVal; - float cosArray[2]; - float offDiagA2 = 0; - float temp11, temp21, temp12, temp22, temp13, temp23; - - a = diagA[kk - 1]; // second last diagonal - b = offDiagA[kk]; - c = diagA[kk]; // last diagonal - d = (a - c) / 2; - - bSqr = b * b; - dSqr = d * d; - temp = sqrtf(dSqr + bSqr); - temp = (d > 0) ? (d + temp) : (d - temp); - mu = c - bSqr / temp; - - // first iteration - x = diagA[0] - mu; - z = offDiagA[1]; - - givens_rot(x, z, &cosArray[0], &lenVal); - cosVal = cosArray[0]; - sinVal = cosArray[1]; - - temp11 = cosVal * diagA[0] - sinVal * offDiagA[1]; - temp21 = sinVal * diagA[0] + cosVal * offDiagA[1]; - - temp12 = cosVal * offDiagA[1] - sinVal * diagA[1]; - temp22 = sinVal * offDiagA[1] + cosVal * diagA[1]; - - temp13 = -sinVal * offDiagA[2]; - temp23 = cosVal * offDiagA[2]; - - diagA[0] = cosVal * temp11 - sinVal * temp12; - offDiagA[1] = sinVal * temp11 + cosVal * temp12; - diagA[1] = sinVal * temp21 + cosVal * temp22; - - // update first two rows of Q - for (jj = 0; jj < matSizeN; jj++) - { - temp11 = Qmat[jj]; - temp21 = Qmat[jj + MAX_MAT_SIZE]; - - Qmat[jj] = cosVal * temp11 - sinVal * temp21; - Qmat[jj + MAX_MAT_SIZE] = cosVal * temp21 + sinVal * temp11; - } - - if (kk > 1) - { - offDiagA[2] = temp23; - offDiagA2 = temp13; - - for (ii = 2; ii < kk; ii++) - { - x = offDiagA[ii - 1]; - z = offDiagA2; - givens_rot(x, z, &cosArray[0], &lenVal); - cosVal = cosArray[0]; - sinVal = cosArray[1]; - - temp11 = cosVal * diagA[ii - 1] - sinVal * offDiagA[ii]; - temp21 = sinVal * diagA[ii - 1] + cosVal * offDiagA[ii]; - - temp12 = cosVal * offDiagA[ii] - sinVal * diagA[ii]; - temp22 = sinVal * offDiagA[ii] + cosVal * diagA[ii]; - - temp13 = -sinVal * offDiagA[ii + 1]; - temp23 = cosVal * offDiagA[ii + 1]; - - offDiagA[ii - 1] = lenVal; - - diagA[ii - 1] = cosVal * temp11 - sinVal * temp12; - offDiagA[ii] = sinVal * temp11 + cosVal * temp12; - diagA[ii] = sinVal * temp21 + cosVal * temp22; - - offDiagA[ii + 1] = temp23; - offDiagA2 = temp13; - // update two rows of Q - for (jj = 0; jj < matSizeN; jj++) - { - temp11 = Qmat[jj + (ii - 1) * MAX_MAT_SIZE]; - temp21 = Qmat[jj + ii * MAX_MAT_SIZE]; - - Qmat[jj + (ii - 1) * MAX_MAT_SIZE] = cosVal * temp11 - sinVal * temp21; - Qmat[jj + ii * MAX_MAT_SIZE] = cosVal * temp21 + sinVal * temp11; - } - } - - // last iteration , ii=kk - x = offDiagA[kk - 1]; - z = offDiagA2; - givens_rot(x, z, &cosArray[0], &lenVal); - cosVal = cosArray[0]; - sinVal = cosArray[1]; - - temp11 = cosVal * diagA[kk - 1] - sinVal * offDiagA[kk]; - temp21 = sinVal * diagA[kk - 1] + cosVal * offDiagA[kk]; - - temp12 = cosVal * offDiagA[kk] - sinVal * diagA[kk]; - temp22 = sinVal * offDiagA[kk] + cosVal * diagA[kk]; - - offDiagA[kk - 1] = lenVal; - diagA[kk - 1] = cosVal * temp11 - sinVal * temp12; - offDiagA[kk] = sinVal * temp11 + cosVal * temp12; - diagA[kk] = sinVal * temp21 + cosVal * temp22; - // update two rows of Q - for (jj = 0; jj < matSizeN; jj++) - { - temp11 = Qmat[jj + (kk - 1) * MAX_MAT_SIZE]; - temp21 = Qmat[jj + kk * MAX_MAT_SIZE]; - - Qmat[jj + (kk - 1) * MAX_MAT_SIZE] = cosVal * temp11 - sinVal * temp21; - Qmat[jj + kk * MAX_MAT_SIZE] = cosVal * temp21 + sinVal * temp11; - } - } - - return 0; -#endif -} - -int unsym_QR_step_Wilkinson(float *matA, int matSizeN, int kk, int offset) -{ -#ifdef ARM_DS5 - int ii, jj; - float a, b, c, d; - float bSqr, dSqr; - float mu, temp; - float x, z; - float lenVal, cosArray[2]; - float *ldPtr; - - float32x2_t cosVec, cosVec2, tempVec3; - float32x2_t colVec0, colVec1, resVec0, resVec1; - float32x2x2_t trnVec; - - a = matA[(kk - 1) * (matSizeN + 1)]; // diagA[kk-1]; // second last diagonal - b = matA[(kk - 1) * (matSizeN + 1) + 1]; // offDiagA[kk]; - c = matA[kk * (matSizeN + 1)]; // diagA[kk]; // last diagonal - if (kk == 1 + offset) - { - float p, q; - d = matA[kk * (matSizeN + 1) - 1]; - - p = -a - c; - q = a * c - b * d; - if (p * p < 4 * q) - { - matA[offset * (matSizeN + 1)] = -p / 2; - matA[offset * (matSizeN + 1) + 1] = 0; - matA[(offset + 1) * (matSizeN + 1)] = -p / 2; - - return 1; - } - } - d = (a - c) / 2; - - bSqr = b * b; - dSqr = d * d; - temp = SQRTF(dSqr + bSqr); - temp = (d > 0) ? (d + temp) : (d - temp); - mu = c - bSqr / temp; - - // first iteration - x = matA[offset * (matSizeN + 1)] - mu; // diagA[0]-mu; - z = matA[offset * (matSizeN + 1) + 1]; // offDiagA[1]; - - givens_rot(x, z, &cosArray[0], &lenVal); - cosVec = vld1_f32((float32_t const *)cosArray); // VLD1.32 {d0}, [r0] - tempVec3 = vrev64_f32(cosVec); // VREV64.32 d0,d0 - tempVec3 = vneg_f32(tempVec3); - trnVec = vtrn_f32(tempVec3, cosVec); // VTRN.32 d0,d0 - cosVec2 = trnVec.val[0]; //[-sin cos] - - ldPtr = matA + offset; - for (jj = offset; jj < matSizeN; jj++) - { // apply Givens rotation on all columns - colVec0 = vld1_f32((float32_t const *)ldPtr); - resVec0 = vmul_lane_f32(cosVec, colVec0, 0); - resVec0 = vmla_lane_f32(resVec0, cosVec2, colVec0, 1); - vst1_f32((float32_t *)ldPtr, resVec0); - ldPtr += matSizeN; - } - - ldPtr = matA + offset * matSizeN; - for (jj = offset; jj < kk; jj += 2) - { // apply Givens rotation on all active rows - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec1 = vld1_f32((float32_t const *)(ldPtr + matSizeN)); - - resVec0 = vmul_lane_f32(colVec0, cosVec, 0); - resVec1 = vmul_lane_f32(colVec1, cosVec, 0); - - resVec0 = vmls_lane_f32(resVec0, colVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, colVec0, cosVec, 1); - - vst1_f32((float32_t *)ldPtr, resVec0); - vst1_f32((float32_t *)(ldPtr + matSizeN), resVec1); - ldPtr += 2; - } - if (jj == kk) - { // one left - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec1 = vld1_f32((float32_t const *)(ldPtr + matSizeN)); - - trnVec = vtrn_f32(colVec0, colVec1); // VTRN.32 d0,d0 - colVec0 = trnVec.val[0]; - resVec0 = vmul_lane_f32(cosVec, colVec0, 0); - resVec0 = vmla_lane_f32(resVec0, cosVec2, colVec0, 1); - ldPtr[0] = vget_lane_f32(resVec0, 0); - ldPtr[matSizeN] = vget_lane_f32(resVec0, 1); - } - if (kk > 1) - { - for (ii = offset + 1; ii < kk; ii++) - { - x = matA[(ii - 1) * (matSizeN + 1) + 1]; - z = matA[(ii - 1) * (matSizeN + 1) + 2]; - givens_rot(x, z, &cosArray[0], &lenVal); - - cosVec = vld1_f32((float32_t const *)cosArray); // VLD1.32 {d0}, [r0] - tempVec3 = vrev64_f32(cosVec); // VREV64.32 d0,d0 - tempVec3 = vneg_f32(tempVec3); - trnVec = vtrn_f32(tempVec3, cosVec); // VTRN.32 d0,d0 - cosVec2 = trnVec.val[0]; //[-sin cos] - - ldPtr = matA + ii + offset * matSizeN; - for (jj = offset; jj < matSizeN; jj++) - { // apply Givens rotation on all columns - colVec0 = vld1_f32((float32_t const *)ldPtr); - resVec0 = vmul_lane_f32(cosVec, colVec0, 0); - resVec0 = vmla_lane_f32(resVec0, cosVec2, colVec0, 1); - vst1_f32((float32_t *)ldPtr, resVec0); - ldPtr += matSizeN; - } - ldPtr = matA + offset + ii * matSizeN; - for (jj = offset; jj < kk; jj += 2) - { // apply Givens rotation on all active rows - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec1 = vld1_f32((float32_t const *)(ldPtr + matSizeN)); - - resVec0 = vmul_lane_f32(colVec0, cosVec, 0); - resVec1 = vmul_lane_f32(colVec1, cosVec, 0); - - resVec0 = vmls_lane_f32(resVec0, colVec1, cosVec, 1); - resVec1 = vmla_lane_f32(resVec1, colVec0, cosVec, 1); - - vst1_f32((float32_t *)ldPtr, resVec0); - vst1_f32((float32_t *)(ldPtr + matSizeN), resVec1); - ldPtr += 2; - } - if (jj == kk) - { // one left - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec1 = vld1_f32((float32_t const *)(ldPtr + matSizeN)); - - trnVec = vtrn_f32(colVec0, colVec1); // VTRN.32 d0,d0 - colVec0 = trnVec.val[0]; - resVec0 = vmul_lane_f32(cosVec, colVec0, 0); - resVec0 = vmla_lane_f32(resVec0, cosVec2, colVec0, 1); - ldPtr[0] = vget_lane_f32(resVec0, 0); - ldPtr[matSizeN] = vget_lane_f32(resVec0, 1); - } - } - } - - return 0; -#else - int ii, jj; - float a, b, c, d; - float bSqr, dSqr; - float mu, temp; - float x, z; - float cosVal, sinVal, lenVal; - float cosArray[2]; - float temp11, temp21; - - a = matA[(kk - 1) * (matSizeN + 1)]; // diagA[kk-1]; // second last diagonal - b = matA[(kk - 1) * (matSizeN + 1) + 1]; // offDiagA[kk]; - c = matA[kk * (matSizeN + 1)]; // diagA[kk]; // last diagonal - if (kk == 1 + offset) - { - float p, q; - d = matA[kk * (matSizeN + 1) - 1]; - - p = -a - c; - q = a * c - b * d; - if (p * p < 4 * q) - { - matA[offset * (matSizeN + 1)] = -p / 2; - matA[offset * (matSizeN + 1) + 1] = 0; - matA[(offset + 1) * (matSizeN + 1)] = -p / 2; - - return 1; - } - } - // else{ - d = (a - c) / 2; - //} - - bSqr = b * b; - dSqr = d * d; - temp = sqrtf(dSqr + bSqr); - temp = (d > 0) ? (d + temp) : (d - temp); - mu = c - bSqr / temp; - - // first iteration - x = matA[offset * (matSizeN + 1)] - mu; // diagA[0]-mu; - z = matA[offset * (matSizeN + 1) + 1]; // offDiagA[1]; - - givens_rot(x, z, &cosArray[0], &lenVal); - cosVal = cosArray[0]; - sinVal = cosArray[1]; - - for (jj = offset; jj < matSizeN; jj++) - { // apply Givens rotation on all columns - temp11 = matA[offset + jj * matSizeN]; - temp21 = matA[1 + offset + jj * matSizeN]; - - matA[offset + jj * matSizeN] = cosVal * temp11 - sinVal * temp21; - matA[1 + offset + jj * matSizeN] = cosVal * temp21 + sinVal * temp11; - } - - for (jj = offset; jj <= kk; jj++) - { // apply Givens rotation on all active rows - temp11 = matA[jj + offset * matSizeN]; - temp21 = matA[jj + (1 + offset) * matSizeN]; - - matA[jj + offset * matSizeN] = cosVal * temp11 - sinVal * temp21; - matA[jj + (1 + offset) * matSizeN] = cosVal * temp21 + sinVal * temp11; - } - if (kk > 1) - { - for (ii = offset + 1; ii < kk; ii++) - { - x = matA[(ii - 1) * (matSizeN + 1) + 1]; - z = matA[(ii - 1) * (matSizeN + 1) + 2]; - givens_rot(x, z, &cosArray[0], &lenVal); - cosVal = cosArray[0]; - sinVal = cosArray[1]; - - for (jj = offset; jj < matSizeN; jj++) - { // apply Givens rotation on all columns - temp11 = matA[ii + jj * matSizeN]; - temp21 = matA[(ii + 1) + jj * matSizeN]; - - matA[ii + jj * matSizeN] = cosVal * temp11 - sinVal * temp21; - matA[(ii + 1) + jj * matSizeN] = cosVal * temp21 + sinVal * temp11; - } - - for (jj = offset; jj <= kk; jj++) - { // apply Givens rotation on all active rows - temp11 = matA[jj + ii * matSizeN]; - temp21 = matA[jj + (ii + 1) * matSizeN]; - - matA[jj + ii * matSizeN] = cosVal * temp11 - sinVal * temp21; - matA[jj + (ii + 1) * matSizeN] = cosVal * temp21 + sinVal * temp11; - } - } - } - - return 0; -#endif -} - -// back substitution for LS via QR, solves R*X = Q'B = C for X, R is upper tri-angular -// R is NxM, X is MxM, C is NxM -void myBackSub(float *C_MATR, float *R_MATR, float *X_MAT, int matSizeN, int matSizeM) -{ -#ifdef DEBUG_OUTPUT - FILE *fpOut; -#endif - int ii, jj, kk, row_idx; - float tempVal; - - for (kk = 0; kk < matSizeM; kk++) - { - for (ii = 0; ii < matSizeM; ii++) - { - row_idx = matSizeM - 1 - ii; - - tempVal = C_MATR[row_idx + kk * MAX_MAT_SIZE]; - for (jj = matSizeM - ii; jj < matSizeM; jj++) - { - tempVal -= R_MATR[row_idx + jj * MAX_MAT_SIZE] * X_MAT[jj + kk * matSizeM]; - } - X_MAT[row_idx + kk * matSizeM] = tempVal / R_MATR[(MAX_MAT_SIZE + 1) * row_idx]; - } - } -#ifdef DEBUG_OUTPUT - fpOut = fopen("Psi_hat_C.txt", "w"); - if (fpOut == NULL) - { - fprintf(stderr, "Cannot open output file %s for writing\n", "Psi_hat_C.txt"); - return; - } - - for (ii = 0; ii < matSizeM * matSizeM; ii++) - { - fprintf(fpOut, "%f\n", X_MAT[ii]); - } - fclose(fpOut); -#endif -} - -// regular QR decomposition -void QR_decomposition(float *inMatArr, float *resD, int matSizeN, int matSizeM) -{ -#ifdef ARM_DS5 - int ii, jj, kk, hh_len; - float *xVec; - float beta, xNorm; - - float *R_MATR = inMatArr; - float *Qmat = resD; // Qmat[matSizeN*matSizeN] - float *vVec = Qmat + MAX_MAT_SIZE * matSizeN; // vVec[matSizeN] - float *qVec = vVec + MAX_MAT_SIZE; // qVec[matSizeN] - float *pVec = qVec + MAX_MAT_SIZE; // pVec[matSizeM] - - float *ldPtr, *strPtr; - - float32x2_t storeVec, resVec0, resVec1; - float32x2_t colVec0, colVec1, pVecf32, qVecf32, vVecf32, sumVec; - - storeVec = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - - // initialize Q - strPtr = Qmat; - for (ii = 0; ii < matSizeN; ii++) - { - for (jj = 0; jj < MAX_MAT_SIZE; jj += 2) - { - vst1_f32((float32_t *)strPtr, storeVec); // VST1.32 {d0}, [r0] - strPtr += 2; - } - strPtr[ii - MAX_MAT_SIZE] = 1.0f; - } - - // Householder Tridiagonalization - hh_len = matSizeN; - xVec = R_MATR; - for (kk = 0; kk < matSizeM; kk++) - { // A=V - hh_len = matSizeN - kk; - xVec = R_MATR + kk * (MAX_MAT_SIZE + 1); - - if (!house_holder_vec(xVec, vVec, &beta, &xNorm, hh_len)) - { - for (ii = hh_len; ii < MAX_MAT_SIZE; ii++) - { - vVec[ii] = 0; - } - // apply house holder iteration - pVec[0] = 0; - for (ii = 1; ii < matSizeM - kk; ii += 2) - { // calc. p = beta*A'*v - resVec0 = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - resVec1 = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - ldPtr = R_MATR + (ii + kk) * MAX_MAT_SIZE + kk; - vVecf32 = vld1_f32((float32_t const *)vVec); // VLD1.32 {d0}, [r0] - for (jj = 0; jj < hh_len; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - resVec0 = vmla_f32(resVec0, colVec0, vVecf32); - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); // VLD1.32 {d0}, [r0] - ldPtr += 2; - resVec1 = vmla_f32(resVec1, colVec1, vVecf32); - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); // VLD1.32 {d0}, [r0] - } - sumVec = vpadd_f32(resVec0, resVec1); - sumVec = vmul_n_f32(sumVec, beta); // VMUL.F32 d0,d0,d0[0] - vst1_f32((float32_t *)(pVec + ii), sumVec); // VST1.32 {d0}, [r0] - } - pVec[matSizeM - kk] = 0; - - strPtr = R_MATR + kk * (MAX_MAT_SIZE + 1); - *strPtr++ = xNorm; - for (ii = 1; ii < hh_len; ii++) - { - *strPtr++ = 0; - } - // R(index_col,index_row) = R(index_col,index_row)-v*p'; - for (ii = 1; ii < matSizeM - kk; ii += 2) - { - pVecf32 = vld1_f32((float32_t const *)(pVec + ii)); // VLD1.32 {d0}, [r0] - ldPtr = R_MATR + (ii + kk) * MAX_MAT_SIZE + kk; - vVecf32 = vld1_f32((float32_t const *)vVec); // VLD1.32 {d0}, [r0] - for (jj = 0; jj < hh_len; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - colVec0 = vmls_lane_f32(colVec0, vVecf32, pVecf32, 0); - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); // VLD1.32 {d0}, [r0] - vst1_f32((float32_t *)ldPtr, colVec0); - colVec1 = vmls_lane_f32(colVec1, vVecf32, pVecf32, 1); - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), colVec1); - ldPtr += 2; - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); // VLD1.32 {d0}, [r0] - } - } - - // update Q - for (ii = 0; ii < matSizeN; ii += 2) - { // calc. q= beta*Q(:,index_col)*v; - sumVec = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - ldPtr = Qmat + ii + kk * MAX_MAT_SIZE; - vVecf32 = vld1_f32((float32_t const *)vVec); // VLD1.32 {d0}, [r0] - for (jj = 0; jj < hh_len; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += MAX_MAT_SIZE; - sumVec = vmla_lane_f32(sumVec, colVec0, vVecf32, 0); - colVec1 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += MAX_MAT_SIZE; - sumVec = vmla_lane_f32(sumVec, colVec1, vVecf32, 1); - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); // VLD1.32 {d0}, [r0] - } - sumVec = vmul_n_f32(sumVec, beta); // VMUL.F32 d0,d0,d0[0] - vst1_f32((float32_t *)(qVec + ii), sumVec); // VST1.32 {d0}, [r0] - } - qVec[matSizeN] = 0; - // Q(:,index_col) = Q(:,index_col)-q*v'; - for (ii = 0; ii < matSizeN; ii += 2) - { - qVecf32 = vld1_f32((float32_t const *)(qVec + ii)); - ldPtr = Qmat + ii + kk * MAX_MAT_SIZE; - vVecf32 = vld1_f32((float32_t const *)vVec); // VLD1.32 {d0}, [r0] - for (jj = 0; jj < hh_len; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - colVec0 = vmls_lane_f32(colVec0, qVecf32, vVecf32, 0); // VMLS.F32 d0, - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); // VLD1.32 {d0}, [r0] - colVec1 = vmls_lane_f32(colVec1, qVecf32, vVecf32, 1); // VMLS.F32 d0, - vst1_f32((float32_t *)ldPtr, colVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), colVec1); // VST1.32 {d0}, [r0] - ldPtr += 2 * MAX_MAT_SIZE; - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); // VLD1.32 {d0}, [r0] - } - } - } - hh_len--; - xVec += MAX_MAT_SIZE + 1; - } -#else - int ii, jj, kk, hh_len; - float *xVec; - float beta, xNorm, sum; - float tempVal; - - float *R_MATR = inMatArr; - float *Qmat = resD; // Qmat[matSizeN*matSizeN] - float *vVec = Qmat + MAX_MAT_SIZE * matSizeN; // vVec[matSizeN] - float *qVec = vVec + matSizeN; // qVec[matSizeN] - float *pVec = qVec + matSizeN; // pVec[matSizeM] - - // initialize Q - for (ii = 0; ii < matSizeN; ii++) - { - for (jj = 0; jj < matSizeN; jj++) - { - Qmat[jj + MAX_MAT_SIZE * ii] = 0.0f; - } - Qmat[ii + MAX_MAT_SIZE * ii] = 1.0f; - } - - // Householder Tridiagonalization - for (kk = 0; kk < matSizeM; kk++) - { // A=V - hh_len = matSizeN - kk; - xVec = R_MATR + kk * (MAX_MAT_SIZE + 1); - - if (!house_holder_vec(xVec, vVec, &beta, &xNorm, hh_len)) - { - // apply house holder iteration - pVec[0] = 0; - for (ii = 1; ii < matSizeM - kk; ii++) - { // calc. p = beta*A'*v - sum = 0; - for (jj = 0; jj < hh_len; jj++) - { - sum += R_MATR[(ii + kk) * MAX_MAT_SIZE + (jj + kk)] * vVec[jj]; - } - pVec[ii] = beta * sum; - } - R_MATR[kk * (MAX_MAT_SIZE + 1)] = xNorm; - for (ii = 1; ii < hh_len; ii++) - { - R_MATR[(ii + kk) + kk * MAX_MAT_SIZE] = 0; - } - // R(index_col,index_row) = R(index_col,index_row)-v*p'; - for (ii = 1; ii < matSizeM - kk; ii++) - { - tempVal = pVec[ii]; - for (jj = 0; jj < hh_len; jj++) - { - R_MATR[(ii + kk) * MAX_MAT_SIZE + (jj + kk)] -= vVec[jj] * tempVal; - } - } - - // update Q - for (ii = 0; ii < matSizeN; ii++) - { // calc. q= beta*Q(:,index_col)*v; - sum = 0; - for (jj = 0; jj < hh_len; jj++) - { - sum += Qmat[ii + (jj + kk) * MAX_MAT_SIZE] * vVec[jj]; - } - qVec[ii] = beta * sum; - } - // Q(:,index_col) = Q(:,index_col)-q*v'; - for (ii = 0; ii < matSizeN; ii++) - { - tempVal = qVec[ii]; - for (jj = 0; jj < hh_len; jj++) - { - Qmat[ii + (jj + kk) * MAX_MAT_SIZE] -= vVec[jj] * tempVal; - } - } - } - } -#endif -} - -// symmetric eigen(Shur) decomposition -int QR_algorithm(float *inMatArr, float *resD, int matSizeN, int low_accuracy) -{ - int kk, ii, jj, count, delta_count; - int N_hh_it; - - float *xVec; - float beta, sum, prod, xNorm; - int startIdx, zeroIdx = 0; - unsigned char indexArr[MAX_MAT_SIZE]; - float step; - - float *eigVals = resD; - float *Qmat = eigVals + MAX_MAT_SIZE; // vVec[matSizeN*matSizeN] - - float *vVec = Qmat + MAX_MAT_SIZE * MAX_MAT_SIZE; // vVec[matSizeN] - float *wVec = vVec + MAX_MAT_SIZE; // wVec[matSizeN] - float *pVec = wVec + MAX_MAT_SIZE; // pVec[matSizeN] - - float *diagA = vVec; // diagA[matSizeN] - float *offDiagA = wVec; // offDiagA[matSizeN] - -#ifdef ARM_DS5 - float *ldPtr, *strPtr, *strPtrTr; - float32x2_t storeVec; - float32x2x2_t storeVec2; - float32x2_t colVec0, colVec1, pVecf32, vVecf32, wVecf32, vVecfTr, wVecfTr, sumVec; - - step = (low_accuracy == 1) ? TOL_QR_STEP_LOW : TOL_QR_STEP; - storeVec = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - - // initialize Q - strPtr = Qmat; - for (ii = 0; ii < MAX_MAT_SIZE; ii++) - { - for (jj = 0; jj < MAX_MAT_SIZE; jj += 2) - { - vst1_f32((float32_t *)strPtr, storeVec); // VST1.32 {d0}, [r0] - strPtr += 2; - } - strPtr[ii - MAX_MAT_SIZE] = 1.0f; - } - - // Householder Tridiagonalization - xVec = inMatArr + 1; - N_hh_it = matSizeN - 1; - for (kk = 1; kk < matSizeN - 1; kk++) - { - if (!house_holder_vec(xVec, vVec, &beta, &xNorm, N_hh_it)) - { // calculate house holder vector - for (ii = N_hh_it; ii < MAX_MAT_SIZE; ii++) - { - vVec[ii] = 0; - } - // apply house holder iteration - for (ii = 0; ii < N_hh_it; ii += 2) - { // calc. p = beta*A*v and q = beta*Q'*v; - sumVec = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - ldPtr = inMatArr + (kk * MAX_MAT_SIZE + (ii + kk)); - for (jj = 0; jj < N_hh_it; jj += 2) - { - vVecf32 = vld1_f32((float32_t const *)(vVec + jj)); // VLD1.32 {d0}, [r0] - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += MAX_MAT_SIZE; - sumVec = vmla_lane_f32(sumVec, colVec0, vVecf32, 0); // VMLA.F32 d0, d0, d0[0] - - colVec1 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += MAX_MAT_SIZE; - sumVec = vmla_lane_f32(sumVec, colVec1, vVecf32, 1); // VMLA.F32 d0, d0, d0[0] - } - sumVec = vmul_n_f32(sumVec, beta); // VMUL.F32 d0,d0,d0[0] - vst1_f32((float32_t *)(pVec + ii), sumVec); // VST1.32 {d0}, [r0] - } - pVec[N_hh_it] = 0; - - sumVec = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - for (jj = 0; jj < N_hh_it; jj += 2) - { // calc. (beta*p'*v/2) - pVecf32 = vld1_f32((float32_t const *)(pVec + jj)); // VLD1.32 {d0}, [r0] - vVecf32 = vld1_f32((float32_t const *)(vVec + jj)); // VLD1.32 {d0}, [r0] - sumVec = vmla_f32(sumVec, pVecf32, vVecf32); // VMLA.F32 d0,d0,d0 - } - sumVec = vpadd_f32(sumVec, sumVec); // VPADD.F32 d0,d0,d0 - sum = vget_lane_f32(sumVec, 0); // VMOV.32 r0, d0[0] - prod = beta * sum / 2; - - for (ii = 0; ii < N_hh_it; ii += 2) - { // calc. w = p-(beta*p'*v/2)*v; - vVecf32 = vld1_f32((float32_t const *)(vVec + ii)); // VLD1.32 {d0}, [r0] - pVecf32 = vld1_f32((float32_t const *)(pVec + ii)); // VLD1.32 {d0}, [r0] - pVecf32 = vmls_n_f32(pVecf32, vVecf32, prod); // VMLS.F32 d0, d0, d0[0] - vst1_f32((float32_t *)(wVec + ii), pVecf32); // VST1.32 {d0}, [r0] - } - - strPtr = inMatArr + ((kk)*MAX_MAT_SIZE + (kk - 1)); - strPtrTr = strPtr - (MAX_MAT_SIZE - 1); // inMatArr + ((kk-1)*matSizeN+(kk)); - *strPtr = xNorm; - strPtr += MAX_MAT_SIZE; - *strPtrTr++ = xNorm; - for (ii = 1; ii < N_hh_it; ii++) - { - *strPtr = 0; - strPtr += MAX_MAT_SIZE; - *strPtrTr++ = 0; - } - - // calc A(index_kk,index_kk)-v*w'-w*v'; - for (ii = 0; ii < N_hh_it; ii += 2) - { - ldPtr = inMatArr + (ii + kk) * MAX_MAT_SIZE + (ii + kk); - vVecf32 = vld1_f32((float32_t const *)(vVec + ii)); // VLD1.32 {d0}, [r0] - wVecf32 = vld1_f32((float32_t const *)(wVec + ii)); // VLD1.32 {d0}, [r0] - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); // VLD1.32 {d0}, [r0] - colVec0 = vmls_lane_f32(colVec0, vVecf32, wVecf32, 0); // VMLS.F32 d0, - colVec1 = vmls_lane_f32(colVec1, vVecf32, wVecf32, 1); // VMLS.F32 d0, - colVec0 = vmls_lane_f32(colVec0, wVecf32, vVecf32, 0); // VMLS.F32 d0, - colVec1 = vmls_lane_f32(colVec1, wVecf32, vVecf32, 1); // VMLS.F32 d0, - vst1_f32((float32_t *)ldPtr, colVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), colVec1); // VST1.32 {d0}, [r0] - - ldPtr -= ii; // ldPtr = inMatArr+ (ii+kk)*matSizeN + kk; - strPtr = inMatArr + kk * MAX_MAT_SIZE + (ii + kk); - for (jj = 0; jj < ii; jj += 2) - { - wVecfTr = vld1_f32((float32_t const *)(wVec + jj)); // VLD1.32 {d0}, [r0] - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); // VLD1.32 {d0}, [r0] - colVec0 = vmls_lane_f32(colVec0, wVecfTr, vVecf32, 0); // VMLS.F32 d0, - colVec1 = vmls_lane_f32(colVec1, wVecfTr, vVecf32, 1); // VMLS.F32 d0, - vVecfTr = vld1_f32((float32_t const *)(vVec + jj)); // VLD1.32 {d0}, [r0] - colVec0 = vmls_lane_f32(colVec0, vVecfTr, wVecf32, 0); // VMLS.F32 d0, - colVec1 = vmls_lane_f32(colVec1, vVecfTr, wVecf32, 1); // VMLS.F32 d0, - vst1_f32((float32_t *)ldPtr, colVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), colVec1); // VST1.32 {d0}, [r0] - ldPtr += 2; - storeVec2 = vtrn_f32(colVec0, colVec1); // VTRN.32 d0,d0 - vst1_f32((float32_t *)strPtr, storeVec2.val[0]); // VST1.32 {d0}, [r0] - strPtr += MAX_MAT_SIZE; - vst1_f32((float32_t *)strPtr, storeVec2.val[1]); // VST1.32 {d0}, [r0] - strPtr += MAX_MAT_SIZE; - } - } - - // update Q - for (ii = 0; ii < matSizeN; ii += 2) - { // calc. p = beta*QQ0(index_kk,:)'*v; - sumVec = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - ldPtr = Qmat + kk * MAX_MAT_SIZE + ii; - for (jj = 0; jj < N_hh_it; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += MAX_MAT_SIZE; - colVec1 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += MAX_MAT_SIZE; - vVecf32 = vld1_f32((float32_t const *)(vVec + jj)); // VLD1.32 {d0}, [r0] - sumVec = vmla_lane_f32(sumVec, colVec0, vVecf32, 0); - sumVec = vmla_lane_f32(sumVec, colVec1, vVecf32, 1); - } - sumVec = vmul_n_f32(sumVec, beta); // VMUL.F32 d0,d0,d0[0] - vst1_f32((float32_t *)(pVec + ii), sumVec); // VST1.32 {d0}, [r0] - } - // Q0(index_kk,:)-v*p'; - for (ii = 0; ii < N_hh_it; ii += 2) - { - vVecf32 = vld1_f32((float32_t const *)(vVec + ii)); // VLD1.32 {d0}, [r0] - ldPtr = Qmat + (ii + kk) * MAX_MAT_SIZE; - for (jj = 0; jj < matSizeN; jj += 2) - { - pVecf32 = vld1_f32((float32_t const *)(pVec + jj)); // VLD1.32 {d0}, [r0] - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - colVec1 = vld1_f32((float32_t const *)(ldPtr + MAX_MAT_SIZE)); // VLD1.32 {d0}, [r0] - colVec0 = vmls_lane_f32(colVec0, pVecf32, vVecf32, 0); // VMLS.F32 d0, - colVec1 = vmls_lane_f32(colVec1, pVecf32, vVecf32, 1); // VMLS.F32 d0, - - vst1_f32((float32_t *)ldPtr, colVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(ldPtr + MAX_MAT_SIZE), colVec1); // VST1.32 {d0}, [r0] - ldPtr += 2; - } - } - } - xVec += MAX_MAT_SIZE + 1; - N_hh_it--; - } -#else - float temp, sum1, vTemp; - - if (low_accuracy == 1) - step = TOL_QR_STEP_LOW; - else - step = TOL_QR_STEP; - - // initialize Q - for (ii = 0; ii < MAX_MAT_SIZE; ii++) - { - for (jj = 0; jj < MAX_MAT_SIZE; jj++) - { - Qmat[jj + MAX_MAT_SIZE * ii] = 0.0f; - } - Qmat[ii + MAX_MAT_SIZE * ii] = 1.0f; - vVec[ii] = 0; - } - - // Householder Tridiagonalization - for (kk = 1; kk < matSizeN - 1; kk++) - { - N_hh_it = matSizeN - kk; - xVec = inMatArr + kk + (kk - 1) * MAX_MAT_SIZE; - // calculate house holder vector - if (!house_holder_vec(xVec, vVec, &beta, &xNorm, N_hh_it)) - { - // apply house holder iteration - for (ii = 0; ii < N_hh_it; ii += 2) - { // calc. p = beta*A*v and q = beta*Q'*v; - sum = 0; - sum1 = 0; - for (jj = 0; jj < N_hh_it; jj++) - { - vTemp = vVec[jj]; - temp = inMatArr[(jj + kk) * MAX_MAT_SIZE + (ii + kk)]; - sum += temp * vTemp; - temp = inMatArr[(jj + kk) * MAX_MAT_SIZE + (ii + 1 + kk)]; - sum1 += temp * vTemp; - } - pVec[ii] = beta * sum; - pVec[ii + 1] = beta * sum1; - } - sum = 0; // calc. (beta*p'*v/2) - for (jj = 0; jj < N_hh_it; jj++) - { - sum += pVec[jj] * vVec[jj]; - } - prod = beta * sum / 2; - for (ii = 0; ii < N_hh_it; ii++) - { // calc. w = p-(beta*p'*v/2)*v; - wVec[ii] = pVec[ii] - prod * vVec[ii]; - } - inMatArr[(kk)*MAX_MAT_SIZE + (kk - 1)] = xNorm; - inMatArr[(kk - 1) * MAX_MAT_SIZE + (kk)] = xNorm; - for (ii = 1; ii < N_hh_it; ii++) - { - inMatArr[(ii + kk) * MAX_MAT_SIZE + (kk - 1)] = 0; - inMatArr[(kk - 1) * MAX_MAT_SIZE + (ii + kk)] = 0; - } - // calc A(index_kk,index_kk)-v*w'-w*v'; - for (ii = 0; ii < N_hh_it; ii++) - { - sum = vVec[ii] * wVec[ii] + wVec[ii] * vVec[ii]; - inMatArr[(ii + kk) * MAX_MAT_SIZE + (ii + kk)] -= sum; - for (jj = 0; jj < ii; jj++) - { - sum = vVec[ii] * wVec[jj] + wVec[ii] * vVec[jj]; - temp = inMatArr[(ii + kk) * MAX_MAT_SIZE + (jj + kk)]; - temp -= sum; - inMatArr[(ii + kk) * MAX_MAT_SIZE + (jj + kk)] = temp; - inMatArr[(jj + kk) * MAX_MAT_SIZE + (ii + kk)] = temp; - } - } - // update Q - for (ii = N_hh_it; ii < matSizeN; ii++) - { - vVec[ii] = 0; - } - for (ii = 0; ii < matSizeN; ii += 2) - { // calc. p = beta*QQ0(index_kk,:)'*v; - sum = 0; - sum1 = 0; - for (jj = 0; jj < N_hh_it; jj++) - { - sum += Qmat[(jj + kk) * MAX_MAT_SIZE + ii] * vVec[jj]; - sum1 += Qmat[(jj + kk) * MAX_MAT_SIZE + ii + 1] * vVec[jj]; - } - pVec[ii] = beta * sum; - pVec[ii + 1] = beta * sum1; - } - // Q0(index_kk,:)-v*p'; - for (ii = 0; ii < N_hh_it; ii++) - { - for (jj = 0; jj < matSizeN; jj++) - { - Qmat[(ii + kk) * MAX_MAT_SIZE + jj] -= vVec[ii] * pVec[jj]; - } - } - } - } -#endif - // Implicit Symmetric QR Step with Wilkinson Shift - diagA[0] = inMatArr[0]; // copy tridiagonal D - offDiagA[0] = 0; - indexArr[0] = 0; - for (ii = 1; ii < matSizeN; ii++) - { - diagA[ii] = inMatArr[ii * (MAX_MAT_SIZE + 1)]; - offDiagA[ii] = inMatArr[ii * (MAX_MAT_SIZE + 1) - 1]; - if (FABSF(offDiagA[ii]) < step) - { // find decoupled blocks - zeroIdx++; - indexArr[zeroIdx] = ii; - } - } - count = 0; - - startIdx = indexArr[zeroIdx--]; - for (kk = matSizeN - 1; kk > 0; kk--) - { - if (kk > startIdx) - { - delta_count = 0; - while ((FABSF(offDiagA[kk]) > step * (FABSF(diagA[kk - 1]) + FABSF(diagA[kk]))) && - (delta_count < WILK_IT_MAX)) - { - QR_step_Wilkinson(diagA + startIdx, offDiagA + startIdx, Qmat + startIdx * MAX_MAT_SIZE, matSizeN, - kk - startIdx); - delta_count++; - } - count += delta_count; - if (kk - startIdx > 2) - { - zeroIdx++; - // check for newly diagonalized blocks - for (ii = startIdx + 1; ii < kk; ii++) - { - if (FABSF(offDiagA[ii]) < step) - { // find decoupled blocks - zeroIdx++; - indexArr[zeroIdx] = ii; - } - } - startIdx = indexArr[zeroIdx--]; - } - } - else - { - if (startIdx > 0) - { - startIdx = indexArr[zeroIdx--]; - } - } - } - for (ii = 0; ii < matSizeN; ii++) - { - eigVals[ii] = diagA[ii]; - } - - return 0; -} - -// unsymmetric eigen(Shur) decomposition, only solves for eigen values -int unsym_QR_algorithm(float *inMatArr, float *resD, int matSizeN) -{ - int kk, ii, jj, count, delta_count; - int N_hh_it; - float *xVec; - float beta, sum, xNorm, tempVal; - int startIdx, zeroIdx = 0; - unsigned char indexArr[MAX_MAT_SIZE]; - - float *eigVals = resD; - float *vVec = eigVals + MAX_MAT_SIZE; // vVec[matSizeN] - float *pVec0 = vVec + MAX_MAT_SIZE; // pVec0[matSizeN] - float *pVec1 = pVec0 + MAX_MAT_SIZE; // pVec1[matSizeN] - -#ifdef ARM_DS5 - float *ldPtr, *strPtr; - float32x2_t resVec0, resVec1; - float32x2_t colVec0, colVec1, pVecf32, vVecf32, sumVec; - - // Householder Tridiagonalization - N_hh_it = matSizeN - 1; - xVec = inMatArr + 1; - for (kk = 1; kk < matSizeN - 1; kk++) - { - // calculate house holder vector - if (!house_holder_vec(xVec, vVec, &beta, &xNorm, N_hh_it)) - { - vVec[N_hh_it] = 0; - - // apply house holder iteration - for (ii = 0; ii < N_hh_it; ii += 2) - { // calc. p0 = beta*A(index_kk,index_kk)'*v - resVec0 = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - resVec1 = vdup_n_f32(0.0f); // VDUP.32 d0,r0 - ldPtr = inMatArr + kk + (ii + kk) * matSizeN; - vVecf32 = vld1_f32((float32_t const *)vVec); // VLD1.32 {d0}, [r0] - for (jj = 0; jj < N_hh_it; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - resVec0 = vmla_f32(resVec0, colVec0, vVecf32); - colVec1 = vld1_f32((float32_t const *)(ldPtr + matSizeN)); // VLD1.32 {d0}, [r0] - resVec1 = vmla_f32(resVec1, colVec1, vVecf32); - ldPtr += 2; - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); // VLD1.32 {d0}, [r0] - } - sumVec = vpadd_f32(resVec0, resVec1); - sumVec = vmul_n_f32(sumVec, beta); // VMUL.F32 d0,d0,d0[0] - vst1_f32((float32_t *)(pVec0 + ii), sumVec); // VST1.32 {d0}, [r0] - } - pVec0[N_hh_it] = 0; - strPtr = inMatArr + (kk) + (kk - 1) * matSizeN; - *strPtr++ = xNorm; - for (ii = 1; ii < N_hh_it; ii++) - { - *strPtr++ = 0; - } - // calc A(index_kk,index_kk)-v*p0' - for (ii = 0; ii < N_hh_it; ii += 2) - { - pVecf32 = vld1_f32((float32_t const *)(pVec0 + ii)); // VLD1.32 {d0}, [r0] - vVecf32 = vld1_f32((float32_t const *)vVec); // VLD1.32 {d0}, [r0] - for (jj = 0; jj < N_hh_it; jj += 2) - { - colVec0 = vld1_f32( - (float32_t const *)(inMatArr + (jj + kk) + (ii + kk) * matSizeN)); // VLD1.32 {d0}, [r0] - colVec0 = vmls_lane_f32(colVec0, vVecf32, pVecf32, 0); // VMLS.F32 d0, - colVec1 = vld1_f32( - (float32_t const *)(inMatArr + (jj + kk) + (ii + 1 + kk) * matSizeN)); // VLD1.32 {d0}, [r0] - colVec1 = vmls_lane_f32(colVec1, vVecf32, pVecf32, 1); // VMLS.F32 d0, - vst1_f32((float32_t *)(inMatArr + (jj + kk) + (ii + kk) * matSizeN), colVec0); // VST1.32 {d0}, [r0] - vst1_f32((float32_t *)(inMatArr + (jj + kk) + (ii + 1 + kk) * matSizeN), - colVec1); // VST1.32 {d0}, [r0] - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); // VLD1.32 {d0}, [r0] - } - } - // second update - for (ii = 0; ii < matSizeN; ii += 2) - { // calc. p1 = beta*A(:,index_kk)*v - sumVec = vdup_n_f32(0.0f); - ldPtr = inMatArr + ii + kk * matSizeN; - vVecf32 = vld1_f32((float32_t const *)vVec); // VLD1.32 {d0}, [r0] - for (jj = 0; jj < N_hh_it; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += matSizeN; - sumVec = vmla_lane_f32(sumVec, colVec0, vVecf32, 0); - colVec1 = vld1_f32((float32_t const *)ldPtr); // VLD1.32 {d0}, [r0] - ldPtr += matSizeN; - sumVec = vmla_lane_f32(sumVec, colVec1, vVecf32, 1); - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); // VLD1.32 {d0}, [r0] - } - sumVec = vmul_n_f32(sumVec, beta); // VMUL.F32 d0,d0,d0[0] - vst1_f32((float32_t *)(pVec1 + ii), sumVec); - } - pVec1[matSizeN] = 0; - - // calc A(:,index_kk) -p1*v' - for (ii = 0; ii < matSizeN; ii += 2) - { - pVecf32 = vld1_f32((float32_t const *)pVec1 + ii); - ldPtr = inMatArr + ii + kk * matSizeN; - vVecf32 = vld1_f32((float32_t const *)vVec); - for (jj = 0; jj < N_hh_it; jj += 2) - { - colVec0 = vld1_f32((float32_t const *)ldPtr); - colVec0 = vmls_lane_f32(colVec0, pVecf32, vVecf32, 0); // VMLS.F32 d0, - colVec1 = vld1_f32((float32_t const *)(ldPtr + matSizeN)); - colVec1 = vmls_lane_f32(colVec1, pVecf32, vVecf32, 1); // VMLS.F32 d0, - vst1_f32((float32_t *)ldPtr, colVec0); - ldPtr += matSizeN; - vst1_f32((float32_t *)ldPtr, colVec1); - ldPtr += matSizeN; - vVecf32 = vld1_f32((float32_t const *)(vVec + jj + 2)); - } - } - } - N_hh_it--; - xVec += matSizeN + 1; - } -#else - // Householder Tridiagonalization - for (kk = 1; kk < matSizeN - 1; kk++) - { - N_hh_it = matSizeN - kk; - xVec = inMatArr + kk + (kk - 1) * matSizeN; - // calculate house holder vector - if (!house_holder_vec(xVec, vVec, &beta, &xNorm, N_hh_it)) - { - // apply house holder iteration - for (ii = 0; ii < N_hh_it; ii++) - { // calc. p0 = beta*A(index_kk,index_kk)'*v - sum = 0; - for (jj = 0; jj < N_hh_it; jj++) - { - sum += inMatArr[(jj + kk) + (ii + kk) * matSizeN] * vVec[jj]; - } - pVec0[ii] = beta * sum; - } - inMatArr[(kk) + (kk - 1) * matSizeN] = xNorm; - for (ii = 1; ii < N_hh_it; ii++) - { - inMatArr[(ii + kk) + (kk - 1) * matSizeN] = 0; - } - // calc A(index_kk,index_kk)-v*p0' - for (ii = 0; ii < N_hh_it; ii++) - { - for (jj = 0; jj < N_hh_it; jj++) - { - sum = vVec[jj] * pVec0[ii]; - inMatArr[(jj + kk) + (ii + kk) * matSizeN] -= sum; - } - } - // second update - for (ii = 0; ii < matSizeN; ii++) - { // calc. p1 = beta*A(:,index_kk)*v - sum = 0; - for (jj = 0; jj < N_hh_it; jj++) - { - sum += inMatArr[ii + (jj + kk) * matSizeN] * vVec[jj]; - } - pVec1[ii] = beta * sum; - } - // calc A(:,index_kk) -p1*v' - for (ii = 0; ii < matSizeN; ii++) - { - for (jj = 0; jj < N_hh_it; jj++) - { - sum = pVec1[ii] * vVec[jj]; - inMatArr[ii + (jj + kk) * matSizeN] -= sum; - } - } - } - } -#endif - - // Implicit Symmetric QR Step with Wilkinson Shift - indexArr[0] = 0; - for (ii = 1; ii < matSizeN; ii++) - { - tempVal = FABSF(inMatArr[(ii - 1) * (matSizeN + 1) + 1]); - sum = FABSF(inMatArr[(ii - 1) * (matSizeN + 1)]) + FABSF(inMatArr[ii * (matSizeN + 1)]); - if (tempVal < TOL_QR_STEP * sum) - { // find decoupled blocks - zeroIdx++; - indexArr[zeroIdx] = ii; - } - } - count = 0; - - startIdx = indexArr[zeroIdx--]; - for (kk = matSizeN - 1; kk > 0; kk--) - { - if (kk > startIdx) - { - tempVal = FABSF(inMatArr[(kk - 1) * (matSizeN + 1) + 1]); - sum = FABSF(inMatArr[(kk - 1) * (matSizeN + 1)]) + FABSF(inMatArr[kk * (matSizeN + 1)]); - delta_count = 0; - while ((tempVal > TOL_QR_STEP * sum) && (delta_count < WILK_IT_MAX)) - { - // while((tempVal > TOL_QR_STEP * sum)){ - unsym_QR_step_Wilkinson(inMatArr, matSizeN, kk, startIdx); - delta_count++; - tempVal = FABSF(inMatArr[(kk - 1) * (matSizeN + 1) + 1]); - sum = FABSF(inMatArr[(kk - 1) * (matSizeN + 1)]) + FABSF(inMatArr[kk * (matSizeN + 1)]); - } - count += delta_count; - - if (kk > 2) - { - // find newly decoupled blocks - indexArr[0] = 0; - zeroIdx = 0; - for (ii = 1; ii < kk; ii++) - { - tempVal = FABSF(inMatArr[(ii - 1) * (matSizeN + 1) + 1]); - sum = FABSF(inMatArr[(ii - 1) * (matSizeN + 1)]) + FABSF(inMatArr[ii * (matSizeN + 1)]); - if (tempVal < TOL_QR_STEP * sum) - { // find decoupled blocks - zeroIdx++; - indexArr[zeroIdx] = ii; - } - } - startIdx = indexArr[zeroIdx--]; - } - } - else - { - if (startIdx > 0) - { - startIdx = indexArr[zeroIdx--]; - } - } - } - for (ii = 0; ii < matSizeN; ii++) - { - eigVals[ii] = inMatArr[ii * (matSizeN + 1)]; - } - - return 0; -} - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_QR_algorithm.h b/mcux/middleware/wifi_nxp/wls/wls_QR_algorithm.h deleted file mode 100644 index adcd3bcd09..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_QR_algorithm.h +++ /dev/null @@ -1,43 +0,0 @@ -#if CONFIG_WLS_CSI_PROC -/** @file wls_QR_algorithm.h - * - * @brief This file contains header for QR math functions - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * DFW header for QR math functions - ************************************************************************/ - -#ifndef WLS_QR_ALGORITHM_H -#define WLS_QR_ALGORITHM_H - -// MAX_MAT_SIZE needs to be >=2*SIG_SUBSP_DIM_MAX -#define MAX_MAT_SIZE 16 - -#ifdef ARM_DS5 -#define SQRTF(x) __sqrt(x) -#define FABSF(x) __fabsf(x) -#else -#define SQRTF(x) sqrtf(x) -#define FABSF(x) fabsf(x) -#endif - -// eigen(Shur) decomposition for symmetric matrix, returns eigen vectors in Q -int QR_algorithm(float *inMatArr, float *resD, int matSizeN, int low_accuracy); - -// eigen(Shur) decomposition for unsymmetric matrix, no Q -int unsym_QR_algorithm(float *inMatArr, float *resD, int matSizeN); - -// solves LS using QR -void QR_decomposition(float *inMatArr, float *resD, int matSizeN, int matSizeM); - -void myBackSub(float *Q_MATR, float *R_MATR, float *MAT_OUT, int matSizeN, int matSizeM); - -#endif - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_api.c b/mcux/middleware/wifi_nxp/wls/wls_api.c deleted file mode 100644 index 1695ce1953..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_api.c +++ /dev/null @@ -1,288 +0,0 @@ -/** @file wls_api.c - * - * @brief This file contains source code for CSI processing API. - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * DFW source code for CSI processing API. - ************************************************************************/ - -#include -#if CONFIG_WLS_CSI_PROC - -// Standard includes. -// #include -// #include -#include - -// Specific includes. -#include "wls_param_defines.h" -#include "wls_structure_defs.h" - -#include "wls_radix4Fft.h" -#ifdef ENABLE_AOA -#include "wls_aoa_processing.h" -#endif -#ifdef ARM_DS5 -#include "wls_processing_Neon_Intrinsic.h" -#endif - -#include "wls_processing.h" -#ifdef FFT_PARALLEL -#include "wls_processing_parallel.h" -#endif -#ifdef ENABLE_SUBSPACE_FTIMING -#include "wls_subspace_processing.h" -#endif - -#ifdef DEBUG_OUT_FD -void saveDebug(unsigned int *writePointer, unsigned int *readPointer, int size) -{ - int ii; - - for (ii = 0; ii < size; ii++) - { - writePointer[ii] = readPointer[ii]; - } -} -#endif - -int wls_process_csi( - unsigned int *bufferMemory, // CSI buffer - unsigned int *fftBuffer, // 2k times (MAX_RX*MAX_TX+1) scratch memory - hal_wls_packet_params_t *packetparams, // values from Rx/Tx-Info, used mostly to find correct CSI buffer - hal_wls_processing_input_params_t *inputVals, // WLS / AoA CSI Processing Parameters Structure - unsigned int *resArray) // outputs � phase roll, first path, pktinfo, difference of rxInfo CSI TSF counters -{ // weight1, weight2, angle, delay (16 bits each) for AoA processing per Peak - - int csiDataSize; - int fftSize, ifftSizeOsf, firstPathDelay, maxIdx; - unsigned int maxVal; - unsigned int *fftInBuffer, *pdpOut; - unsigned int powerPerSubband[4 * MAX_RX * MAX_TX]; // max num. of 40 MHz subbands - unsigned int totalpower[MAX_RX * MAX_TX + 1]; - int phaseRollNg[MAX_RX * MAX_TX]; - unsigned int headerBuffer[HEADER_LEN]; - - int header_length, start_csi = 0; - unsigned int tempVec[2] = {0, 0}; -#ifdef ENABLE_AOA - hal_cal_struc_t calData; -#endif - hal_csirxinfo_t *csirxinfo; - hal_pktinfo_t *pktinfo; - - memcpy(&headerBuffer, bufferMemory, HEADER_LEN * sizeof(unsigned int)); - - if (!(inputVals->enableCsi)) - { - return -1; - } - -#if defined(DEBUG_CYCLE_TIME) - DEBUG_DELTA_TIME_US1 = ((UINT64)HAL_REGS32(0x8000a604) << 32) + HAL_REGS32(0x8000a600); -#endif - - csirxinfo = (hal_csirxinfo_t *)headerBuffer; - - tempVec[0] = (unsigned int)csirxinfo->pktinfo; - pktinfo = (hal_pktinfo_t *)tempVec; - - if (pktinfo->packetType < 3) - { // HT, legacy - if (pktinfo->packetType == 0) - { // legacy - pktinfo->sigBw = pktinfo->rxDevBw; - } - - if (pktinfo->sigBw) - { // bw > 20 MHz - pktinfo->NgDsfShift = pktinfo->Ng + 1; - } - else - { // 20 MHz - pktinfo->NgDsfShift = 0; - } - } - else - { // VHT, HE - pktinfo->NgDsfShift = 0; - // pktinfo->Ng = 0; // not used - } - -#ifdef SMAC_BFINFO - header_length = csirxinfo->header_length; - csiDataSize = bufferMemory[header_length] - 1; - start_csi = header_length + 1; -#else - header_length = HEADER_LEN; - start_csi = header_length; - if (csirxinfo->ltf) - { - start_csi += bufferMemory[header_length]; - } - csiDataSize = bufferMemory[start_csi] - 1; - start_csi++; -#endif - - fftSize = pktinfo->sigBw + IFFT_OSF_SHIFT - pktinfo->NgDsfShift; - pktinfo->fftSize = (fftSize < MAX_IFFT_SIZE_SHIFT) ? fftSize : MAX_IFFT_SIZE_SHIFT; - pktinfo->scOffset = 0; - ifftSizeOsf = 1 << (pktinfo->fftSize + 6); -#if defined(FFT_INPLACE) - fftInBuffer = fftBuffer; -#else - fftInBuffer = fftBuffer + NUM_PARALLEL * ifftSizeOsf; -#endif - // expand data from 8->16 bit, demodulate pilots for L-LTF, measure power and linear phase - if (pktinfo->packetType > 2) - { // VHT+HE (Ng=1) -#ifdef FFT_PARALLEL - readHexDataDemodulateProcessVhtHeNg1Parallel(pktinfo, inputVals, bufferMemory + start_csi, csiDataSize, - fftInBuffer, powerPerSubband, phaseRollNg, packetparams->chNum); -#else - readHexDataDemodulateProcessVhtHeNg1(pktinfo, inputVals, bufferMemory + start_csi, csiDataSize, fftInBuffer, - powerPerSubband, phaseRollNg, packetparams->chNum); -#endif - if (pktinfo->sigBw == 3) // 160 MHz only - detectPhaseJump(pktinfo, inputVals, fftInBuffer, phaseRollNg); - } - else - { // Legacy, HT, Ng=2/4 -#ifdef FFT_PARALLEL - readHexDataDemodulateProcessParallel(pktinfo, inputVals, bufferMemory + start_csi, csiDataSize, fftInBuffer, - powerPerSubband, phaseRollNg, packetparams->chNum); -#else - readHexDataDemodulateProcess(pktinfo, inputVals, bufferMemory + start_csi, csiDataSize, fftInBuffer, - powerPerSubband, phaseRollNg, packetparams->chNum); -#endif - } - if (pktinfo->packetType == 0) - { // in case of legacy packets, check active subbands && (packetparams->ftmSignalBWsigBw) - findActiveSubbands(pktinfo, powerPerSubband, totalpower, packetparams->chNum, packetparams->ftmSignalBW); - zeroOutTones(pktinfo, fftInBuffer, ifftSizeOsf); - } - else - { - calculateTotalPower(pktinfo, powerPerSubband, totalpower); - } -#ifndef STA_20_ONLY - if (((pktinfo->packetType < 4) && (pktinfo->sigBw > 0)) || (pktinfo->rxDevBw == 3)) - { // not for HE and BW > 20 MHz -#if defined(FFT_PARALLEL) && defined(ARM_DS5) - removeToneRotationIntrinsic(pktinfo, fftInBuffer, ifftSizeOsf); -#elif defined(FFT_PARALLEL) && !defined(ARM_DS5) - removeToneRotationParallel(pktinfo, fftInBuffer, ifftSizeOsf); -#else - removeToneRotation(pktinfo, fftInBuffer, ifftSizeOsf); -#endif - } - if ((pktinfo->packetType == 0) && (pktinfo->rxDevBw > 0)) - { // all legacy except full interpolation for 20in20 - processLegacyPackets(pktinfo, fftInBuffer, ifftSizeOsf, phaseRollNg); - } -#endif - if ((pktinfo->packetType > 2) || ((pktinfo->packetType == 1) && (pktinfo->Ng == 0)) // add HT20, HT40 case, Ng=2 - || ((pktinfo->packetType == 0) && (pktinfo->sigBw == 0) && (pktinfo->rxDevBw == 0))) // add Leg20 case, DevBw=0 - { -#if defined(FFT_PARALLEL) && defined(ARM_DS5) - interpolatePilotsIntrinsic(pktinfo, fftInBuffer, ifftSizeOsf, phaseRollNg, totalpower); -#elif defined(FFT_PARALLEL) && !defined(ARM_DS5) - interpolatePilotsParallel(pktinfo, fftInBuffer, ifftSizeOsf, phaseRollNg, totalpower); -#else - interpolatePilots(pktinfo, fftInBuffer, ifftSizeOsf, phaseRollNg, totalpower); -#endif - } -#if defined(DEBUG_OUT_FD) && !defined(ARM_DS5) - saveDebug(bufferMemory, fftInBuffer + 0 * ifftSizeOsf, ifftSizeOsf); -#endif - // ifft processing - ifftProcessing(pktinfo, fftInBuffer, fftBuffer, ifftSizeOsf); // ifftSizeOsf might not match pktinfo->sigBw - - // update - fftSize = (1 << (pktinfo->sigBw + 6)); - ifftSizeOsf = 1 << (pktinfo->fftSize + 6); - pktinfo->rsvd1 = 0; - - resArray[2] = (unsigned int)tempVec[0]; // = pktinfo - resArray[3] = (unsigned int)tempVec[1]; // = pktinfo - -#if defined(DEBUG_OUT_TD) && !defined(ARM_DS5) - saveDebug(bufferMemory + data_length, fftBuffer + 0 * ifftSizeOsf, NUM_PARALLEL * ifftSizeOsf); -#endif - // determine first path - pdpOut = fftBuffer + (MAX_RX * MAX_TX) * ifftSizeOsf; // last buffer - - if (inputVals->useToaMin == 1) - { - calcPdpAndFirstPathMin(pktinfo, fftBuffer, pdpOut, totalpower, &maxIdx, &maxVal, &firstPathDelay); - } - else - { -#if defined(FFT_PARALLEL) && defined(ARM_DS5) - calcPdpAndMaxIntrinsic(pktinfo, fftBuffer, pdpOut, totalpower, &maxIdx, &maxVal); -#else - calcPdpAndMaxParallel(pktinfo, fftBuffer, pdpOut, totalpower, &maxIdx, &maxVal); -#endif - firstPathDelay = findFirstPath(pktinfo, pdpOut, maxIdx, maxVal, 1); - } - if (pktinfo->packetType > 2) - { // Ng=1 case - resArray[0] = ((phaseRollNg[0] * fftSize << 2) / 5) >> (pktinfo->sigBw); - } - else - { // Ng=2/4 case - resArray[0] = ((phaseRollNg[0] * fftSize << 1) / 5) >> (pktinfo->sigBw + pktinfo->Ng); - } -#ifdef ENABLE_SUBSPACE_FTIMING - { - int fineTimingRes, retVal = 1; - if (inputVals->useSubspace == 1) - { - retVal = calcSubspaceFineTiming(pktinfo, fftBuffer, totalpower, firstPathDelay, &fineTimingRes, pdpOut, - packetparams); - } - if (retVal) - { // error or not uses, use first path - fineTimingRes = firstPathDelay; - } - resArray[1] = ((fineTimingRes << (14 - TOA_FPATH_BIPT)) / 5) >> (pktinfo->fftSize + pktinfo->NgDsfShift); - } -#else - // final format is 32.TOA_FPATH_BIPT in micro seconds - resArray[1] = ((firstPathDelay << (14 - TOA_FPATH_BIPT)) / 5) >> (pktinfo->fftSize + pktinfo->NgDsfShift); -#endif -#ifdef ENABLE_AOA - if (inputVals->enableAoA && pktinfo->nRx) - { // at least 2 Rx paths - readCalDataNew(&calData, packetparams, pktinfo, inputVals); - resArray[4] = maxVal >> 15; - if (inputVals->useFindAngleDelayPeaks && (pktinfo->nRx > 1)) - { // at least 3 Rx paths - findAngleDelayPeaks(pktinfo, packetparams, inputVals, &calData, fftBuffer, pdpOut, totalpower, maxIdx, - resArray + 4); - } - else - { - findAngleLinPhase(pktinfo, packetparams, &calData, fftBuffer, totalpower, firstPathDelay, resArray + 5); - } - } - if (inputVals->dumpRawAngle) - { - dumpRawComplex(pktinfo, fftBuffer, firstPathDelay, resArray + 8); - } -#endif -#if defined(DEBUG_CYCLE_TIME) - DEBUG_DELTA_TIME_US2 = ((UINT64)HAL_REGS32(0x8000a604) << 32) + HAL_REGS32(0x8000a600); - DEBUG_DELTA_TIME_US = DEBUG_DELTA_TIME_US2 - DEBUG_DELTA_TIME_US1; -#endif - - return 0; -} - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_api.h b/mcux/middleware/wifi_nxp/wls/wls_api.h deleted file mode 100644 index 25a370ca86..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_api.h +++ /dev/null @@ -1,31 +0,0 @@ -#if CONFIG_WLS_CSI_PROC -/** @file wls_api.h - * - * @brief This file contains header file for WLS processing fixed-point version API - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * Header file for WLS processing fixed-point version API - ************************************************************************/ -#ifndef WLS_API_H -#define WLS_API_H - -#include "wls_structure_defs.h" -#ifdef DFW_CSI_PROC -#include "dsp_cmd.h" -#endif - -int wls_process_csi(unsigned int *bufferMemory, - unsigned int *fftInBuffer, - hal_wls_packet_params_t *packetparams, - hal_wls_processing_input_params_t *inputVals, - unsigned int *resArray); - -#endif - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_processing.c b/mcux/middleware/wifi_nxp/wls/wls_processing.c deleted file mode 100644 index ed0c61b82b..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_processing.c +++ /dev/null @@ -1,2738 +0,0 @@ -/** @file wls_processing.c - * - * @brief This file contains processing functions to calculate CSI correction for WLS time stamps - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ -* DFW processing functions to calculate CSI correction for WLS time stamps -************************************************************************/ - -#include -#if CONFIG_WLS_CSI_PROC - -// Standard includes. -#include -#include -//#include - -#include "wls_radix4Fft.h" -#include "wls_structure_defs.h" -#include "wls_processing.h" -#ifdef FFT_PARALLEL -#include "wls_processing_parallel.h" -#endif -#ifdef ARM_DS5 -#include "wls_processing_Neon_Intrinsic.h" -#endif -#include "wls_param_defines.h" - -#ifdef FLOATING_POINT -#include -#endif - -#define NUM_ATAN_IT 10 - -//#define ONE_OVER_PI ((int)((1<>(30-MPY_BIPT)) //0x145F306D=1.0/PI*(1<<30) -#define TABLE_SIZE_POW 6 -#define TABLE_SIZE (1<>IIR_FORMAT_NP)&(MAX_FFT_SIZE-1)) // adjust index format to twiddle table -#ifdef STA_20_ONLY -const short phiCorr64[64] = {P(-2560), P(-2049), P(-1731), P(-1527), P(-1376), P(-1254), P(-1151), P(-1061), P(-981), P(-910), P(-845), P(-785), P(-730), P(-679), P(-631), P(-586), P(-543), P(-502), P(-462), P(-424), P(-388), P(-352), P(-318), P(-284), P(-251), P(-218), P(-186), P(-155), P(-123), P(-92), P(-61), P(-31), P(0), P(31), P(61), P(92), P(123), P(155), P(186), P(218), P(251), P(284), P(318), P(352), P(388), P(424), P(462), P(502), P(543), P(586), P(631), P(679), P(730), P(785), P(845), P(910), P(981), P(1061), P(1151), P(1254), P(1376), P(1527), P(1731), P(2049)}; -#else -const short phiCorr512[512] ={P(-2560), P(-2488), P(-2417), P(-2348), P(-2281), P(-2218), P(-2158), P(-2101), P(-2049), P(-1999), P(-1953), P(-1910), P(-1870), P(-1832), P(-1796), P(-1763), P(-1731), P(-1701), P(-1673), P(-1646), P(-1620), P(-1595), P(-1572), P(-1549), P(-1527), P(-1506), P(-1486), P(-1466), P(-1447), P(-1429), P(-1411), P(-1393), P(-1376), P(-1360), P(-1344), P(-1328), P(-1313), P(-1297), P(-1283), P(-1268), P(-1254), P(-1241), P(-1227), P(-1214), P(-1201), P(-1188), P(-1176), P(-1163), P(-1151), P(-1139), P(-1128), P(-1116), P(-1105), P(-1094), P(-1083), P(-1072), P(-1061), P(-1051), P(-1040), P(-1030), P(-1020), P(-1010), P(-1001), P(-991), P(-981), P(-972), P(-963), P(-954), P(-945), P(-936), P(-927), P(-918), P(-910), P(-901), P(-893), P(-885), P(-877), P(-869), P(-861), P(-853), P(-845), P(-837), P(-829), P(-822), P(-814), P(-807), P(-800), P(-792), P(-785), P(-778), P(-771), P(-764), P(-757), P(-750), P(-744), P(-737), P(-730), P(-724), P(-717), P(-711), P(-704), P(-698), P(-691), P(-685), P(-679), P(-673), P(-667), P(-661), P(-655), P(-649), P(-643), P(-637), P(-631), P(-625), P(-619), P(-614), P(-608), P(-602), P(-597), P(-591), P(-586), P(-580), P(-575), P(-569), P(-564), P(-558), P(-553), P(-548), P(-543), P(-537), P(-532), P(-527), P(-522), P(-517), P(-512), P(-507), P(-502), P(-497), P(-492), P(-487), P(-482), P(-477), P(-472), P(-467), P(-462), P(-457), P(-453), P(-448), P(-443), P(-438), P(-434), P(-429), P(-424), P(-420), P(-415), P(-410), P(-406), P(-401), P(-397), P(-392), P(-388), P(-383), P(-379), P(-374), P(-370), P(-365), P(-361), P(-357), P(-352), P(-348), P(-343), P(-339), P(-335), P(-330), P(-326), P(-322), P(-318), P(-313), P(-309), P(-305), P(-301), P(-296), P(-292), P(-288), P(-284), P(-280), P(-276), P(-271), P(-267), P(-263), P(-259), P(-255), P(-251), P(-247), P(-243), P(-238), P(-234), P(-230), P(-226), P(-222), P(-218), P(-214), P(-210), P(-206), P(-202), P(-198), P(-194), P(-190), P(-186), P(-182), P(-178), P(-174), P(-170), P(-166), P(-162), P(-159), P(-155), P(-151), P(-147), P(-143), P(-139), P(-135), P(-131), P(-127), P(-123), P(-119), P(-116), P(-112), P(-108), P(-104), P(-100), P(-96), P(-92), P(-88), P(-85), P(-81), P(-77), P(-73), P(-69), P(-65), P(-61), P(-58), P(-54), P(-50), P(-46), P(-42), P(-38), P(-35), P(-31), P(-27), P(-23), P(-19), P(-15), P(-11), P(-8), P(-4), P(0), P(4), P(8), P(11), P(15), P(19), P(23), P(27), P(31), P(35), P(38), P(42), P(46), P(50), P(54), P(58), P(61), P(65), P(69), P(73), P(77), P(81), P(85), P(88), P(92), P(96), P(100), P(104), P(108), P(112), P(116), P(119), P(123), P(127), P(131), P(135), P(139), P(143), P(147), P(151), P(155), P(159), P(162), P(166), P(170), P(174), P(178), P(182), P(186), P(190), P(194), P(198), P(202), P(206), P(210), P(214), P(218), P(222), P(226), P(230), P(234), P(238), P(243), P(247), P(251), P(255), P(259), P(263), P(267), P(271), P(276), P(280), P(284), P(288), P(292), P(296), P(301), P(305), P(309), P(313), P(318), P(322), P(326), P(330), P(335), P(339), P(343), P(348), P(352), P(357), P(361), P(365), P(370), P(374), P(379), P(383), P(388), P(392), P(397), P(401), P(406), P(410), P(415), P(420), P(424), P(429), P(434), P(438), P(443), P(448), P(453), P(457), P(462), P(467), P(472), P(477), P(482), P(487), P(492), P(497), P(502), P(507), P(512), P(517), P(522), P(527), P(532), P(537), P(543), P(548), P(553), P(558), P(564), P(569), P(575), P(580), P(586), P(591), P(597), P(602), P(608), P(614), P(619), P(625), P(631), P(637), P(643), P(649), P(655), P(661), P(667), P(673), P(679), P(685), P(691), P(698), P(704), P(711), P(717), P(724), P(730), P(737), P(744), P(750), P(757), P(764), P(771), P(778), P(785), P(792), P(800), P(807), P(814), P(822), P(829), P(837), P(845), P(853), P(861), P(869), P(877), P(885), P(893), P(901), P(910), P(918), P(927), P(936), P(945), P(954), P(963), P(972), P(981), P(991), P(1001), P(1010), P(1020), P(1030), P(1040), P(1051), P(1061), P(1072), P(1083), P(1094), P(1105), P(1116), P(1128), P(1139), P(1151), P(1163), P(1176), P(1188), P(1201), P(1214), P(1227), P(1241), P(1254), P(1268), P(1283), P(1297), P(1313), P(1328), P(1344), P(1360), P(1376), P(1393), P(1411), P(1429), P(1447), P(1466), P(1486), P(1506), P(1527), P(1549), P(1572), P(1595), P(1620), P(1646), P(1673), P(1701), P(1731), P(1763), P(1796), P(1832), P(1870), P(1910), P(1953), P(1999), P(2049), P(2101), P(2158), P(2218), P(2281), P(2348), P(2417), P(2488)}; -const short phiCorrScBt512[512] ={P(-3584), P(-3243), P(-2979), P(-2789), P(-2648), P(-2534), P(-2440), P(-2358), P(-2286), P(-2220), P(-2161), P(-2106), P(-2056), P(-2009), P(-1965), P(-1924), P(-1886), P(-1850), P(-1815), P(-1783), P(-1752), P(-1722), P(-1694), P(-1667), P(-1642), P(-1617), P(-1593), P(-1570), P(-1548), P(-1526), P(-1506), P(-1485), P(-1466), P(-1447), P(-1429), P(-1411), P(-1393), P(-1376), P(-1360), P(-1343), P(-1328), P(-1312), P(-1297), P(-1282), P(-1268), P(-1253), P(-1240), P(-1226), P(-1213), P(-1199), P(-1187), P(-1174), P(-1161), P(-1149), P(-1137), P(-1125), P(-1114), P(-1102), P(-1091), P(-1080), P(-1069), P(-1059), P(-1048), P(-1038), P(-1027), P(-1017), P(-1007), P(-997), P(-988), P(-978), P(-969), P(-959), P(-950), P(-941), P(-932), P(-923), P(-915), P(-906), P(-898), P(-889), P(-881), P(-873), P(-864), P(-856), P(-848), P(-841), P(-833), P(-825), P(-817), P(-810), P(-802), P(-795), P(-788), P(-780), P(-773), P(-766), P(-759), P(-752), P(-745), P(-738), P(-732), P(-725), P(-718), P(-712), P(-705), P(-699), P(-692), P(-686), P(-679), P(-673), P(-667), P(-661), P(-655), P(-648), P(-642), P(-636), P(-630), P(-625), P(-619), P(-613), P(-607), P(-601), P(-596), P(-590), P(-584), P(-579), P(-573), P(-568), P(-562), P(-557), P(-551), P(-546), P(-540), P(-535), P(-530), P(-524), P(-519), P(-514), P(-509), P(-504), P(-498), P(-493), P(-488), P(-483), P(-478), P(-473), P(-468), P(-463), P(-458), P(-453), P(-449), P(-444), P(-439), P(-434), P(-429), P(-424), P(-420), P(-415), P(-410), P(-405), P(-401), P(-396), P(-391), P(-387), P(-382), P(-378), P(-373), P(-368), P(-364), P(-359), P(-355), P(-350), P(-346), P(-341), P(-337), P(-333), P(-328), P(-324), P(-319), P(-315), P(-311), P(-306), P(-302), P(-297), P(-293), P(-289), P(-284), P(-280), P(-276), P(-272), P(-267), P(-263), P(-259), P(-255), P(-250), P(-246), P(-242), P(-238), P(-234), P(-229), P(-225), P(-221), P(-217), P(-213), P(-209), P(-205), P(-200), P(-196), P(-192), P(-188), P(-184), P(-180), P(-176), P(-172), P(-168), P(-164), P(-159), P(-155), P(-151), P(-147), P(-143), P(-139), P(-135), P(-131), P(-127), P(-123), P(-119), P(-115), P(-111), P(-107), P(-103), P(-99), P(-95), P(-91), P(-87), P(-83), P(-79), P(-75), P(-71), P(-67), P(-63), P(-59), P(-55), P(-51), P(-47), P(-44), P(-40), P(-36), P(-32), P(-28), P(-24), P(-20), P(-16), P(-12), P(-8), P(-4), P(0), P(4), P(8), P(12), P(16), P(20), P(24), P(28), P(32), P(36), P(40), P(44), P(47), P(51), P(55), P(59), P(63), P(67), P(71), P(75), P(79), P(83), P(87), P(91), P(95), P(99), P(103), P(107), P(111), P(115), P(119), P(123), P(127), P(131), P(135), P(139), P(143), P(147), P(151), P(155), P(159), P(164), P(168), P(172), P(176), P(180), P(184), P(188), P(192), P(196), P(200), P(205), P(209), P(213), P(217), P(221), P(225), P(229), P(234), P(238), P(242), P(246), P(250), P(255), P(259), P(263), P(267), P(272), P(276), P(280), P(284), P(289), P(293), P(297), P(302), P(306), P(311), P(315), P(319), P(324), P(328), P(333), P(337), P(341), P(346), P(350), P(355), P(359), P(364), P(368), P(373), P(378), P(382), P(387), P(391), P(396), P(401), P(405), P(410), P(415), P(420), P(424), P(429), P(434), P(439), P(444), P(449), P(453), P(458), P(463), P(468), P(473), P(478), P(483), P(488), P(493), P(498), P(504), P(509), P(514), P(519), P(524), P(530), P(535), P(540), P(546), P(551), P(557), P(562), P(568), P(573), P(579), P(584), P(590), P(596), P(601), P(607), P(613), P(619), P(625), P(630), P(636), P(642), P(648), P(655), P(661), P(667), P(673), P(679), P(686), P(692), P(699), P(705), P(712), P(718), P(725), P(732), P(738), P(745), P(752), P(759), P(766), P(773), P(780), P(788), P(795), P(802), P(810), P(817), P(825), P(833), P(841), P(848), P(856), P(864), P(873), P(881), P(889), P(898), P(906), P(915), P(923), P(932), P(941), P(950), P(959), P(969), P(978), P(988), P(997), P(1007), P(1017), P(1027), P(1038), P(1048), P(1059), P(1069), P(1080), P(1091), P(1102), P(1114), P(1125), P(1137), P(1149), P(1161), P(1174), P(1187), P(1199), P(1213), P(1226), P(1240), P(1253), P(1268), P(1282), P(1297), P(1312), P(1328), P(1343), P(1360), P(1376), P(1393), P(1411), P(1429), P(1447), P(1466), P(1485), P(1506), P(1526), P(1548), P(1570), P(1593), P(1617), P(1642), P(1667), P(1694), P(1722), P(1752), P(1783), P(1815), P(1850), P(1886), P(1924), P(1965), P(2009), P(2056), P(2106), P(2161), P(2220), P(2286), P(2358), P(2440), P(2534), P(2648), P(2789), P(2979), P(3243)}; -#endif -#endif - -// input is in 16p14 -#define ONE_16P14 (1<<14) -int myAsin(int x){ - - int numerator, denominator, y, z; - - if(x >= ONE_16P14){ - return (1<<(MPY_BIPT-2)); // PI/2 - } - else if(-x >= ONE_16P14){ - return -(1<<(MPY_BIPT-2)); // -PI/2 - } - else if(x==0){ - return 0; - } - else{ - y = ONE_16P14 -((x*x)>>14); - denominator = mySqrt(y); - numerator = (x<<7); - z = numerator/denominator; - return myAtan2(ONE_16P14, z); - } -} - - -int myAtan2(int valI, int valQ){ - - int x, y_old, y_new; - short cosValInt, sinValInt; - int idx, zerosI, zerosQ; - int ii, quadrant = 0; - unsigned int tempVal; - unsigned int *cosTablePtr = (unsigned int*)radix4FftTwiddleArr; - - if(!valI || !valQ){ - if(!valI && !valQ){ // undefined - return 0; - } - else if(!valI){ - if(valQ>0) // PI/2 - return (1<<(MPY_BIPT-2)); - else // -PI/2 - return -(1<<(MPY_BIPT-2)); - } - else{ // valQ == 0 - if(valI>0) // 0 - return 0; - else // PI - return (1<<(MPY_BIPT-1)); - } - } - - // determine quadrant, change sign - if(valQ<0){ - valQ = -valQ; - quadrant+=2; - } - if(valI<0){ - valI=-valI; - quadrant++; - } - //x = 1.0f*valQ/valI; - zerosQ = __clz(valQ); - valQ<<=(zerosQ-1); - ii = zerosQ-1; - zerosI = __clz(valI); - if(zerosI<16){ // get at least 16 bit precision - valI>>=(16-zerosI); - ii += (16-zerosI); - } - x = valQ/valI; - if(MPY_BIPT>ii) - x<<=(MPY_BIPT-ii); - else - x>>=(ii-MPY_BIPT); - - // initialize - if(x < 3<<(MPY_BIPT-2)) // 0.75 - y_old = (x*ONE_OVER_PI)>>(MPY_BIPT+1); //x/(2*PI); - else if(x < 4<>2; - y_old += 3<<(MPY_BIPT-2); //(0.25f*(x-1)+0.75f)/(2*PI); - y_old*=ONE_OVER_PI; - y_old>>=MPY_BIPT+1; - } - else - y_old = (15<<(MPY_BIPT-6)); // approx of (1.5f/(2*PI))<>(MPY_BIPT-1); -#ifdef TWIDDLE_HALF_SIZE - tempVal = cosTablePtr[idx&(MAX_FFT_SIZE/2-1)]; - if(idx&(MAX_FFT_SIZE/2)){ // [-pi,0) - cosValInt = -(short) (tempVal&0xffff); - sinValInt = -(short) (tempVal>>16); - } - else{ // [0, pi) - cosValInt = (short) (tempVal&0xffff); - sinValInt = (short) (tempVal>>16); - } -#else - tempVal = cosTablePtr[idx&(MAX_FFT_SIZE-1)]; // apply modulo - cosValInt = (short) (tempVal&0xffff); - sinValInt = (short) (tempVal>>16); -#endif - y_new = (x*(cosValInt+(1<>TWIDDLE_BIPT; - y_new -=sinValInt>>(TWIDDLE_BIPT-MPY_BIPT); - y_new *=ONE_OVER_PI; - y_new >>=(MPY_BIPT+2); - y_new +=y_old; - - if(y_new>(1<<(MPY_BIPT-2))-1) - y_new = (1<<(MPY_BIPT-2))-8; - - y_old = y_new; - } - - switch(quadrant){ - case 1: - return -y_new+(1<<(MPY_BIPT-1)); - case 2: - return -y_new; - case 3: - return y_new-(1<<(MPY_BIPT-1)); //-PI - default: - return y_new; - } -} - -unsigned int mySqrtLut(int x){ - unsigned int y = 0; - int zerosX, shift, shiftBack, idx, offset; - unsigned int y1; - - if(x>0){ // return 0 for non-positive values - zerosX = __clz(x); - shift = 32-TABLE_SIZE_POW-zerosX; - shiftBack = (shift+1)>>1; - if(shiftBack>0){ - idx = x>>(2*shiftBack); - offset = x-(idx<<(2*shiftBack)); - y = sqrtLUT[idx-1]+1; - y *=(1<<(2*shiftBack))-offset; - y1 = sqrtLUT[idx]+1; - y1 *=offset; - y+=y1; - shiftBack -=(2*shiftBack)+MY_SQRT_BIPT-(TABLE_SIZE_POW>>1); - } - else{ - idx = x<<(-2*shiftBack); - y = sqrtLUT[idx-1]+1; - shiftBack -=MY_SQRT_BIPT-(TABLE_SIZE_POW>>1); - } - if(shiftBack>0){ - y<<=shiftBack; - } - else{ - y>>=(-shiftBack); - } - } - return y; -} - -#ifndef ARM_DS5 -unsigned int mySqrt(int x){ - int ii, digits, N; - unsigned int bit, temp, res = 0; - unsigned num = (x>0)? x:(-x); - - digits = 32-__clz(num); - digits>>=1; - N = digits+1; - digits<<=1; - bit = 1<>=1; - if(num>temp){ - num-=temp; - res +=bit; - } - bit>>=2; - } - return res; -} -#endif - -// general function: unpacks complex subcarriers, demodulates pilots, and estimates phase-roll and power per subband -void processSubCarrier(unsigned short *dataPtrTmp, short *writePtr, unsigned int pilotVals, const short* phiCorrPtr, int numCsiScHlf, int dataPtrStride, int phiCorrPtrStride, unsigned int *myStack){ - int ii; - -#if defined(ARM_GCC) || defined(ARM_DS5) -#ifdef ARM_DS5 - int reg6, reg7, reg8; -#endif -#ifndef REMOVE_IIR - unsigned int tempVal = myStack[0]; - unsigned int powerEst = myStack[1]; - int runningSumI = myStack[2]; - int runningSumQ = myStack[3]; -#endif -#else - signed char tempI, tempQ, pilotSym; - unsigned int tempVal; - unsigned short utemp16; - short lastValI, lastValQ; - short tempValI = (myStack[0])&0xffff; - short tempValQ = ((myStack[0])>>16)&0xffff; - unsigned int powerEst = myStack[1]; - int runningSumI = myStack[2]; - int runningSumQ = myStack[3]; -#ifdef REMOVE_IIR - int idx; - short cosPhi, sinPhi; - int resI, resQ; -#endif -#endif - - for(ii=0;ii>8); - pilotSym = 2*(pilotVals&1)-1; pilotVals>>=1; - lastValI = tempValI; - lastValQ = tempValQ; - tempValI = (short)tempI*pilotSym; - tempValQ = (short)tempQ*pilotSym; - powerEst += tempValI*tempValI + tempValQ*tempValQ; -#ifdef REMOVE_IIR - idx = *phiCorrPtr; phiCorrPtr+=phiCorrPtrStride; - cosPhi = radix4FftTwiddleArr[2*idx]; - sinPhi = radix4FftTwiddleArr[2*idx+1]; - resI = tempValI*cosPhi-tempValQ*sinPhi; - resQ = tempValI*sinPhi+tempValQ*cosPhi; - tempValI = (short)(resI>>(TWIDDLE_BIPT-8)); - tempValQ = (short)(resQ>>(TWIDDLE_BIPT-8)); - runningSumI += ((lastValI*tempValI) + (lastValQ*tempValQ))>>12; - runningSumQ += ((lastValI*tempValQ) - (lastValQ*tempValI))>>12; - writePtr[0] = tempValI; - writePtr[1] = tempValQ; -#else - runningSumI += ((lastValI*tempValI) + (lastValQ*tempValQ)); - runningSumQ += ((lastValI*tempValQ) - (lastValQ*tempValI)); - writePtr[0] = tempValI<<8; - writePtr[1] = tempValQ<<8; -#endif - writePtr+=2; -#endif - } - -#if defined(ARM_GCC) || defined(ARM_DS5) -#ifndef REMOVE_IIR - myStack[0] = tempVal; - myStack[1] = powerEst; - myStack[2] = runningSumI; - myStack[3] = runningSumQ; -#endif -#else - tempVal = ((unsigned int)tempValQ)<<16; - myStack[0] = (((unsigned int)tempValI)&0xffff) | tempVal; - myStack[1] = powerEst; - myStack[2] = runningSumI; - myStack[3] = runningSumQ; -#endif -} - - -// separate function due to non-uniform spacing of subcarriers; keep zeros between subcarriers to avoid distortion -// FFT size is larger, but not significant due to 20 MHz case -void processSubCarrier20MHz(unsigned short *dataPtrTmp, short *writePtr, unsigned int pilotVals, const short* phiCorrPtr, int numCsiScHlf, int dataPtrStride, int phiCorrPtrStride, unsigned char Ng, unsigned char jumpIdx, unsigned int *myStack){ - int ii; -#if defined(ARM_GCC) || defined(ARM_DS5) - -#ifdef ARM_DS5 - int reg6, reg7, reg8; -#endif -#ifndef REMOVE_IIR - unsigned int tempVal = myStack[0]; - unsigned int powerEst = myStack[1]; - int runningSumI = myStack[2]; - int runningSumQ = myStack[3]; -#endif - -#else // not ARM - - signed char tempI, tempQ, pilotSym; - unsigned int tempVal; - unsigned short utemp16; - short lastValI, lastValQ; - short tempValI = (myStack[0])&0xffff; - short tempValQ = ((myStack[0])>>16)&0xffff; - unsigned int powerEst = myStack[1]; - int runningSumI = myStack[2]; - int runningSumQ = myStack[3]; -#ifdef REMOVE_IIR - int idx; - short cosPhi, sinPhi; - int resI, resQ; -#endif - -#endif - - for(ii=0;ii>8); - pilotSym = 2*(pilotVals&1)-1; pilotVals>>=1; - lastValI = tempValI; - lastValQ = tempValQ; - tempValI = (short)tempI*pilotSym; - tempValQ = (short)tempQ*pilotSym; - powerEst += tempValI*tempValI + tempValQ*tempValQ; -#ifdef REMOVE_IIR - idx = *phiCorrPtr; phiCorrPtr+=phiCorrPtrStride; - cosPhi = radix4FftTwiddleArr[2*idx]; - sinPhi = radix4FftTwiddleArr[2*idx+1]; - resI = tempValI*cosPhi-tempValQ*sinPhi; - resQ = tempValI*sinPhi+tempValQ*cosPhi; - tempValI = (short)(resI>>(TWIDDLE_BIPT-8)); - tempValQ = (short)(resQ>>(TWIDDLE_BIPT-8)); -#else - tempValI<<=8; - tempValQ<<=8; -#endif - runningSumI += ((lastValI*tempValI) + (lastValQ*tempValQ))>>12; - runningSumQ += ((lastValI*tempValQ) - (lastValQ*tempValI))>>12; - writePtr[0] = tempValI; - writePtr[1] = tempValQ; - writePtr+=2*Ng; -#endif - if(ii == jumpIdx){ - writePtr-=2; -#ifdef REMOVE_IIR - phiCorrPtr-=phiCorrPtrStride/Ng; -#endif - } - } -#if defined(ARM_GCC) || defined(ARM_DS5) -#ifndef REMOVE_IIR - myStack[0] = tempVal; - myStack[1] = powerEst; - myStack[2] = runningSumI; - myStack[3] = runningSumQ; -#endif -#else // not ARM - tempVal = ((unsigned int)tempValQ)<<16; - myStack[0] = (((unsigned int)tempValI)&0xffff) | tempVal; - myStack[1] = powerEst; - myStack[2] = runningSumI; - myStack[3] = runningSumQ; -#endif -} - -// function cycles through rx/tx combinations and then calls subfunction for processing -// for 40 MHz per 20 MHz subband, for 80/160 MHz per 40 MHz block -void readHexDataDemodulateProcessVhtHeNg1(hal_pktinfo_t *pktinfo, hal_wls_processing_input_params_t *inputVals, unsigned int* dataPtr, int csiDataSize, unsigned int *fftInBfr, unsigned int *powerPerSubband, int *phaseRollPtr, int chNum) { - - unsigned short *dataPtrTmp; - short *writePtr; - const short *phiCorrPtrTemp = 0; - int ii, jj, kk, bb; - int pp, nRx, nTx; - int dcZeros = 0; - int dcZeros2 = 0; - int numBands = 1; - short scStart = 0; - short scStop = 0; - int scStart2 = 0; - int scStop2 = 0; - int numTonesIt, tonePos; - int nRx0 = pktinfo->nRx + 1; - int nTx0 = pktinfo->nTx + 1; - //int NgDsfShift = pktinfo->NgDsfShift; - int sigSubbands = pktinfo->sigBw; - //int subbandSize = 1 << (6 - NgDsfShift); - int bandwidth = 20 << sigSubbands; - int ifftSizeOsf = 1 << (pktinfo->fftSize + 6); - int dataPtrStride = nTx0*nRx0; // now in 2 byte steps - const unsigned int *pilotValsPtr = pilotDummyArray; - int phiCorrPtrStride = 0; - unsigned int tempPow, myStack[4]; // includes tempVal|powerEst| runningSumI|runningSumQ - unsigned char pilotToneIndexVHT80[SC5_VHT80_PILOTS] = { 25,53,89,117,139,167,203,231 }; - unsigned char pilotToneIndexVHT40[SC5_VHT40_PILOTS] = { 11,39,53,75,89,117 }; - unsigned char pilotToneIndexVHT20[SC5_VHT20_PILOTS] = { 11,25,39,53 }; - const unsigned char *pilotIdx; - int numPilots, pilotSkip, dcSkip; - -#ifdef REMOVE_IIR - int devSubbands = pktinfo->rxDevBw; - const short *phiCorrPtr = 0; - int phiCorrScale = 0; - int channelOffset = 0; - int deviceBandwidth = 20 << devSubbands; - //if (deviceBandwidth > 80) { // 80+80, is really 80 MHz - // deviceBandwidth = 80; - // devSubbands = 2; - //} -#ifdef STA_20_ONLY - phiCorrPtr = phiCorr64; -#else - phiCorrPtr = phiCorr512; - if (pktinfo->rxDevBw) { // > 20 MHz - phiCorrPtr = phiCorrScBt512; - } -#endif -#endif - nTx = (pktinfo->nTxnTx) ? (pktinfo->nTx + 1) : (inputVals->nTx + 1); - if (MAX_TXnTx = nTx - 1; - nRx = (pktinfo->nRxnRx) ? (pktinfo->nRx + 1) : (inputVals->nRx + 1); - if (MAX_RXnRx = nRx - 1; - -#if defined(REMOVE_IIR) && !defined(STA_20_ONLY) - if (bandwidth> (2 + sigSubbands))&((1 << (devSubbands - sigSubbands)) - 1); - } - else { - channelOffset = ((chNum - 149) >> (2 + sigSubbands))&((1 << (devSubbands - sigSubbands)) - 1); - } - // add offset - phiCorrPtr += channelOffset*(1 << (9 + sigSubbands - devSubbands)); - } - phiCorrScale = 3 - devSubbands; // since filter is in 160 MHz resolution -#endif - - if (pktinfo->packetType == 3) { // VHT - pilotSkip = 0; - dcSkip = 0; - if ((bandwidth == 80) || (bandwidth == 160)) { - // 80 MHz 128+[-122:-2 2:122] - // minus pilots on [103, 75, 39, 11, -103, -75, -39, -11] - numPilots = SC5_VHT80_PILOTS; - pilotIdx = pilotToneIndexVHT80; - dcZeros = 3; - scStart = 6; - scStop = 127; - scStart2 = 130; - scStop2 = 251; - if (bandwidth == 160) { - numBands = 2; - dcZeros2 = 11; -#ifndef COMPUTE_80P80_AS_160 - pktinfo->sigBw = pktinfo->sigBw - 1; - pktinfo->nRx = 2 * nRx - 1; -#endif - } - } - else if (bandwidth == 40) { - // 40 MHz 64+[-58:-2 2:58] - // minus pilots on [-53,-25,-11,11,25,53] - numPilots = SC5_VHT40_PILOTS; - pilotIdx = pilotToneIndexVHT40; - dcZeros = 3; - scStart = 6; - scStop = 63; - scStart2 = 66; - scStop2 = 123; - } - else { //if(bandwidth == 20) - // 20 MHz 32+[-28:-1 1:28] - // minus pilots on [-21, -7, 7, 21] - numPilots = SC5_VHT20_PILOTS; - pilotIdx = pilotToneIndexVHT20; - dcZeros = 1; - scStart = 4; - scStop = 32; - scStart2 = 33; - scStop2 = 61; - } - } - else { // HE - pilotSkip = 2; - if ((bandwidth == 80) || (bandwidth == 160)) { - // 80 MHz 128+[-125:-1 1:125] - // minus pilots on [-117, -100, -23, -6, 6, 23, 100, 117] - numPilots = SC5_HE80_PILOTS; - pilotIdx = pilotToneIndexHE80; - dcZeros = 1; - dcSkip = 0; - scStart = 3; - scStop = 128; - scStart2 = 129; - scStop2 = 254; - if (bandwidth == 160) { - numBands = 2; - dcZeros2 = 5; -#ifndef COMPUTE_80P80_AS_160 - pktinfo->sigBw = pktinfo->sigBw - 1; - pktinfo->nRx = 2 * nRx - 1; -#endif - } - } - else if (bandwidth == 40) { - // 40 MHz 64+[-61:-1 1:61] - // minus pilots on [-53,-36,-26,-9,9,26,36,53] - numPilots = SC5_HE40_PILOTS; - pilotIdx = pilotToneIndexHE40; - dcZeros = 1; - dcSkip = 0; - scStart = 3; - scStop = 64; - scStart2 = 65; - scStop2 = 126; - } - else { //if(bandwidth == 20) - // 20 MHz 32+[-31:0 1:31] - // minus pilots on [-29, -12, 12, 29] - numPilots = SC5_HE20_PILOTS; - pilotIdx = pilotToneIndexHE20; - dcZeros = 0; - dcSkip = 1; - scStart = 1; - scStop = 33; - scStart2 = 33; - scStop2 = 64; - } - } -#ifdef REMOVE_IIR - phiCorrPtr += scStart << phiCorrScale; - phiCorrPtrStride = 1 << phiCorrScale; -#endif - - pp = 0; - for (ii = 0; iinRx + 1; - int nTx = pktinfo->nTx + 1; - int ifftSizeOsf = 1 << (pktinfo->fftSize + 6); - unsigned int *cosTablePtr = (unsigned int*)radix4FftTwiddleArr; - int dcEdge = 0; - int runningSumI = 0; - int runningSumQ = 0; - - unsigned int* readPtr; - unsigned int tempValI, tempValQ, tempValLd; - short tempI0, tempQ0, tempI1, tempQ1; - short cosPhi, sinPhi; - int resLeftI, resLeftQ, resRightI, resRightQ; - int idx; - pktinfo->dcPhase = 0; - - if (pktinfo->packetType == 3) { // VHT case - dcEdge = 6; - } - else { // HE case - dcEdge = 3; - } - - for (ii = 0;ii < nTx;ii++) { - for (jj = 0;jj < nRx;jj+= NUM_PARALLEL) { - readPtr = (fftInBfr + NUM_PARALLEL * ifftSizeOsf * (jj + ii * nRx)); - - for (qq = 0; qq < NUM_PARALLEL; qq++){ - // load dc edge I/Q values - tempValI = readPtr[NUM_PARALLEL * (256 - dcEdge) + qq]; - tempI0 = (tempValI & 0xffff); - tempQ0 = (tempValI >> 16); - - tempValQ = readPtr[NUM_PARALLEL * (256 + dcEdge) + qq]; - tempI1 = (tempValQ & 0xffff); - tempQ1 = (tempValQ >> 16); - - // adjust for phase roll(2 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - - idx = (dcEdge*phaseRollPtr[qq + jj + ii*nRx] *MAX_FFT_SIZE) >> MPY_BIPT; // Ng=1 - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi = (tempValLd & 0xffff); - sinPhi = (tempValLd >> 16); - - resLeftI = ((int)cosPhi*tempI0) - ((int)sinPhi*tempQ0); - resLeftQ = ((int)cosPhi*tempQ0) + ((int)sinPhi*tempI0); - tempI0 = (short)(resLeftI >> TWIDDLE_BIPT); - tempQ0 = (short)(resLeftQ >> TWIDDLE_BIPT); - - if (pktinfo->packetType == 3) { // account for tone rotation - resRightI = - ((int)sinPhi*tempQ1) - ((int)cosPhi*tempI1); - resRightQ = ((int)sinPhi*tempI1) - ((int)cosPhi*tempQ1); - } - else { - resRightI = ((int)cosPhi*tempI1) + ((int)sinPhi*tempQ1); - resRightQ = ((int)cosPhi*tempQ1) - ((int)sinPhi*tempI1); - } - tempI1 = (short)(resRightI >> TWIDDLE_BIPT); - tempQ1 = (short)(resRightQ >> TWIDDLE_BIPT); - - // calculate phase jump - runningSumI += (tempI0*tempI1 + tempQ0*tempQ1)/2; - runningSumQ += (tempI0*tempQ1 - tempQ0*tempI1)/2; - } - } - } - - idx = myAtan2(runningSumI, runningSumQ); - - if (idx > (1 << (MPY_BIPT - 3))) { // pi/4 - pktinfo->dcPhase = 0x1; - } - else if (idx < -(1 << (MPY_BIPT - 3))) { // -pi/4 - pktinfo->dcPhase = 0x3; - } -} - -void readHexDataDemodulateProcess(hal_pktinfo_t *pktinfo, hal_wls_processing_input_params_t *inputVals, unsigned int* dataPtr, int csiDataSize, unsigned int *fftInBfr, unsigned int *powerPerSubband, int *phaseRollPtr, int chNum){ - - unsigned short *dataPtrTmp; - short *writePtr; - const short *phiCorrPtrTemp = 0; -#if (MAX_RX>1) || (MAX_TX>1) - int ii, jj; -#endif - int pp, nRx, nTx; - int dcZeros = 0; - int scStart = 0; - int numCsiScHlf; - int nRx0 = pktinfo->nRx+1; - int nTx0 = pktinfo->nTx+1; - int NgShift = 1+pktinfo->Ng; - int Ng = 1<NgDsfShift; - int sigSubbands = pktinfo->sigBw; - int subbandSize = 1<<(6-NgDsfShift); - int bandwidth = 20<fftSize+6); - int dataPtrStride = nTx0*nRx0; // now in 2 byte steps - const unsigned int *pilotValsPtr; - int phiCorrPtrStride = 0; - unsigned int myStack[4]; // includes tempVal|powerEst| runningSumI|runningSumQ -#ifdef REMOVE_IIR - const short *phiCorrPtr = 0; - int devSubbands = pktinfo->rxDevBw; - int phiCorrScale = 0; - int deviceBandwidth = 20<rxDevBw){ // > 20 MHz - phiCorrPtr = phiCorrScBt512; - } -#endif -#endif - { - pilotValsPtr = pilotDummyArray; - } - - nTx = (pktinfo->nTxnTx)?(pktinfo->nTx+1):(inputVals->nTx+1); - if(MAX_TXnTx = nTx-1; - nRx = (pktinfo->nRxnRx)?(pktinfo->nRx+1):(inputVals->nRx+1); - if(MAX_RXnRx = nRx-1; - -#if defined(REMOVE_IIR) && !defined(STA_20_ONLY) - if(bandwidth>(2+sigSubbands))&((1<<(devSubbands-sigSubbands))-1); - } - else{ - channelOffset = ((chNum-149)>>(2+sigSubbands))&((1<<(devSubbands-sigSubbands))-1); - } - // add offset - phiCorrPtr += channelOffset*(1<<(9+sigSubbands-devSubbands)); - } - phiCorrScale = 3-devSubbands; // since filter is in 160 MHz resolution -#endif - if(bandwidth == 20){ // 20 MHz [1:Ng:27 28 N-28:Ng:N-2 N-1]; - numCsiScHlf = (14>>(NgShift-1))+1; - pilotValsPtr += Ng-2; - dcZeros = 1; - scStart = 4; -#ifdef REMOVE_IIR - phiCorrPtr += 4<>(NgShift-1); // = dataPerChannel/2 - pilotValsPtr += Ng+2; - dcZeros = 2-NgShift; - scStart = ((subbandSize<1) || (MAX_TX>1) - for(ii=0;iipacketType) ? (numCsiScHlf - 2) : 0; - writePtr +=((numCsiScHlf-1)*(Ng-1)-1)*2; -#ifdef REMOVE_IIR - phiCorrPtrTemp -= 2*phiCorrPtrStride-phiCorrPtrStride/Ng; -#endif - processSubCarrier20MHz(dataPtrTmp, writePtr, pilotValsPtr[1], phiCorrPtrTemp, numCsiScHlf, dataPtrStride, phiCorrPtrStride, Ng, jumpIdx, myStack); - } - else { - if(bandwidth == 160){ - processSubCarrier(dataPtrTmp, writePtr, pilotValsPtr[0], phiCorrPtrTemp, numCsiScHlf, dataPtrStride, phiCorrPtrStride, myStack); - dataPtrTmp +=numCsiScHlf*dataPtrStride; - writePtr +=numCsiScHlf*2; -#ifdef REMOVE_IIR - phiCorrPtrTemp +=numCsiScHlf*phiCorrPtrStride; -#endif - powerPerSubband[pp++] = myStack[1]; - myStack[1] = 0; - } - processSubCarrier(dataPtrTmp, writePtr, pilotValsPtr[1], phiCorrPtrTemp, numCsiScHlf, dataPtrStride, phiCorrPtrStride, myStack); - } - powerPerSubband[pp++] = myStack[1]; - myStack[1] = 0; - -#if (MAX_RX>1) || (MAX_TX>1) - phaseRollPtr[(jj+ii*nRx)] = myAtan2(myStack[2], myStack[3]); -#else - *phaseRollPtr = myAtan2(myStack[2], myStack[3]); -#endif - } - } -} - -void calculateTotalPower(hal_pktinfo_t *pktinfo, unsigned int *powerPerSubband, unsigned int *totalpower){ - - int ii, jj, tt, qq; - unsigned int tempVal; - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; - int numSubbands = 1<<(pktinfo->sigBw); - int numSubBlocks; - if(pktinfo->sigBw>1){ // for 80 and 160 MHz, 40 MHz subbands - numSubBlocks = numSubbands>>1; - } - else{ // 20 an 40 MHz - numSubBlocks = 2; - } - - // determine scaling - for(ii=0;iinRx+1; - int nTx = pktinfo->nTx+1; - - for(ii=0;iisigBw == 0) { // 20 MHz cases -#if defined(FFT_PARALLEL) && defined(ARM_DS5) - interpolateBandEdges20Intrinsic(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr + jj + ii*nRx); -#elif defined(FFT_PARALLEL) && !defined(ARM_DS5) - interpolateBandEdges20Parallel(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr + jj + ii*nRx); -#else - interpolateBandEdges20(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr[jj + ii*nRx]); -#endif - } - else{ - if (pktinfo->Ng) { // Ng=4 case -#if defined(FFT_PARALLEL) && defined(ARM_DS5) - interpolateBandEdgesIntrinsic(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr + jj + ii*nRx); -#elif defined(FFT_PARALLEL) && !defined(ARM_DS5) - interpolateBandEdgesParallel(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr + jj + ii*nRx); -#else - interpolateBandEdges(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr[jj + ii*nRx]); -#endif - } - else { // 40 MHz case -#if defined(FFT_PARALLEL) && defined(ARM_DS5) - interpolateBandEdges40Intrinsic(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr + jj + ii*nRx); -#elif defined(FFT_PARALLEL) && !defined(ARM_DS5) - interpolateBandEdges40Parallel(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr + jj + ii*nRx); -#else - interpolateBandEdges40(pktinfo, fftInBuffer + bufferspacing*(jj + ii*nRx), phaseRollPtr[jj + ii*nRx]); -#endif - } - } - } - } -} -#endif - -void ifftProcessing(hal_pktinfo_t *pktinfo, unsigned int *fftInBuffer, unsigned int *fftOutBuffer, int bufferspacing){ - int ii, jj; - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - short *pSrc = (short*)fftInBuffer; -#ifndef FFT_INPLACE - short *pDst = (short*)fftOutBuffer; -#endif - -#ifdef FFT_INPLACE - for (ii = 0;iinRx + 1; - int nTx = pktinfo->nTx + 1; - int numSubbands = 1 << pktinfo->sigBw; - int bandwidth = 20 * numSubbands; - int fftSizeHalf = (32 * numSubbands) >> (pktinfo->NgDsfShift); - int fftSize = 2 * fftSizeHalf; - int numBands = 1; - int NgShift = 0; - int extraSc = 0; - const unsigned char *extraScIdx = NULL; - unsigned int powerScale = 1; - - if (pktinfo->packetType == 3) { // VHT -#ifndef STA_20_ONLY - if ((bandwidth == 80) || (bandwidth == 160)) { - // 80 MHz 128+[-122:-2 2:122] - // minus pilots on [103, 75, 39, 11, -103, -75, -39, -11] - numPilots = SC5_VHT80_PILOTS; - pilotIdx = pilotToneIndexVHT80; - dcZeros = 3; - if (bandwidth == 160) { - fftSizeHalf >>= 1; - fftSize >>= 1; - numBands = 2; - } - } - else if (bandwidth == 40) { - // 40 MHz 64+[-58:-2 2:58] - // minus pilots on [-53,-25,-11,11,25,53] - numPilots = SC5_VHT40_PILOTS; - pilotIdx = pilotToneIndexVHT40; - dcZeros = 3; - } - else -#endif - { //if(bandwidth == 20) - // 20 MHz 32+[-28:-1 1:28] - // minus pilots on [-21, -7, 7, 21] - numPilots = SC5_VHT20_PILOTS; - pilotIdx = pilotToneIndexVHT20; - dcZeros = 1; - } - } - else { - if (pktinfo->packetType == 0) { // Leg 20in20 only - // pilotToneIndexLEG20[SC5_HT20_PILOTS - // 20 MHz 32+[-28:2:-2 -1 1 2:2:28] - numPilots = SC5_HT20_PILOTS; - pilotIdx = pilotToneIndexLEG20; - dcZeros = 2; // not really DC - NgShift = 1; // Ng=2 - extraSc = 0; //2; // change phase on two subcarriers - powerScale = 2; - } - else if (pktinfo->packetType == 1) { -#ifndef STA_20_ONLY - if(pktinfo->sigBw==1){ //HT40 - numPilots = 0; - pilotIdx = pilotToneIndexHT20; // not used - dcZeros = 1; - extraSc = 0; // not used - } - else -#endif - { // HT 20 - // pilotToneIndexHT20[SC5_HT20_PILOTS - // 20 MHz 32+[-28:2:-2 -1 1:2:27 28] - // minus pilots on [-21, -7, 7, 21] - numPilots = SC5_HT20_PILOTS; - pilotIdx = pilotToneIndexHT20; - dcZeros = 0; - NgShift = 1; // Ng=2 - extraSc = 2; // change phase on two subcarriers - extraScIdx = pilotToneIndexVHT20 + 2; // skip negative half - powerScale = 2; - } - } - else { // HE -#ifndef STA_20_ONLY - if ((bandwidth == 80) || (bandwidth == 160)) { - // 80 MHz 128+[-125:-1 1:125] - // minus pilots on [-117, -100, -23, -6, 6, 23, 100, 117] - numPilots = SC5_HE80_PILOTS; - pilotIdx = pilotToneIndexHE80; - dcZeros = 1; - if (bandwidth == 160) { - fftSizeHalf >>= 1; - fftSize >>= 1; - numBands = 2; - } - } - else if (bandwidth == 40) { - // 40 MHz 64+[-61:-1 1:61] - // minus pilots on [-53,-36,-26,-9,9,26,36,53] - numPilots = SC5_HE40_PILOTS; - pilotIdx = pilotToneIndexHE40; - dcZeros = 1; - } - else -#endif - { //if(bandwidth == 20) - // 20 MHz 32+[-31:0 1:31] - // minus pilots on [-29, -12, 12, 29] - numPilots = SC5_HE20_PILOTS; - pilotIdx = pilotToneIndexHE20; - dcZeros = 0; - } - } - } - - for (ii = 0; ii < nTx; ii++){ - for (jj = 0; jj < nRx; jj++){ - - // account for extra non-zero subcarriers is substantial - totalpower[jj + ii*nRx] = powerScale*totalpower[jj + ii*nRx]; - - phaseRollNg = (phaseRollPtr[jj + ii*nRx])>> NgShift; - - idx = (phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi1 = (tempValLd & 0xffff); - sinPhi1 = (tempValLd >> 16); - if (dcZeros > 1){ - idx = (2 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi2 = (tempValLd & 0xffff); - sinPhi2 = (tempValLd >> 16); - - idx = (3 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi3 = (tempValLd & 0xffff); - sinPhi3 = (tempValLd >> 16); - } - - for (bb = 0; bb < numBands; bb++){ - readPtr = fftInBuffer + bb*fftSize + bufferspacing*(jj + ii*nRx); - - for (kk = 0; kk < extraSc; kk++) { - tempValLd = readPtr[extraScIdx[kk]]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI = (((int)cosPhi1*tempI) + ((int)sinPhi1*tempQ)); - resRightQ = (((int)cosPhi1*tempQ) - ((int)sinPhi1*tempI)); - convertI = resRightI >> TWIDDLE_BIPT; - convertQ = resRightQ >> TWIDDLE_BIPT; - - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[extraScIdx[kk]] = tempValSt; - } - - if (dcZeros == 2) { // not really DC, just fix edge tones for Legacy 20 MHz - tempValLd = readPtr[pilotToneIndexLEG20[0] + 1]; // first subcarrier - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI = (((int)cosPhi2*tempI) + ((int)sinPhi2*tempQ)); - resRightQ = (((int)cosPhi2*tempQ) - ((int)sinPhi2*tempI)); - convertI = resRightI >> TWIDDLE_BIPT; - convertQ = resRightQ >> TWIDDLE_BIPT; - - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[pilotToneIndexLEG20[0] - 1] = tempValSt; - - tempValLd = readPtr[pilotToneIndexLEG20[SC5_HT20_PILOTS-1]-1]; // last subcarrier - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI = (((int)cosPhi2*tempI) - ((int)sinPhi2*tempQ)); - resRightQ = (((int)cosPhi2*tempQ) + ((int)sinPhi2*tempI)); - convertI = resRightI >> TWIDDLE_BIPT; - convertQ = resRightQ >> TWIDDLE_BIPT; - - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[pilotToneIndexLEG20[SC5_HT20_PILOTS-1]+1] = tempValSt; - } - - // interpolate pilots - for (kk = 0; kk < numPilots; kk++){ - tempValLd = readPtr[pilotIdx[kk] + 1]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI = (((int)cosPhi1*tempI) + ((int)sinPhi1*tempQ)) / 2; - resRightQ = (((int)cosPhi1*tempQ) - ((int)sinPhi1*tempI)) / 2; - - tempValLd = readPtr[pilotIdx[kk] - 1]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resLeftI = (((int)cosPhi1*tempI) - ((int)sinPhi1*tempQ)) / 2; - resLeftQ = (((int)cosPhi1*tempQ) + ((int)sinPhi1*tempI)) / 2; - - convertI = (resLeftI + resRightI) >> TWIDDLE_BIPT; - convertQ = (resLeftQ + resRightQ) >> TWIDDLE_BIPT; - - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[pilotIdx[kk]] = tempValSt; - } - - // interpolate DC - if (dcZeros == 3){ - tempValLd = readPtr[fftSizeHalf + 2]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI = (((int)cosPhi1*tempI) + ((int)sinPhi1*tempQ)) / 2; - resRightQ = (((int)cosPhi1*tempQ) - ((int)sinPhi1*tempI)) / 2; - - resMidI = (((int)cosPhi2*tempI) + ((int)sinPhi2*tempQ)) / 2; - resMidQ = (((int)cosPhi2*tempQ) - ((int)sinPhi2*tempI)) / 2; - - resLeftI = (((int)cosPhi3*tempI) + ((int)sinPhi3*tempQ)) / 2; - resLeftQ = (((int)cosPhi3*tempQ) - ((int)sinPhi3*tempI)) / 2; - - tempValLd = readPtr[fftSizeHalf - 2]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI += (((int)cosPhi3*tempI) - ((int)sinPhi3*tempQ)) / 2; - resRightQ += (((int)cosPhi3*tempQ) + ((int)sinPhi3*tempI)) / 2; - - resMidI += (((int)cosPhi2*tempI) - ((int)sinPhi2*tempQ)) / 2; - resMidQ += (((int)cosPhi2*tempQ) + ((int)sinPhi2*tempI)) / 2; - - resLeftI += (((int)cosPhi1*tempI) - ((int)sinPhi1*tempQ)) / 2; - resLeftQ += (((int)cosPhi1*tempQ) + ((int)sinPhi1*tempI)) / 2; - - convertI = resRightI >> TWIDDLE_BIPT; - convertQ = resRightQ >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[fftSizeHalf+1] = tempValSt; - - convertI = resMidI >> TWIDDLE_BIPT; - convertQ = resMidQ >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[fftSizeHalf] = tempValSt; - - convertI = resLeftI >> TWIDDLE_BIPT; - convertQ = resLeftQ >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[fftSizeHalf-1] = tempValSt; - } - else if (dcZeros == 1) { // dcZeros == 1 - tempValLd = readPtr[fftSizeHalf + 1]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - resMidI = (((int)cosPhi1*tempI) + ((int)sinPhi1*tempQ)) / 2; - resMidQ = (((int)cosPhi1*tempQ) - ((int)sinPhi1*tempI)) / 2; - - tempValLd = readPtr[fftSizeHalf - 1]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - resMidI += (((int)cosPhi1*tempI) - ((int)sinPhi1*tempQ)) / 2; - resMidQ += (((int)cosPhi1*tempQ) + ((int)sinPhi1*tempI)) / 2; - - convertI = resMidI >> TWIDDLE_BIPT; - convertQ = resMidQ >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[fftSizeHalf] = tempValSt; - } - } - } - } - -} - -void interpolateBandEdges20(hal_pktinfo_t *pktinfo, unsigned int *fftInBuffer, int phaseRollNg) { - - unsigned int *cosTablePtr = (unsigned int*)radix4FftTwiddleArr; - short sinPhi1, cosPhi1, sinPhi2, cosPhi2; - short sinPhi0, cosPhi0; - - int convertI, convertQ; - int resLeftI1, resLeftQ1, resRightI1, resRightQ1; - int resLeftI2, resLeftQ2, resRightI2, resRightQ2; - short tempI, tempQ; - unsigned int tempValLd, tempValSt; - - int idx; - - int psb = pktinfo->psb; - int NgDsfShift = pktinfo->NgDsfShift; - int subbandSize = 1 << (6 - NgDsfShift); - - unsigned int *readPtr = fftInBuffer; - - idx = (phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi1 = (tempValLd & 0xffff); - sinPhi1 = (tempValLd >> 16); - - if (pktinfo->rxDevBw == 1) { // 40 MHz device bandwidth - idx = (2 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi2 = (tempValLd & 0xffff); - sinPhi2 = (tempValLd >> 16); - - idx = (phaseRollNg*MAX_FFT_SIZE) >> (MPY_BIPT + 1); // divide by 2 for Ng=2 - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi0 = (tempValLd & 0xffff); - sinPhi0 = (tempValLd >> 16); - - // interpolate edge for 20/40 - if (psb == 0) { // left side, right edge - tempValLd = readPtr[subbandSize - 3]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resLeftI1 = ((int)cosPhi2*tempI) - ((int)sinPhi2*tempQ); - resLeftQ1 = ((int)cosPhi2*tempQ) + ((int)sinPhi2*tempI); - - resLeftI2 = ((int)cosPhi1*tempI) - ((int)sinPhi1*tempQ); - resLeftQ2 = ((int)cosPhi1*tempQ) + ((int)sinPhi1*tempI); - - convertI = resLeftI1 >> TWIDDLE_BIPT; - convertQ = resLeftQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize - 1] = tempValSt; - - convertI = resLeftI2 >> TWIDDLE_BIPT; - convertQ = resLeftQ2 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize - 2] = tempValSt; - - // change phase on sc -32 / 32 - tempValLd = readPtr[subbandSize / 2]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resLeftI2 = (((int)cosPhi0*tempI) - ((int)sinPhi0*tempQ)); - resLeftQ2 = (((int)cosPhi0*tempQ) + ((int)sinPhi0*tempI)); - - convertI = resLeftI2 >> TWIDDLE_BIPT; - convertQ = resLeftQ2 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize / 2] = tempValSt; - } - else { //if (psb == 1) // right side, left edge - tempValLd = readPtr[subbandSize + 3]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI2 = ((int)cosPhi1*tempI) + ((int)sinPhi1*tempQ); - resRightQ2 = ((int)cosPhi1*tempQ) - ((int)sinPhi1*tempI); - - resRightI1 = ((int)cosPhi2*tempI) + ((int)sinPhi2*tempQ); - resRightQ1 = ((int)cosPhi2*tempQ) - ((int)sinPhi2*tempI); - - convertI = resRightI2 >> TWIDDLE_BIPT; - convertQ = resRightQ2 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize + 2] = tempValSt; - - convertI = resRightI1 >> TWIDDLE_BIPT; - convertQ = resRightQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize + 1] = tempValSt; - - // change phase on sc -32 / 32 - tempValLd = readPtr[3 * subbandSize / 2]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI1 = (((int)cosPhi0*tempI) - ((int)sinPhi0*tempQ)); - resRightQ1 = (((int)cosPhi0*tempQ) + ((int)sinPhi0*tempI)); - - convertI = resRightI1 >> TWIDDLE_BIPT; - convertQ = resRightQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[3 * subbandSize / 2] = tempValSt; - } - } - else { // pktinfo->rxDevBw == 2) // 80 MHz device bandwidth - tempValLd = readPtr[psb*subbandSize+1]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resLeftI1 = ((int)cosPhi1*tempI) + ((int)sinPhi1*tempQ); - resLeftQ1 = ((int)cosPhi1*tempQ) - ((int)sinPhi1*tempI); - - convertI = resLeftI1 >> TWIDDLE_BIPT; - convertQ = resLeftQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[psb*subbandSize] = tempValSt; - - tempValLd = readPtr[(psb+1)*subbandSize - 2]; // right edge - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI1 = (((int)cosPhi1*tempI) - ((int)sinPhi1*tempQ)); - resRightQ1 = (((int)cosPhi1*tempQ) + ((int)sinPhi1*tempI)); - - convertI = resRightI1 >> TWIDDLE_BIPT; - convertQ = resRightQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[(psb + 1)*subbandSize - 1] = tempValSt; - } -} - -void interpolateBandEdges40(hal_pktinfo_t *pktinfo, unsigned int *fftInBuffer, int phaseRollNg) { - - unsigned int *cosTablePtr = (unsigned int*)radix4FftTwiddleArr; - short sinPhi1, cosPhi1, sinPhi2, cosPhi2, sinPhi3, cosPhi3; - short sinPhi4, cosPhi4, sinPhi5, cosPhi5, sinPhi0, cosPhi0; - - int convertI, convertQ; - int resLeftI1, resLeftQ1, resRightI1, resRightQ1, resMidI, resMidQ; - int resLeftI2, resLeftQ2, resRightI2, resRightQ2; - short tempI, tempQ; - unsigned int tempValLd, tempValSt; - - int idx; - - int NgDsfShift = pktinfo->NgDsfShift; - int subbandSize = 1 << (6 - NgDsfShift); - - unsigned int *readPtr = fftInBuffer; - - idx = (phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi1 = (tempValLd & 0xffff); - sinPhi1 = (tempValLd >> 16); - - idx = (2 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi2 = (tempValLd & 0xffff); - sinPhi2 = (tempValLd >> 16); - - idx = (3 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi3 = (tempValLd & 0xffff); - sinPhi3 = (tempValLd >> 16); - - idx = (4 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi4 = (tempValLd & 0xffff); - sinPhi4 = (tempValLd >> 16); - - idx = (5 * phaseRollNg*MAX_FFT_SIZE) >> MPY_BIPT; - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi5 = (tempValLd & 0xffff); - sinPhi5 = (tempValLd >> 16); - - - tempValLd = readPtr[subbandSize + 3]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI2 = 5*(((int)cosPhi1*tempI) + ((int)sinPhi1*tempQ)) / 6; - resRightQ2 = 5*(((int)cosPhi1*tempQ) - ((int)sinPhi1*tempI)) / 6; - - resRightI1 = 4*(((int)cosPhi2*tempI) + ((int)sinPhi2*tempQ)) / 6; - resRightQ1 = 4*(((int)cosPhi2*tempQ) - ((int)sinPhi2*tempI)) / 6; - - resMidI = (((int)cosPhi3*tempI) + ((int)sinPhi3*tempQ)) / 2; - resMidQ = (((int)cosPhi3*tempQ) - ((int)sinPhi3*tempI)) / 2; - - resLeftI1 = 2*(((int)cosPhi4*tempI) + ((int)sinPhi4*tempQ)) / 6; - resLeftQ1 = 2*(((int)cosPhi4*tempQ) - ((int)sinPhi4*tempI)) / 6; - - resLeftI2 = (((int)cosPhi5*tempI) + ((int)sinPhi5*tempQ)) / 6; - resLeftQ2 = (((int)cosPhi5*tempQ) - ((int)sinPhi5*tempI)) / 6; - - tempValLd = readPtr[subbandSize - 3]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI2 += (((int)cosPhi5*tempI) - ((int)sinPhi5*tempQ)) / 6; - resRightQ2 += (((int)cosPhi5*tempQ) + ((int)sinPhi5*tempI)) / 6; - - resRightI1 += 2*(((int)cosPhi4*tempI) - ((int)sinPhi4*tempQ)) / 6; - resRightQ1 += 2*(((int)cosPhi4*tempQ) + ((int)sinPhi4*tempI)) / 6; - - resMidI += (((int)cosPhi3*tempI) - ((int)sinPhi3*tempQ)) / 2; - resMidQ += (((int)cosPhi3*tempQ) + ((int)sinPhi3*tempI)) / 2; - - resLeftI1 += 4*(((int)cosPhi2*tempI) - ((int)sinPhi2*tempQ)) / 6; - resLeftQ1 += 4*(((int)cosPhi2*tempQ) + ((int)sinPhi2*tempI)) / 6; - - resLeftI2 += 5*(((int)cosPhi1*tempI) - ((int)sinPhi1*tempQ)) / 6; - resLeftQ2 += 5*(((int)cosPhi1*tempQ) + ((int)sinPhi1*tempI)) / 6; - - convertI = resRightI2 >> TWIDDLE_BIPT; - convertQ = resRightQ2 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize + 2] = tempValSt; - - convertI = resRightI1 >> TWIDDLE_BIPT; - convertQ = resRightQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize + 1] = tempValSt; - - convertI = resMidI >> TWIDDLE_BIPT; - convertQ = resMidQ >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize] = tempValSt; - - convertI = resLeftI1 >> TWIDDLE_BIPT; - convertQ = resLeftQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize - 1] = tempValSt; - - convertI = resLeftI2 >> TWIDDLE_BIPT; - convertQ = resLeftQ2 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize - 2] = tempValSt; - - // change phase on sc -32 / 32 - idx = (phaseRollNg*MAX_FFT_SIZE) >> (MPY_BIPT+1); // divide by 2 for Ng=2 - idx &= (MAX_FFT_SIZE - 1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi0 = (tempValLd & 0xffff); - sinPhi0 = (tempValLd >> 16); - - tempValLd = readPtr[subbandSize/2]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resLeftI2 = (((int)cosPhi0*tempI) - ((int)sinPhi0*tempQ)); - resLeftQ2 = (((int)cosPhi0*tempQ) + ((int)sinPhi0*tempI)); - - convertI = resLeftI2 >> TWIDDLE_BIPT; - convertQ = resLeftQ2 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[subbandSize/2] = tempValSt; - - tempValLd = readPtr[3*subbandSize/2]; - tempI = (tempValLd & 0xffff); - tempQ = (tempValLd >> 16); - - resRightI1 = (((int)cosPhi0*tempI) - ((int)sinPhi0*tempQ)); - resRightQ1 = (((int)cosPhi0*tempQ) + ((int)sinPhi0*tempI)); - - convertI = resRightI1 >> TWIDDLE_BIPT; - convertQ = resRightQ1 >> TWIDDLE_BIPT; - tempValSt = (convertI & 0xffff) | (convertQ << 16); - readPtr[3*subbandSize/2] = tempValSt; -} - -void interpolateBandEdges(hal_pktinfo_t *pktinfo, unsigned int *fftInBuffer, int phaseRollNg){ - - int ii; - int NgDsfShift = pktinfo->NgDsfShift; - int numSubbands = 1<sigBw; - int subbandSize = 1<<(6-NgDsfShift); - unsigned int *readPtr = fftInBuffer+pktinfo->scOffset; - - readPtr[0] = 0; - for(ii=1;ii>MPY_BIPT; - idx &=(MAX_FFT_SIZE-1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi1 = (tempValLd&0xffff); - sinPhi1 = (tempValLd>>16); - - idx = (phaseRollNg*MAX_FFT_SIZE)>>(MPY_BIPT-1); - idx &=(MAX_FFT_SIZE-1); // apply modulo - tempValLd = cosTablePtr[idx]; - cosPhi2 = (tempValLd&0xffff); - sinPhi2 = (tempValLd>>16); -#endif - - // contribution by lltf_missing(2,:)+2 - tempValLd = valRight[2]; - tempI = (tempValLd&0xffff); - tempQ = (tempValLd>>16); - - resRightI = (((int)cosPhi2*tempI)+((int)sinPhi2*tempQ))/4; - resRightQ = (((int)cosPhi2*tempQ)-((int)sinPhi2*tempI))/4; - - // contribution by lltf_missing(2,:)+1 - tempValLd = valRight[1]; - tempI = (tempValLd&0xffff); - tempQ = (tempValLd>>16); - - resRightI += (((int)cosPhi1*tempI)+((int)sinPhi1*tempQ))/2; - resRightQ += (((int)cosPhi1*tempQ)-((int)sinPhi1*tempI))/2; - - resLeftI = (((int)cosPhi2*tempI)+((int)sinPhi2*tempQ))/4; - resLeftQ = (((int)cosPhi2*tempQ)-((int)sinPhi2*tempI))/4; - - // contribution by lltf_missing(1,:)-1 - tempValLd = valLeft[-1]; - tempI = (tempValLd&0xffff); - tempQ = (tempValLd>>16); - - resRightI += (((int)cosPhi2*tempI)-((int)sinPhi2*tempQ))/4; - resRightQ += (((int)cosPhi2*tempQ)+((int)sinPhi2*tempI))/4; - - resLeftI += (((int)cosPhi1*tempI)-((int)sinPhi1*tempQ))/2; - resLeftQ += (((int)cosPhi1*tempQ)+((int)sinPhi1*tempI))/2; -#ifdef FLOATING_POINT - convertI = (int)floorf(resRightI+0.5f); - convertQ = (int)floorf(resRightQ+0.5f); -#else - convertI = resRightI>>TWIDDLE_BIPT; - convertQ = resRightQ>>TWIDDLE_BIPT; -#endif - tempValSt = (convertI&0xffff) | (convertQ<<16); - valRight[0] = tempValSt; - - // contribution by lltf_missing(1,:)-2 - tempValLd = valLeft[-2]; - tempI = (tempValLd&0xffff); - tempQ = (tempValLd>>16); - - resLeftI += (((int)cosPhi2*tempI)-((int)sinPhi2*tempQ))/4; - resLeftQ += (((int)cosPhi2*tempQ)+((int)sinPhi2*tempI))/4; - -#ifdef FLOATING_POINT - convertI = (int)floorf(resLeftI+0.5f); - convertQ = (int)floorf(resLeftQ+0.5f); -#else - convertI = resLeftI>>TWIDDLE_BIPT; - convertQ = resLeftQ>>TWIDDLE_BIPT; -#endif - tempValSt = (convertI&0xffff) | (convertQ<<16); - valLeft[0] = tempValSt; -} - -void findActiveSubbands(hal_pktinfo_t *pktinfo, unsigned int *powerPerSubband, unsigned int *totalpower, int chNum, int ftmSignalBW){ - - int ii, qq, rx, tx, subBandPer, numSubBlocks, fftSize; - int subBlock, offset, channelMask, flag=1; - int channelOffset = 0; - - int sigBwTemp = pktinfo->sigBw; // initialized to devBw - int NgDsfShift = pktinfo->NgDsfShift; - int numSubbands = 1<nRx+1; - int nTx = pktinfo->nTx+1; - unsigned int topHalf, bottomHalf, topHalfTemp, bottomHalfTemp; - - // figure out channel offset - if(chNum>35){ // 5 GHz - if(chNum<149){ - channelOffset = ((chNum-36)>>2)&(numSubbands-1); - } - else{ - channelOffset = ((chNum-149)>>2)&(numSubbands-1); - } - } - pktinfo->scOffset = channelOffset; - - if(sigBwTemp>1){ // for 80 and 160 MHz, 40 MHz subbands - subBandPer = 2; - channelOffset>>=1; - } - else{ - subBandPer = 1; - } - - numSubBlocks = numSubbands/subBandPer; offset =0; - subBlock = numSubBlocks; - - if (numSubBlocks > 1) { - do { - channelMask = subBlock - 1; - subBlock >>= 1; - bottomHalf = 0; topHalf = 0; flag = 0; - for (tx = 0;tx < nTx;tx++) { - for (rx = 0;rx < nRx;rx += NUM_PARALLEL) { - for (qq = 0;qq < NUM_PARALLEL; qq++) { - bottomHalfTemp = 0; topHalfTemp = 0; - for (ii = 0;ii < subBlock;ii++) { - bottomHalfTemp += powerPerSubband[qq + (ii + offset*subBlock)*NUM_PARALLEL + (rx + tx*nRx)*numSubBlocks]; - topHalfTemp += powerPerSubband[qq + (ii + (offset + 1)*subBlock)*NUM_PARALLEL + (rx + tx*nRx)*numSubBlocks]; - } - totalpower[qq + rx + tx*nRx] = topHalfTemp + bottomHalfTemp; - bottomHalf += bottomHalfTemp; - topHalf += topHalfTemp; - } - } - } - bottomHalf /= subBlock*subbandSize*nTx*nRx; - topHalf /= subBlock*subbandSize*nTx*nRx; - - if (((channelOffset &channelMask) >= subBlock) && ((bottomHalf < SUBBAND_DET_THRESH) || (ftmSignalBW < sigBwTemp))) { - flag = 1; - sigBwTemp--; - offset = 2 * offset + 2; - numSubbands >>= 1; - //pktinfo->scOffset += numSubbands*subbandSize; - } - else if ((topHalf < SUBBAND_DET_THRESH) || (ftmSignalBW < sigBwTemp)) { - flag = 1; - sigBwTemp--; - numSubbands >>= 1; - } - } while (flag && (subBlock > 1)); - } - else { - // determine scaling - numSubBlocks = 2; - for (tx = 0;tx < nTx;tx++) { - for (rx = 0;rx < nRx;rx += NUM_PARALLEL) { - for (qq = 0; qq < NUM_PARALLEL; qq++) { - unsigned int tempVal = 0; - for (ii = 0;ii < numSubBlocks;ii++) { - tempVal += powerPerSubband[qq + ii*NUM_PARALLEL + (rx + tx*nRx)*numSubBlocks]; - } - totalpower[qq + rx + tx*nRx] = tempVal; - } - } - } - } - - if (sigBwTemp > ftmSignalBW) - sigBwTemp = ftmSignalBW; - - channelOffset = (pktinfo->scOffset) >> sigBwTemp; - pktinfo->scOffset = subbandSize *(channelOffset << sigBwTemp); - - // write back - if(sigBwTemp!=pktinfo->sigBw){ - pktinfo->sigBw = sigBwTemp; - fftSize = pktinfo->sigBw +IFFT_OSF_SHIFT -pktinfo->NgDsfShift; - pktinfo->fftSize = (fftSize > MIN_IFFT_SIZE_SHIFT)? fftSize:MIN_IFFT_SIZE_SHIFT; - } -} - -void zeroOutTones(hal_pktinfo_t *pktinfo, unsigned int *fftInBuffer, int bufferspacing){ - - int ii, jj, kk; - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; - int scOffset = pktinfo->scOffset; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - int numSubbands = 1<<(pktinfo->sigBw); - int subbandSize = 1<<(6-pktinfo->NgDsfShift); - - unsigned int *writePtr; - - for(ii=0;iisigBw); - int subbandSize = 1<<(6-pktinfo->NgDsfShift); - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; -#if defined(ARM_GCC) || defined(ARM_DS5) - unsigned int myConst = 0x10001; - unsigned int myZero = 0x0; -#ifdef ARM_DS5 - int reg6, reg7; -#endif -#else - short tempI, tempQ; -#endif - - for(ii=0;iiscOffset); - if(bandwidth==40){ // second 20 MHz multiply by 1i -#if defined(ARM_GCC) || defined(ARM_DS5) - writePtr+=2*subbandSize; - for(kk=0;kk=80) && (pktinfo->packetType<4) ){ // first 20 MHz multiply by -1 -#if defined(ARM_GCC) || defined(ARM_DS5) - for(kk=0;kkdcPhase == 0x3) { // case where we multiply by pi/2 - for (kk = 4 * subbandSize;kk < 5 * subbandSize;kk++) { - tempI = writePtr[2 * kk]; - tempQ = writePtr[2 * kk + 1]; - writePtr[2 * kk] = tempQ; - writePtr[2 * kk + 1] = -tempI; - } - for (kk = 5 * subbandSize;kk < 8 * subbandSize;kk++) { - tempI = writePtr[2 * kk]; - tempQ = writePtr[2 * kk + 1]; - writePtr[2 * kk] = -tempQ; - writePtr[2 * kk + 1] = tempI; - } - } - else if (pktinfo->dcPhase == 0x1) { // case where we multiply by -pi/2 - for (kk = 4 * subbandSize;kk < 5 * subbandSize;kk++) { - tempI = writePtr[2 * kk]; - tempQ = writePtr[2 * kk + 1]; - writePtr[2 * kk] = -tempQ; - writePtr[2 * kk + 1] = tempI; - } - for (kk = 5 * subbandSize;kk < 8 * subbandSize;kk++) { - tempI = writePtr[2 * kk]; - tempQ = writePtr[2 * kk + 1]; - writePtr[2 * kk] = tempQ; - writePtr[2 * kk + 1] = -tempI; - } - } - else { // no extra rotation - for (kk = 4 * subbandSize;kk < 5 * subbandSize;kk++) { - tempI = writePtr[2 * kk]; - tempQ = writePtr[2 * kk + 1]; - writePtr[2 * kk] = -tempI; - writePtr[2 * kk + 1] = -tempQ; - } - } -#endif - } -#endif - } - else if (pktinfo->packetType == 4) { // only HE160 - if (pktinfo->dcPhase == 0x3) { // case where we multiply by pi/2 - for (kk = 4 * subbandSize;kk < 8 * subbandSize;kk++) { - tempI = writePtr[2 * kk]; - tempQ = writePtr[2 * kk + 1]; - writePtr[2 * kk] = -tempQ; - writePtr[2 * kk + 1] = tempI; - } - } - else if (pktinfo->dcPhase == 0x1) { // case where we multiply by -pi/2 - for (kk = 4 * subbandSize;kk < 8 * subbandSize;kk++) { - tempI = writePtr[2 * kk]; - tempQ = writePtr[2 * kk + 1]; - writePtr[2 * kk] = tempQ; - writePtr[2 * kk + 1] = -tempI; - } - } - } - } - } - -} - -#endif // STA_20_ONLY - -void calcPdpAndMaxRx(unsigned int *currentValPtr, unsigned int *pdpOutBuffer, int ifftSizeOsf, unsigned int* maxVals, int *maxIndeces){ -#ifdef ARM_DS5 - int reg6; -#else - short tempI, tempQ; - unsigned int tempVal; -#endif - int tt, maxIdx = -1; - unsigned int currentSum, currentMax = 0; - - for(tt=0;tt>16)&0xffff; - tempVal = tempI*tempI; - currentSum = tempVal; - tempVal = tempQ*tempQ; - currentSum += tempVal; -#endif - pdpOutBuffer[tt]= currentSum; - if(currentSum>currentMax){ - currentMax =currentSum; - maxIdx = tt; - } - } - *maxVals = currentMax; - *maxIndeces = maxIdx; -} - -void calcPdpAndMaxRxParallel(unsigned int *currentValPtr, unsigned int *pdpOutBuffer, int ifftSizeOsf, unsigned int* maxVals, int *maxIndeces) { - short tempI, tempQ; - unsigned int tempVal; - int tt, pp, maxIdx[NUM_PARALLEL]; - unsigned int currentSum, currentMax[NUM_PARALLEL]; - - for (pp = 0; pp < NUM_PARALLEL; pp++) { - currentMax[pp] = 0; - maxIdx[pp] = -1; - } - - for (tt = 0;tt> 16) & 0xffff; - tempVal = tempI*tempI; - currentSum = tempVal; - tempVal = tempQ*tempQ; - currentSum += tempVal; - pdpOutBuffer[NUM_PARALLEL*tt+pp] = currentSum; - if (currentSum > currentMax[pp]) { - currentMax[pp] = currentSum; - maxIdx[pp] = tt; - } - } - } - for (pp = 0; pp < NUM_PARALLEL; pp++) { - maxVals[pp] = currentMax[pp]; - maxIndeces[pp] = maxIdx[pp]; - } -} - -void calcPdpAndFirstPathMin(hal_pktinfo_t *pktinfo, unsigned int *fftOutBuffer, unsigned int *pdpOutBuffer, unsigned int *totalpower, int *idxRes, unsigned int *valRes, int *firstPathDelay){ - - int ii, jj, pp, firstPathMin, maxIdx; - unsigned short *tdScaling = (unsigned short*)totalpower;//[MAX_RX*MAX_TX]; - unsigned int *currentValPtr =fftOutBuffer; - unsigned int tempVal, maxVals[MAX_RX*MAX_TX]; - int maxIndeces[MAX_RX*MAX_TX], firstPathRx[MAX_RX*MAX_TX]; - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; - //int NgDsfShift = pktinfo->NgDsfShift; - int numSubbands = 1<sigBw; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - int NgShift, subbandSize; - if(pktinfo->packetType>2){ // Ng=1 - NgShift = 0; - } - else - { - NgShift = pktinfo->Ng+1; - } - subbandSize = 1<<(6-NgShift); - - // determine scaling - for(ii=0;iifftSize/Ng)*powerH[jj+ii*nRx]); - tempVal = totalpower[jj+ii*nRx]; - if(tempVal){ - tempVal *= nTx*nRx*(numSubbands*subbandSize); - tempVal >>=(31-16-10); - tdScaling[2*(jj+ii*nRx)] = (unsigned short)((ifftSizeOsf*ifftSizeOsf<<10)/tempVal); //(1<<31) - tdScaling[2*(jj+ii*nRx)+1] = 0; // use 4 byte - } - else{ - tdScaling[jj+ii*nRx] =1; - } - } - } - - for(ii=0;iitempVal){ - tempVal = maxVals[jj+ii*nRx]; - maxIdx = jj+ii*nRx; - } - } - } - *idxRes = maxIndeces[maxIdx]; - *valRes = (tempVal*tdScaling[2*maxIdx])*nTx*nRx; - *firstPathDelay = firstPathMin; -} - - -#if (MAX_RX>1) || (MAX_TX>1) -void calcPdpAndMax(hal_pktinfo_t *pktinfo, unsigned int *fftOutBuffer, unsigned int *pdpOutBuffer, unsigned int *totalpower, int *idxRes, unsigned int *valRes){ - - int ii, jj, tt, maxIdx = -1; - unsigned short *scalePtr, *tdScaling = (unsigned short*)totalpower;//[MAX_RX*MAX_TX]; - unsigned int *ldPtr, *currentValPtr =fftOutBuffer; - unsigned int currentSum, tempVal, currentMax = 0; - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; - int numSubbands = 1<sigBw; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - int NgShift, subbandSize; -#if defined(ARM_DS5) - int reg6, reg7, reg8; -#elif !defined(ARM_GCC) - short tempI, tempQ; - unsigned short scaleVal; -#endif - - if(pktinfo->packetType>2){ // Ng=1 - NgShift = 0; - } - else - { - NgShift = pktinfo->Ng+1; - } - subbandSize = 1<<(6-NgShift); - - // determine scaling - for(ii=0;iifftSize/Ng)*powerH[jj+ii*nRx]); - tempVal = totalpower[jj+ii*nRx]; - if(tempVal){ - tempVal *= nTx*nRx*(numSubbands*subbandSize); - tempVal >>=(31-16-10); - tdScaling[2*(jj+ii*nRx)] = (unsigned short)((ifftSizeOsf*ifftSizeOsf<<10)/tempVal); //(1<<31) - tdScaling[2*(jj+ii*nRx)+1] = 0; // use 4 byte - } - else{ - tdScaling[jj+ii*nRx] =1; - } - } - } - - for(tt=0;tt>16)&0xffff; - tempVal = tempI*tempI; - currentSum += tempVal*scaleVal; - tempVal = tempQ*tempQ; - currentSum += tempVal*scaleVal; -#endif - } - } - pdpOutBuffer[tt]= currentSum; - currentValPtr++; - - if(currentSum>currentMax){ - currentMax =currentSum; - maxIdx = tt; - } - } - *idxRes = maxIdx; - *valRes = currentMax; -} - -void calcPdpAndMaxParallel(hal_pktinfo_t *pktinfo, unsigned int *fftOutBuffer, unsigned int *pdpOutBuffer, unsigned int *totalpower, int *idxRes, unsigned int *valRes) { - - int ii, jj, pp, tt, maxIdx = -1; - unsigned short *scalePtr, *tdScaling = (unsigned short*)totalpower;//[MAX_RX*MAX_TX]; - unsigned int *ldPtr, *currentValPtr = fftOutBuffer; - unsigned int currentSum, tempVal, currentMax = 0; - int nRx = pktinfo->nRx + 1; - int nTx = pktinfo->nTx + 1; - int numSubbands = 1 << pktinfo->sigBw; - int ifftSizeOsf = 1 << (pktinfo->fftSize + 6); - int NgShift, subbandSize; - short tempI, tempQ; - unsigned short scaleVal; - - if (pktinfo->packetType>2) { // Ng=1 - NgShift = 0; - } - else - { - NgShift = pktinfo->Ng + 1; - } - subbandSize = 1 << (6 - NgShift); - - // determine scaling - for (ii = 0;iifftSize/Ng)*powerH[jj+ii*nRx]); - tempVal = totalpower[jj + ii*nRx]; - if (tempVal) { - tempVal *= nTx*nRx*(numSubbands*subbandSize); - tempVal >>= (31 - 16 - 10); - if(ifftSizeOsf>1024) - tdScaling[2 * (jj + ii*nRx)] = (unsigned short)((ifftSizeOsf*ifftSizeOsf << 8) / (tempVal>>2)); //(1<<31) - else - tdScaling[2 * (jj + ii*nRx)] = (unsigned short)((ifftSizeOsf*ifftSizeOsf << 10) / tempVal ); //(1<<31) - tdScaling[2 * (jj + ii*nRx) + 1] = 0; // use 4 byte - } - else { - tdScaling[jj + ii*nRx] = 1; - } - } - } - - for (tt = 0;tt> 16) & 0xffff; - tempVal = tempI*tempI; - currentSum += tempVal*scaleVal; - tempVal = tempQ*tempQ; - currentSum += tempVal*scaleVal; - - ldPtr++; - scalePtr += 2; - } - ldPtr += NUM_PARALLEL*ifftSizeOsf-NUM_PARALLEL; - } - } - pdpOutBuffer[tt] = currentSum; - currentValPtr+= NUM_PARALLEL; - - if (currentSum>currentMax) { - currentMax = currentSum; - maxIdx = tt; - } - } - *idxRes = maxIdx; - *valRes = currentMax; -} - -#else - -void calcPdpAndMax(hal_pktinfo_t *pktinfo, unsigned int *fftOutBuffer, unsigned int *pdpOutBuffer, unsigned int *totalpower, int *idxRes, unsigned int *valRes){ - int tt, maxIdx = -1; - unsigned short *tdScaling = (unsigned short*)totalpower;//[MAX_RX*MAX_TX]; - unsigned int *currentValPtr =fftOutBuffer; - unsigned int currentSum, tempVal, currentMax = 0; - //int nRx = pktinfo->nRx+1; - //int nTx = pktinfo->nTx+1; - int numSubbands = 1<sigBw; - int NgShift = pktinfo->Ng+1; - int subbandSize = 1<<(6-NgShift); - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - short tempI, tempQ; - unsigned short scaleVal; - - // determine scaling - //1.0f/(nRx*nTx*(aoaSysParam->fftSize/Ng)*powerH[jj+ii*nRx]); - tempVal = *totalpower; - if(tempVal){ - tempVal *= numSubbands*subbandSize; - tempVal >>=(31-16-10); - tdScaling[0] = (unsigned short)((ifftSizeOsf*ifftSizeOsf<<10)/tempVal); //(1<<31) - tdScaling[1] = 0; // use 4 byte - } - else{ - *tdScaling =1; - } - - scaleVal = *tdScaling; - for(tt=0;tt>16)&0xffff; - tempVal = tempI*tempI; - currentSum = tempVal*scaleVal; - tempVal = tempQ*tempQ; - currentSum += tempVal*scaleVal; - pdpOutBuffer[tt]= currentSum; - currentValPtr++; - - if(currentSum>currentMax){ - currentMax =currentSum; - maxIdx = tt; - } - } - *idxRes = maxIdx; - *valRes = currentMax; -} -#endif - -int findFirstPath(hal_pktinfo_t *pktinfo, unsigned int *pdpOutBuffer, int maxIdx, unsigned int maxVal, int stride){ - - int ii, firstLen, secondLen, mask; - unsigned int y0, ym1, yp1, denominator, peakThresh, tempVal; - int numerator, delta, interpDelay, firstPath; - //unsigned short denomShort; - unsigned int *readPtr; - int NgDsfShift = pktinfo->NgDsfShift; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - - int totalLen, negLen, posLen; - - totalLen = ifftSizeOsf; - totalLen /= (pktinfo->packetType>3)? (TD_FRAC_OF_FFT_HE >> NgDsfShift):(TD_FRAC_OF_FFT >> NgDsfShift); - negLen = ifftSizeOsf / (FFT_ADV_FRAC >> NgDsfShift); // - posLen = totalLen - negLen; - - peakThresh = maxVal>>PEAK_THRESH_SHIFT; - if(peakThreshposLen){ // max in negative portion of PDP, no need to process positive part - firstPath = maxIdx-ifftSizeOsf; - firstLen = negLen+firstPath+2; - secondLen = 0; - } - else{ // max in positive portion of PDP, start with negative part, positive next - firstPath = maxIdx; - firstLen = negLen; - secondLen = firstPath; - } - - // negative part - readPtr = pdpOutBuffer+ stride*(ifftSizeOsf-negLen); - ym1 = 0; - y0 = maxVal; - yp1 = readPtr[0]; - for(ii=1;iiym1) && (y0>yp1)){ // local maximum - if(y0>peakThresh){ - firstPath =ii-1-negLen; - secondLen = 0; // no need for positive part - break; - } - } - } - - // positive part - readPtr = pdpOutBuffer; - for(ii=0;iiym1) && (y0>yp1)){ // local maximum - if(y0>peakThresh){ - firstPath =ii-1; - break; - } - } - } - - // interpolate peak - mask = ifftSizeOsf-1; - // shift to avoid overflow - ym1 = pdpOutBuffer[stride*((firstPath-1)&mask)]>>8; - y0 = pdpOutBuffer[stride*(firstPath&mask)]>>8; - yp1 = pdpOutBuffer[stride*((firstPath+1)&mask)]>>8; - - denominator = 2*(2*y0-yp1-ym1); - numerator = (yp1-ym1); - - tempVal = (numerator>0)? numerator: (-numerator); - ii = __clz(tempVal); - if(ii>TOA_FPATH_BIPT){ - tempVal<<=TOA_FPATH_BIPT; - delta = tempVal / denominator; - } - else{ - tempVal<<=(ii-1); - delta = tempVal / (denominator>>(TOA_FPATH_BIPT-ii+1)); - } - delta = (numerator>0)? delta:-delta; - - // format is 32.TOA_FPATH_BIPT - //delta /=denomShort; - //delta<<=TOA_FPATH_BIPT; - //delta /= denominator; - if((delta>(1<<(TOA_FPATH_BIPT-1))) || (-delta >(1<<(TOA_FPATH_BIPT-1)))){ - delta = 0; // overflow in division - } - interpDelay = (firstPath<nRx+1; - int nTx = pktinfo->nTx+1; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - unsigned int *currentValPtr, tempVal; - - offset = (peakIdx+(1<<7))>>8; // round back - if(offset<0){ - offset+=ifftSizeOsf; - } - currentValPtr = fftBuffer+offset; - - for(ii=0;ii -#if CONFIG_WLS_CSI_PROC - -#include -#include "wls_param_defines.h" -#include "wls_radix4Fft.h" -//#include "math.h" - -//#define ARM_DEBUG -#if defined(MAX_FFT_SIZE_2048) -const INT16 radix4FftTwiddleArr[2 * MAX_FFT_SIZE] = { 0x7FFF, 0x0000, 0x7FFF, 0x0065, 0x7FFF, 0x00C9, 0x7FFF, 0x012E, 0x7FFE, 0x0192, 0x7FFC, 0x01F7, 0x7FFA, 0x025B, 0x7FF8, 0x02C0, 0x7FF6, 0x0324, 0x7FF4, 0x0389, - 0x7FF1, 0x03ED, 0x7FED, 0x0452, 0x7FEA, 0x04B6, 0x7FE6, 0x051B, 0x7FE2, 0x057F, 0x7FDD, 0x05E3, 0x7FD9, 0x0648, 0x7FD3, 0x06AC, 0x7FCE, 0x0711, 0x7FC8, 0x0775, - 0x7FC2, 0x07D9, 0x7FBC, 0x083E, 0x7FB5, 0x08A2, 0x7FAE, 0x0906, 0x7FA7, 0x096B, 0x7FA0, 0x09CF, 0x7F98, 0x0A33, 0x7F90, 0x0A97, 0x7F87, 0x0AFB, 0x7F7E, 0x0B60, - 0x7F75, 0x0BC4, 0x7F6C, 0x0C28, 0x7F62, 0x0C8C, 0x7F58, 0x0CF0, 0x7F4E, 0x0D54, 0x7F43, 0x0DB8, 0x7F38, 0x0E1C, 0x7F2D, 0x0E80, 0x7F22, 0x0EE4, 0x7F16, 0x0F47, - 0x7F0A, 0x0FAB, 0x7EFD, 0x100F, 0x7EF0, 0x1073, 0x7EE3, 0x10D6, 0x7ED6, 0x113A, 0x7EC8, 0x119E, 0x7EBA, 0x1201, 0x7EAC, 0x1265, 0x7E9D, 0x12C8, 0x7E8E, 0x132B, - 0x7E7F, 0x138F, 0x7E70, 0x13F2, 0x7E60, 0x1455, 0x7E50, 0x14B9, 0x7E3F, 0x151C, 0x7E2F, 0x157F, 0x7E1E, 0x15E2, 0x7E0C, 0x1645, 0x7DFB, 0x16A8, 0x7DE9, 0x170B, - 0x7DD6, 0x176E, 0x7DC4, 0x17D1, 0x7DB1, 0x1833, 0x7D9E, 0x1896, 0x7D8A, 0x18F9, 0x7D77, 0x195B, 0x7D63, 0x19BE, 0x7D4E, 0x1A20, 0x7D3A, 0x1A83, 0x7D25, 0x1AE5, - 0x7D0F, 0x1B47, 0x7CFA, 0x1BA9, 0x7CE4, 0x1C0C, 0x7CCE, 0x1C6E, 0x7CB7, 0x1CD0, 0x7CA0, 0x1D31, 0x7C89, 0x1D93, 0x7C72, 0x1DF5, 0x7C5A, 0x1E57, 0x7C42, 0x1EB8, - 0x7C2A, 0x1F1A, 0x7C11, 0x1F7B, 0x7BF9, 0x1FDD, 0x7BDF, 0x203E, 0x7BC6, 0x209F, 0x7BAC, 0x2101, 0x7B92, 0x2162, 0x7B78, 0x21C3, 0x7B5D, 0x2224, 0x7B42, 0x2284, - 0x7B27, 0x22E5, 0x7B0B, 0x2346, 0x7AEF, 0x23A7, 0x7AD3, 0x2407, 0x7AB7, 0x2467, 0x7A9A, 0x24C8, 0x7A7D, 0x2528, 0x7A60, 0x2588, 0x7A42, 0x25E8, 0x7A24, 0x2648, - 0x7A06, 0x26A8, 0x79E7, 0x2708, 0x79C9, 0x2768, 0x79AA, 0x27C7, 0x798A, 0x2827, 0x796A, 0x2886, 0x794A, 0x28E5, 0x792A, 0x2945, 0x790A, 0x29A4, 0x78E9, 0x2A03, - 0x78C8, 0x2A62, 0x78A6, 0x2AC1, 0x7885, 0x2B1F, 0x7863, 0x2B7E, 0x7840, 0x2BDC, 0x781E, 0x2C3B, 0x77FB, 0x2C99, 0x77D8, 0x2CF7, 0x77B4, 0x2D55, 0x7790, 0x2DB3, - 0x776C, 0x2E11, 0x7748, 0x2E6F, 0x7723, 0x2ECC, 0x76FE, 0x2F2A, 0x76D9, 0x2F87, 0x76B4, 0x2FE5, 0x768E, 0x3042, 0x7668, 0x309F, 0x7642, 0x30FC, 0x761B, 0x3159, - 0x75F4, 0x31B5, 0x75CD, 0x3212, 0x75A6, 0x326E, 0x757E, 0x32CB, 0x7556, 0x3327, 0x752D, 0x3383, 0x7505, 0x33DF, 0x74DC, 0x343B, 0x74B3, 0x3497, 0x7489, 0x34F2, - 0x7460, 0x354E, 0x7436, 0x35A9, 0x740B, 0x3604, 0x73E1, 0x365F, 0x73B6, 0x36BA, 0x738B, 0x3715, 0x735F, 0x3770, 0x7334, 0x37CA, 0x7308, 0x3825, 0x72DC, 0x387F, - 0x72AF, 0x38D9, 0x7282, 0x3933, 0x7255, 0x398D, 0x7228, 0x39E7, 0x71FA, 0x3A40, 0x71CC, 0x3A9A, 0x719E, 0x3AF3, 0x7170, 0x3B4C, 0x7141, 0x3BA5, 0x7112, 0x3BFE, - 0x70E3, 0x3C57, 0x70B3, 0x3CAF, 0x7083, 0x3D08, 0x7053, 0x3D60, 0x7023, 0x3DB8, 0x6FF2, 0x3E10, 0x6FC2, 0x3E68, 0x6F90, 0x3EC0, 0x6F5F, 0x3F17, 0x6F2D, 0x3F6F, - 0x6EFB, 0x3FC6, 0x6EC9, 0x401D, 0x6E97, 0x4074, 0x6E64, 0x40CB, 0x6E31, 0x4121, 0x6DFE, 0x4178, 0x6DCA, 0x41CE, 0x6D96, 0x4224, 0x6D62, 0x427A, 0x6D2E, 0x42D0, - 0x6CF9, 0x4326, 0x6CC4, 0x437B, 0x6C8F, 0x43D1, 0x6C5A, 0x4426, 0x6C24, 0x447B, 0x6BEE, 0x44D0, 0x6BB8, 0x4524, 0x6B82, 0x4579, 0x6B4B, 0x45CD, 0x6B14, 0x4621, - 0x6ADD, 0x4675, 0x6AA5, 0x46C9, 0x6A6E, 0x471D, 0x6A36, 0x4770, 0x69FD, 0x47C4, 0x69C5, 0x4817, 0x698C, 0x486A, 0x6953, 0x48BD, 0x691A, 0x490F, 0x68E0, 0x4962, - 0x68A7, 0x49B4, 0x686D, 0x4A06, 0x6832, 0x4A58, 0x67F8, 0x4AAA, 0x67BD, 0x4AFB, 0x6782, 0x4B4D, 0x6747, 0x4B9E, 0x670B, 0x4BEF, 0x66D0, 0x4C40, 0x6693, 0x4C91, - 0x6657, 0x4CE1, 0x661B, 0x4D31, 0x65DE, 0x4D81, 0x65A1, 0x4DD1, 0x6564, 0x4E21, 0x6526, 0x4E71, 0x64E9, 0x4EC0, 0x64AB, 0x4F0F, 0x646C, 0x4F5E, 0x642E, 0x4FAD, - 0x63EF, 0x4FFB, 0x63B0, 0x504A, 0x6371, 0x5098, 0x6332, 0x50E6, 0x62F2, 0x5134, 0x62B2, 0x5181, 0x6272, 0x51CF, 0x6232, 0x521C, 0x61F1, 0x5269, 0x61B0, 0x52B6, - 0x616F, 0x5303, 0x612E, 0x534F, 0x60EC, 0x539B, 0x60AA, 0x53E7, 0x6068, 0x5433, 0x6026, 0x547F, 0x5FE4, 0x54CA, 0x5FA1, 0x5515, 0x5F5E, 0x5560, 0x5F1B, 0x55AB, - 0x5ED7, 0x55F6, 0x5E94, 0x5640, 0x5E50, 0x568A, 0x5E0C, 0x56D4, 0x5DC8, 0x571E, 0x5D83, 0x5767, 0x5D3E, 0x57B1, 0x5CF9, 0x57FA, 0x5CB4, 0x5843, 0x5C6F, 0x588C, - 0x5C29, 0x58D4, 0x5BE3, 0x591C, 0x5B9D, 0x5964, 0x5B57, 0x59AC, 0x5B10, 0x59F4, 0x5AC9, 0x5A3B, 0x5A82, 0x5A82, 0x5A3B, 0x5AC9, 0x59F4, 0x5B10, 0x59AC, 0x5B57, - 0x5964, 0x5B9D, 0x591C, 0x5BE3, 0x58D4, 0x5C29, 0x588C, 0x5C6F, 0x5843, 0x5CB4, 0x57FA, 0x5CF9, 0x57B1, 0x5D3E, 0x5767, 0x5D83, 0x571E, 0x5DC8, 0x56D4, 0x5E0C, - 0x568A, 0x5E50, 0x5640, 0x5E94, 0x55F6, 0x5ED7, 0x55AB, 0x5F1B, 0x5560, 0x5F5E, 0x5515, 0x5FA1, 0x54CA, 0x5FE4, 0x547F, 0x6026, 0x5433, 0x6068, 0x53E7, 0x60AA, - 0x539B, 0x60EC, 0x534F, 0x612E, 0x5303, 0x616F, 0x52B6, 0x61B0, 0x5269, 0x61F1, 0x521C, 0x6232, 0x51CF, 0x6272, 0x5181, 0x62B2, 0x5134, 0x62F2, 0x50E6, 0x6332, - 0x5098, 0x6371, 0x504A, 0x63B0, 0x4FFB, 0x63EF, 0x4FAD, 0x642E, 0x4F5E, 0x646C, 0x4F0F, 0x64AB, 0x4EC0, 0x64E9, 0x4E71, 0x6526, 0x4E21, 0x6564, 0x4DD1, 0x65A1, - 0x4D81, 0x65DE, 0x4D31, 0x661B, 0x4CE1, 0x6657, 0x4C91, 0x6693, 0x4C40, 0x66D0, 0x4BEF, 0x670B, 0x4B9E, 0x6747, 0x4B4D, 0x6782, 0x4AFB, 0x67BD, 0x4AAA, 0x67F8, - 0x4A58, 0x6832, 0x4A06, 0x686D, 0x49B4, 0x68A7, 0x4962, 0x68E0, 0x490F, 0x691A, 0x48BD, 0x6953, 0x486A, 0x698C, 0x4817, 0x69C5, 0x47C4, 0x69FD, 0x4770, 0x6A36, - 0x471D, 0x6A6E, 0x46C9, 0x6AA5, 0x4675, 0x6ADD, 0x4621, 0x6B14, 0x45CD, 0x6B4B, 0x4579, 0x6B82, 0x4524, 0x6BB8, 0x44D0, 0x6BEE, 0x447B, 0x6C24, 0x4426, 0x6C5A, - 0x43D1, 0x6C8F, 0x437B, 0x6CC4, 0x4326, 0x6CF9, 0x42D0, 0x6D2E, 0x427A, 0x6D62, 0x4224, 0x6D96, 0x41CE, 0x6DCA, 0x4178, 0x6DFE, 0x4121, 0x6E31, 0x40CB, 0x6E64, - 0x4074, 0x6E97, 0x401D, 0x6EC9, 0x3FC6, 0x6EFB, 0x3F6F, 0x6F2D, 0x3F17, 0x6F5F, 0x3EC0, 0x6F90, 0x3E68, 0x6FC2, 0x3E10, 0x6FF2, 0x3DB8, 0x7023, 0x3D60, 0x7053, - 0x3D08, 0x7083, 0x3CAF, 0x70B3, 0x3C57, 0x70E3, 0x3BFE, 0x7112, 0x3BA5, 0x7141, 0x3B4C, 0x7170, 0x3AF3, 0x719E, 0x3A9A, 0x71CC, 0x3A40, 0x71FA, 0x39E7, 0x7228, - 0x398D, 0x7255, 0x3933, 0x7282, 0x38D9, 0x72AF, 0x387F, 0x72DC, 0x3825, 0x7308, 0x37CA, 0x7334, 0x3770, 0x735F, 0x3715, 0x738B, 0x36BA, 0x73B6, 0x365F, 0x73E1, - 0x3604, 0x740B, 0x35A9, 0x7436, 0x354E, 0x7460, 0x34F2, 0x7489, 0x3497, 0x74B3, 0x343B, 0x74DC, 0x33DF, 0x7505, 0x3383, 0x752D, 0x3327, 0x7556, 0x32CB, 0x757E, - 0x326E, 0x75A6, 0x3212, 0x75CD, 0x31B5, 0x75F4, 0x3159, 0x761B, 0x30FC, 0x7642, 0x309F, 0x7668, 0x3042, 0x768E, 0x2FE5, 0x76B4, 0x2F87, 0x76D9, 0x2F2A, 0x76FE, - 0x2ECC, 0x7723, 0x2E6F, 0x7748, 0x2E11, 0x776C, 0x2DB3, 0x7790, 0x2D55, 0x77B4, 0x2CF7, 0x77D8, 0x2C99, 0x77FB, 0x2C3B, 0x781E, 0x2BDC, 0x7840, 0x2B7E, 0x7863, - 0x2B1F, 0x7885, 0x2AC1, 0x78A6, 0x2A62, 0x78C8, 0x2A03, 0x78E9, 0x29A4, 0x790A, 0x2945, 0x792A, 0x28E5, 0x794A, 0x2886, 0x796A, 0x2827, 0x798A, 0x27C7, 0x79AA, - 0x2768, 0x79C9, 0x2708, 0x79E7, 0x26A8, 0x7A06, 0x2648, 0x7A24, 0x25E8, 0x7A42, 0x2588, 0x7A60, 0x2528, 0x7A7D, 0x24C8, 0x7A9A, 0x2467, 0x7AB7, 0x2407, 0x7AD3, - 0x23A7, 0x7AEF, 0x2346, 0x7B0B, 0x22E5, 0x7B27, 0x2284, 0x7B42, 0x2224, 0x7B5D, 0x21C3, 0x7B78, 0x2162, 0x7B92, 0x2101, 0x7BAC, 0x209F, 0x7BC6, 0x203E, 0x7BDF, - 0x1FDD, 0x7BF9, 0x1F7B, 0x7C11, 0x1F1A, 0x7C2A, 0x1EB8, 0x7C42, 0x1E57, 0x7C5A, 0x1DF5, 0x7C72, 0x1D93, 0x7C89, 0x1D31, 0x7CA0, 0x1CD0, 0x7CB7, 0x1C6E, 0x7CCE, - 0x1C0C, 0x7CE4, 0x1BA9, 0x7CFA, 0x1B47, 0x7D0F, 0x1AE5, 0x7D25, 0x1A83, 0x7D3A, 0x1A20, 0x7D4E, 0x19BE, 0x7D63, 0x195B, 0x7D77, 0x18F9, 0x7D8A, 0x1896, 0x7D9E, - 0x1833, 0x7DB1, 0x17D1, 0x7DC4, 0x176E, 0x7DD6, 0x170B, 0x7DE9, 0x16A8, 0x7DFB, 0x1645, 0x7E0C, 0x15E2, 0x7E1E, 0x157F, 0x7E2F, 0x151C, 0x7E3F, 0x14B9, 0x7E50, - 0x1455, 0x7E60, 0x13F2, 0x7E70, 0x138F, 0x7E7F, 0x132B, 0x7E8E, 0x12C8, 0x7E9D, 0x1265, 0x7EAC, 0x1201, 0x7EBA, 0x119E, 0x7EC8, 0x113A, 0x7ED6, 0x10D6, 0x7EE3, - 0x1073, 0x7EF0, 0x100F, 0x7EFD, 0x0FAB, 0x7F0A, 0x0F47, 0x7F16, 0x0EE4, 0x7F22, 0x0E80, 0x7F2D, 0x0E1C, 0x7F38, 0x0DB8, 0x7F43, 0x0D54, 0x7F4E, 0x0CF0, 0x7F58, - 0x0C8C, 0x7F62, 0x0C28, 0x7F6C, 0x0BC4, 0x7F75, 0x0B60, 0x7F7E, 0x0AFB, 0x7F87, 0x0A97, 0x7F90, 0x0A33, 0x7F98, 0x09CF, 0x7FA0, 0x096B, 0x7FA7, 0x0906, 0x7FAE, - 0x08A2, 0x7FB5, 0x083E, 0x7FBC, 0x07D9, 0x7FC2, 0x0775, 0x7FC8, 0x0711, 0x7FCE, 0x06AC, 0x7FD3, 0x0648, 0x7FD9, 0x05E3, 0x7FDD, 0x057F, 0x7FE2, 0x051B, 0x7FE6, - 0x04B6, 0x7FEA, 0x0452, 0x7FED, 0x03ED, 0x7FF1, 0x0389, 0x7FF4, 0x0324, 0x7FF6, 0x02C0, 0x7FF8, 0x025B, 0x7FFA, 0x01F7, 0x7FFC, 0x0192, 0x7FFE, 0x012E, 0x7FFF, - 0x00C9, 0x7FFF, 0x0065, 0x7FFF, 0x0000, 0x7FFF, 0xFF9B, 0x7FFF, 0xFF37, 0x7FFF, 0xFED2, 0x7FFF, 0xFE6E, 0x7FFE, 0xFE09, 0x7FFC, 0xFDA5, 0x7FFA, 0xFD40, 0x7FF8, - 0xFCDC, 0x7FF6, 0xFC77, 0x7FF4, 0xFC13, 0x7FF1, 0xFBAE, 0x7FED, 0xFB4A, 0x7FEA, 0xFAE5, 0x7FE6, 0xFA81, 0x7FE2, 0xFA1D, 0x7FDD, 0xF9B8, 0x7FD9, 0xF954, 0x7FD3, - 0xF8EF, 0x7FCE, 0xF88B, 0x7FC8, 0xF827, 0x7FC2, 0xF7C2, 0x7FBC, 0xF75E, 0x7FB5, 0xF6FA, 0x7FAE, 0xF695, 0x7FA7, 0xF631, 0x7FA0, 0xF5CD, 0x7F98, 0xF569, 0x7F90, - 0xF505, 0x7F87, 0xF4A0, 0x7F7E, 0xF43C, 0x7F75, 0xF3D8, 0x7F6C, 0xF374, 0x7F62, 0xF310, 0x7F58, 0xF2AC, 0x7F4E, 0xF248, 0x7F43, 0xF1E4, 0x7F38, 0xF180, 0x7F2D, - 0xF11C, 0x7F22, 0xF0B9, 0x7F16, 0xF055, 0x7F0A, 0xEFF1, 0x7EFD, 0xEF8D, 0x7EF0, 0xEF2A, 0x7EE3, 0xEEC6, 0x7ED6, 0xEE62, 0x7EC8, 0xEDFF, 0x7EBA, 0xED9B, 0x7EAC, - 0xED38, 0x7E9D, 0xECD5, 0x7E8E, 0xEC71, 0x7E7F, 0xEC0E, 0x7E70, 0xEBAB, 0x7E60, 0xEB47, 0x7E50, 0xEAE4, 0x7E3F, 0xEA81, 0x7E2F, 0xEA1E, 0x7E1E, 0xE9BB, 0x7E0C, - 0xE958, 0x7DFB, 0xE8F5, 0x7DE9, 0xE892, 0x7DD6, 0xE82F, 0x7DC4, 0xE7CD, 0x7DB1, 0xE76A, 0x7D9E, 0xE707, 0x7D8A, 0xE6A5, 0x7D77, 0xE642, 0x7D63, 0xE5E0, 0x7D4E, - 0xE57D, 0x7D3A, 0xE51B, 0x7D25, 0xE4B9, 0x7D0F, 0xE457, 0x7CFA, 0xE3F4, 0x7CE4, 0xE392, 0x7CCE, 0xE330, 0x7CB7, 0xE2CF, 0x7CA0, 0xE26D, 0x7C89, 0xE20B, 0x7C72, - 0xE1A9, 0x7C5A, 0xE148, 0x7C42, 0xE0E6, 0x7C2A, 0xE085, 0x7C11, 0xE023, 0x7BF9, 0xDFC2, 0x7BDF, 0xDF61, 0x7BC6, 0xDEFF, 0x7BAC, 0xDE9E, 0x7B92, 0xDE3D, 0x7B78, - 0xDDDC, 0x7B5D, 0xDD7C, 0x7B42, 0xDD1B, 0x7B27, 0xDCBA, 0x7B0B, 0xDC59, 0x7AEF, 0xDBF9, 0x7AD3, 0xDB99, 0x7AB7, 0xDB38, 0x7A9A, 0xDAD8, 0x7A7D, 0xDA78, 0x7A60, - 0xDA18, 0x7A42, 0xD9B8, 0x7A24, 0xD958, 0x7A06, 0xD8F8, 0x79E7, 0xD898, 0x79C9, 0xD839, 0x79AA, 0xD7D9, 0x798A, 0xD77A, 0x796A, 0xD71B, 0x794A, 0xD6BB, 0x792A, - 0xD65C, 0x790A, 0xD5FD, 0x78E9, 0xD59E, 0x78C8, 0xD53F, 0x78A6, 0xD4E1, 0x7885, 0xD482, 0x7863, 0xD424, 0x7840, 0xD3C5, 0x781E, 0xD367, 0x77FB, 0xD309, 0x77D8, - 0xD2AB, 0x77B4, 0xD24D, 0x7790, 0xD1EF, 0x776C, 0xD191, 0x7748, 0xD134, 0x7723, 0xD0D6, 0x76FE, 0xD079, 0x76D9, 0xD01B, 0x76B4, 0xCFBE, 0x768E, 0xCF61, 0x7668, - 0xCF04, 0x7642, 0xCEA7, 0x761B, 0xCE4B, 0x75F4, 0xCDEE, 0x75CD, 0xCD92, 0x75A6, 0xCD35, 0x757E, 0xCCD9, 0x7556, 0xCC7D, 0x752D, 0xCC21, 0x7505, 0xCBC5, 0x74DC, - 0xCB69, 0x74B3, 0xCB0E, 0x7489, 0xCAB2, 0x7460, 0xCA57, 0x7436, 0xC9FC, 0x740B, 0xC9A1, 0x73E1, 0xC946, 0x73B6, 0xC8EB, 0x738B, 0xC890, 0x735F, 0xC836, 0x7334, - 0xC7DB, 0x7308, 0xC781, 0x72DC, 0xC727, 0x72AF, 0xC6CD, 0x7282, 0xC673, 0x7255, 0xC619, 0x7228, 0xC5C0, 0x71FA, 0xC566, 0x71CC, 0xC50D, 0x719E, 0xC4B4, 0x7170, - 0xC45B, 0x7141, 0xC402, 0x7112, 0xC3A9, 0x70E3, 0xC351, 0x70B3, 0xC2F8, 0x7083, 0xC2A0, 0x7053, 0xC248, 0x7023, 0xC1F0, 0x6FF2, 0xC198, 0x6FC2, 0xC140, 0x6F90, - 0xC0E9, 0x6F5F, 0xC091, 0x6F2D, 0xC03A, 0x6EFB, 0xBFE3, 0x6EC9, 0xBF8C, 0x6E97, 0xBF35, 0x6E64, 0xBEDF, 0x6E31, 0xBE88, 0x6DFE, 0xBE32, 0x6DCA, 0xBDDC, 0x6D96, - 0xBD86, 0x6D62, 0xBD30, 0x6D2E, 0xBCDA, 0x6CF9, 0xBC85, 0x6CC4, 0xBC2F, 0x6C8F, 0xBBDA, 0x6C5A, 0xBB85, 0x6C24, 0xBB30, 0x6BEE, 0xBADC, 0x6BB8, 0xBA87, 0x6B82, - 0xBA33, 0x6B4B, 0xB9DF, 0x6B14, 0xB98B, 0x6ADD, 0xB937, 0x6AA5, 0xB8E3, 0x6A6E, 0xB890, 0x6A36, 0xB83C, 0x69FD, 0xB7E9, 0x69C5, 0xB796, 0x698C, 0xB743, 0x6953, - 0xB6F1, 0x691A, 0xB69E, 0x68E0, 0xB64C, 0x68A7, 0xB5FA, 0x686D, 0xB5A8, 0x6832, 0xB556, 0x67F8, 0xB505, 0x67BD, 0xB4B3, 0x6782, 0xB462, 0x6747, 0xB411, 0x670B, - 0xB3C0, 0x66D0, 0xB36F, 0x6693, 0xB31F, 0x6657, 0xB2CF, 0x661B, 0xB27F, 0x65DE, 0xB22F, 0x65A1, 0xB1DF, 0x6564, 0xB18F, 0x6526, 0xB140, 0x64E9, 0xB0F1, 0x64AB, - 0xB0A2, 0x646C, 0xB053, 0x642E, 0xB005, 0x63EF, 0xAFB6, 0x63B0, 0xAF68, 0x6371, 0xAF1A, 0x6332, 0xAECC, 0x62F2, 0xAE7F, 0x62B2, 0xAE31, 0x6272, 0xADE4, 0x6232, - 0xAD97, 0x61F1, 0xAD4A, 0x61B0, 0xACFD, 0x616F, 0xACB1, 0x612E, 0xAC65, 0x60EC, 0xAC19, 0x60AA, 0xABCD, 0x6068, 0xAB81, 0x6026, 0xAB36, 0x5FE4, 0xAAEB, 0x5FA1, - 0xAAA0, 0x5F5E, 0xAA55, 0x5F1B, 0xAA0A, 0x5ED7, 0xA9C0, 0x5E94, 0xA976, 0x5E50, 0xA92C, 0x5E0C, 0xA8E2, 0x5DC8, 0xA899, 0x5D83, 0xA84F, 0x5D3E, 0xA806, 0x5CF9, - 0xA7BD, 0x5CB4, 0xA774, 0x5C6F, 0xA72C, 0x5C29, 0xA6E4, 0x5BE3, 0xA69C, 0x5B9D, 0xA654, 0x5B57, 0xA60C, 0x5B10, 0xA5C5, 0x5AC9, 0xA57E, 0x5A82, 0xA537, 0x5A3B, - 0xA4F0, 0x59F4, 0xA4A9, 0x59AC, 0xA463, 0x5964, 0xA41D, 0x591C, 0xA3D7, 0x58D4, 0xA391, 0x588C, 0xA34C, 0x5843, 0xA307, 0x57FA, 0xA2C2, 0x57B1, 0xA27D, 0x5767, - 0xA238, 0x571E, 0xA1F4, 0x56D4, 0xA1B0, 0x568A, 0xA16C, 0x5640, 0xA129, 0x55F6, 0xA0E5, 0x55AB, 0xA0A2, 0x5560, 0xA05F, 0x5515, 0xA01C, 0x54CA, 0x9FDA, 0x547F, - 0x9F98, 0x5433, 0x9F56, 0x53E7, 0x9F14, 0x539B, 0x9ED2, 0x534F, 0x9E91, 0x5303, 0x9E50, 0x52B6, 0x9E0F, 0x5269, 0x9DCE, 0x521C, 0x9D8E, 0x51CF, 0x9D4E, 0x5181, - 0x9D0E, 0x5134, 0x9CCE, 0x50E6, 0x9C8F, 0x5098, 0x9C50, 0x504A, 0x9C11, 0x4FFB, 0x9BD2, 0x4FAD, 0x9B94, 0x4F5E, 0x9B55, 0x4F0F, 0x9B17, 0x4EC0, 0x9ADA, 0x4E71, - 0x9A9C, 0x4E21, 0x9A5F, 0x4DD1, 0x9A22, 0x4D81, 0x99E5, 0x4D31, 0x99A9, 0x4CE1, 0x996D, 0x4C91, 0x9930, 0x4C40, 0x98F5, 0x4BEF, 0x98B9, 0x4B9E, 0x987E, 0x4B4D, - 0x9843, 0x4AFB, 0x9808, 0x4AAA, 0x97CE, 0x4A58, 0x9793, 0x4A06, 0x9759, 0x49B4, 0x9720, 0x4962, 0x96E6, 0x490F, 0x96AD, 0x48BD, 0x9674, 0x486A, 0x963B, 0x4817, - 0x9603, 0x47C4, 0x95CA, 0x4770, 0x9592, 0x471D, 0x955B, 0x46C9, 0x9523, 0x4675, 0x94EC, 0x4621, 0x94B5, 0x45CD, 0x947E, 0x4579, 0x9448, 0x4524, 0x9412, 0x44D0, - 0x93DC, 0x447B, 0x93A6, 0x4426, 0x9371, 0x43D1, 0x933C, 0x437B, 0x9307, 0x4326, 0x92D2, 0x42D0, 0x929E, 0x427A, 0x926A, 0x4224, 0x9236, 0x41CE, 0x9202, 0x4178, - 0x91CF, 0x4121, 0x919C, 0x40CB, 0x9169, 0x4074, 0x9137, 0x401D, 0x9105, 0x3FC6, 0x90D3, 0x3F6F, 0x90A1, 0x3F17, 0x9070, 0x3EC0, 0x903E, 0x3E68, 0x900E, 0x3E10, - 0x8FDD, 0x3DB8, 0x8FAD, 0x3D60, 0x8F7D, 0x3D08, 0x8F4D, 0x3CAF, 0x8F1D, 0x3C57, 0x8EEE, 0x3BFE, 0x8EBF, 0x3BA5, 0x8E90, 0x3B4C, 0x8E62, 0x3AF3, 0x8E34, 0x3A9A, - 0x8E06, 0x3A40, 0x8DD8, 0x39E7, 0x8DAB, 0x398D, 0x8D7E, 0x3933, 0x8D51, 0x38D9, 0x8D24, 0x387F, 0x8CF8, 0x3825, 0x8CCC, 0x37CA, 0x8CA1, 0x3770, 0x8C75, 0x3715, - 0x8C4A, 0x36BA, 0x8C1F, 0x365F, 0x8BF5, 0x3604, 0x8BCA, 0x35A9, 0x8BA0, 0x354E, 0x8B77, 0x34F2, 0x8B4D, 0x3497, 0x8B24, 0x343B, 0x8AFB, 0x33DF, 0x8AD3, 0x3383, - 0x8AAA, 0x3327, 0x8A82, 0x32CB, 0x8A5A, 0x326E, 0x8A33, 0x3212, 0x8A0C, 0x31B5, 0x89E5, 0x3159, 0x89BE, 0x30FC, 0x8998, 0x309F, 0x8972, 0x3042, 0x894C, 0x2FE5, - 0x8927, 0x2F87, 0x8902, 0x2F2A, 0x88DD, 0x2ECC, 0x88B8, 0x2E6F, 0x8894, 0x2E11, 0x8870, 0x2DB3, 0x884C, 0x2D55, 0x8828, 0x2CF7, 0x8805, 0x2C99, 0x87E2, 0x2C3B, - 0x87C0, 0x2BDC, 0x879D, 0x2B7E, 0x877B, 0x2B1F, 0x875A, 0x2AC1, 0x8738, 0x2A62, 0x8717, 0x2A03, 0x86F6, 0x29A4, 0x86D6, 0x2945, 0x86B6, 0x28E5, 0x8696, 0x2886, - 0x8676, 0x2827, 0x8656, 0x27C7, 0x8637, 0x2768, 0x8619, 0x2708, 0x85FA, 0x26A8, 0x85DC, 0x2648, 0x85BE, 0x25E8, 0x85A0, 0x2588, 0x8583, 0x2528, 0x8566, 0x24C8, - 0x8549, 0x2467, 0x852D, 0x2407, 0x8511, 0x23A7, 0x84F5, 0x2346, 0x84D9, 0x22E5, 0x84BE, 0x2284, 0x84A3, 0x2224, 0x8488, 0x21C3, 0x846E, 0x2162, 0x8454, 0x2101, - 0x843A, 0x209F, 0x8421, 0x203E, 0x8407, 0x1FDD, 0x83EF, 0x1F7B, 0x83D6, 0x1F1A, 0x83BE, 0x1EB8, 0x83A6, 0x1E57, 0x838E, 0x1DF5, 0x8377, 0x1D93, 0x8360, 0x1D31, - 0x8349, 0x1CD0, 0x8332, 0x1C6E, 0x831C, 0x1C0C, 0x8306, 0x1BA9, 0x82F1, 0x1B47, 0x82DB, 0x1AE5, 0x82C6, 0x1A83, 0x82B2, 0x1A20, 0x829D, 0x19BE, 0x8289, 0x195B, - 0x8276, 0x18F9, 0x8262, 0x1896, 0x824F, 0x1833, 0x823C, 0x17D1, 0x822A, 0x176E, 0x8217, 0x170B, 0x8205, 0x16A8, 0x81F4, 0x1645, 0x81E2, 0x15E2, 0x81D1, 0x157F, - 0x81C1, 0x151C, 0x81B0, 0x14B9, 0x81A0, 0x1455, 0x8190, 0x13F2, 0x8181, 0x138F, 0x8172, 0x132B, 0x8163, 0x12C8, 0x8154, 0x1265, 0x8146, 0x1201, 0x8138, 0x119E, - 0x812A, 0x113A, 0x811D, 0x10D6, 0x8110, 0x1073, 0x8103, 0x100F, 0x80F6, 0x0FAB, 0x80EA, 0x0F47, 0x80DE, 0x0EE4, 0x80D3, 0x0E80, 0x80C8, 0x0E1C, 0x80BD, 0x0DB8, - 0x80B2, 0x0D54, 0x80A8, 0x0CF0, 0x809E, 0x0C8C, 0x8094, 0x0C28, 0x808B, 0x0BC4, 0x8082, 0x0B60, 0x8079, 0x0AFB, 0x8070, 0x0A97, 0x8068, 0x0A33, 0x8060, 0x09CF, - 0x8059, 0x096B, 0x8052, 0x0906, 0x804B, 0x08A2, 0x8044, 0x083E, 0x803E, 0x07D9, 0x8038, 0x0775, 0x8032, 0x0711, 0x802D, 0x06AC, 0x8027, 0x0648, 0x8023, 0x05E3, - 0x801E, 0x057F, 0x801A, 0x051B, 0x8016, 0x04B6, 0x8013, 0x0452, 0x800F, 0x03ED, 0x800C, 0x0389, 0x800A, 0x0324, 0x8008, 0x02C0, 0x8006, 0x025B, 0x8004, 0x01F7, - 0x8002, 0x0192, 0x8001, 0x012E, 0x8001, 0x00C9, 0x8000, 0x0065, 0x8000, 0x0000, 0x8000, 0xFF9B, 0x8001, 0xFF37, 0x8001, 0xFED2, 0x8002, 0xFE6E, 0x8004, 0xFE09, - 0x8006, 0xFDA5, 0x8008, 0xFD40, 0x800A, 0xFCDC, 0x800C, 0xFC77, 0x800F, 0xFC13, 0x8013, 0xFBAE, 0x8016, 0xFB4A, 0x801A, 0xFAE5, 0x801E, 0xFA81, 0x8023, 0xFA1D, - 0x8027, 0xF9B8, 0x802D, 0xF954, 0x8032, 0xF8EF, 0x8038, 0xF88B, 0x803E, 0xF827, 0x8044, 0xF7C2, 0x804B, 0xF75E, 0x8052, 0xF6FA, 0x8059, 0xF695, 0x8060, 0xF631, - 0x8068, 0xF5CD, 0x8070, 0xF569, 0x8079, 0xF505, 0x8082, 0xF4A0, 0x808B, 0xF43C, 0x8094, 0xF3D8, 0x809E, 0xF374, 0x80A8, 0xF310, 0x80B2, 0xF2AC, 0x80BD, 0xF248, - 0x80C8, 0xF1E4, 0x80D3, 0xF180, 0x80DE, 0xF11C, 0x80EA, 0xF0B9, 0x80F6, 0xF055, 0x8103, 0xEFF1, 0x8110, 0xEF8D, 0x811D, 0xEF2A, 0x812A, 0xEEC6, 0x8138, 0xEE62, - 0x8146, 0xEDFF, 0x8154, 0xED9B, 0x8163, 0xED38, 0x8172, 0xECD5, 0x8181, 0xEC71, 0x8190, 0xEC0E, 0x81A0, 0xEBAB, 0x81B0, 0xEB47, 0x81C1, 0xEAE4, 0x81D1, 0xEA81, - 0x81E2, 0xEA1E, 0x81F4, 0xE9BB, 0x8205, 0xE958, 0x8217, 0xE8F5, 0x822A, 0xE892, 0x823C, 0xE82F, 0x824F, 0xE7CD, 0x8262, 0xE76A, 0x8276, 0xE707, 0x8289, 0xE6A5, - 0x829D, 0xE642, 0x82B2, 0xE5E0, 0x82C6, 0xE57D, 0x82DB, 0xE51B, 0x82F1, 0xE4B9, 0x8306, 0xE457, 0x831C, 0xE3F4, 0x8332, 0xE392, 0x8349, 0xE330, 0x8360, 0xE2CF, - 0x8377, 0xE26D, 0x838E, 0xE20B, 0x83A6, 0xE1A9, 0x83BE, 0xE148, 0x83D6, 0xE0E6, 0x83EF, 0xE085, 0x8407, 0xE023, 0x8421, 0xDFC2, 0x843A, 0xDF61, 0x8454, 0xDEFF, - 0x846E, 0xDE9E, 0x8488, 0xDE3D, 0x84A3, 0xDDDC, 0x84BE, 0xDD7C, 0x84D9, 0xDD1B, 0x84F5, 0xDCBA, 0x8511, 0xDC59, 0x852D, 0xDBF9, 0x8549, 0xDB99, 0x8566, 0xDB38, - 0x8583, 0xDAD8, 0x85A0, 0xDA78, 0x85BE, 0xDA18, 0x85DC, 0xD9B8, 0x85FA, 0xD958, 0x8619, 0xD8F8, 0x8637, 0xD898, 0x8656, 0xD839, 0x8676, 0xD7D9, 0x8696, 0xD77A, - 0x86B6, 0xD71B, 0x86D6, 0xD6BB, 0x86F6, 0xD65C, 0x8717, 0xD5FD, 0x8738, 0xD59E, 0x875A, 0xD53F, 0x877B, 0xD4E1, 0x879D, 0xD482, 0x87C0, 0xD424, 0x87E2, 0xD3C5, - 0x8805, 0xD367, 0x8828, 0xD309, 0x884C, 0xD2AB, 0x8870, 0xD24D, 0x8894, 0xD1EF, 0x88B8, 0xD191, 0x88DD, 0xD134, 0x8902, 0xD0D6, 0x8927, 0xD079, 0x894C, 0xD01B, - 0x8972, 0xCFBE, 0x8998, 0xCF61, 0x89BE, 0xCF04, 0x89E5, 0xCEA7, 0x8A0C, 0xCE4B, 0x8A33, 0xCDEE, 0x8A5A, 0xCD92, 0x8A82, 0xCD35, 0x8AAA, 0xCCD9, 0x8AD3, 0xCC7D, - 0x8AFB, 0xCC21, 0x8B24, 0xCBC5, 0x8B4D, 0xCB69, 0x8B77, 0xCB0E, 0x8BA0, 0xCAB2, 0x8BCA, 0xCA57, 0x8BF5, 0xC9FC, 0x8C1F, 0xC9A1, 0x8C4A, 0xC946, 0x8C75, 0xC8EB, - 0x8CA1, 0xC890, 0x8CCC, 0xC836, 0x8CF8, 0xC7DB, 0x8D24, 0xC781, 0x8D51, 0xC727, 0x8D7E, 0xC6CD, 0x8DAB, 0xC673, 0x8DD8, 0xC619, 0x8E06, 0xC5C0, 0x8E34, 0xC566, - 0x8E62, 0xC50D, 0x8E90, 0xC4B4, 0x8EBF, 0xC45B, 0x8EEE, 0xC402, 0x8F1D, 0xC3A9, 0x8F4D, 0xC351, 0x8F7D, 0xC2F8, 0x8FAD, 0xC2A0, 0x8FDD, 0xC248, 0x900E, 0xC1F0, - 0x903E, 0xC198, 0x9070, 0xC140, 0x90A1, 0xC0E9, 0x90D3, 0xC091, 0x9105, 0xC03A, 0x9137, 0xBFE3, 0x9169, 0xBF8C, 0x919C, 0xBF35, 0x91CF, 0xBEDF, 0x9202, 0xBE88, - 0x9236, 0xBE32, 0x926A, 0xBDDC, 0x929E, 0xBD86, 0x92D2, 0xBD30, 0x9307, 0xBCDA, 0x933C, 0xBC85, 0x9371, 0xBC2F, 0x93A6, 0xBBDA, 0x93DC, 0xBB85, 0x9412, 0xBB30, - 0x9448, 0xBADC, 0x947E, 0xBA87, 0x94B5, 0xBA33, 0x94EC, 0xB9DF, 0x9523, 0xB98B, 0x955B, 0xB937, 0x9592, 0xB8E3, 0x95CA, 0xB890, 0x9603, 0xB83C, 0x963B, 0xB7E9, - 0x9674, 0xB796, 0x96AD, 0xB743, 0x96E6, 0xB6F1, 0x9720, 0xB69E, 0x9759, 0xB64C, 0x9793, 0xB5FA, 0x97CE, 0xB5A8, 0x9808, 0xB556, 0x9843, 0xB505, 0x987E, 0xB4B3, - 0x98B9, 0xB462, 0x98F5, 0xB411, 0x9930, 0xB3C0, 0x996D, 0xB36F, 0x99A9, 0xB31F, 0x99E5, 0xB2CF, 0x9A22, 0xB27F, 0x9A5F, 0xB22F, 0x9A9C, 0xB1DF, 0x9ADA, 0xB18F, - 0x9B17, 0xB140, 0x9B55, 0xB0F1, 0x9B94, 0xB0A2, 0x9BD2, 0xB053, 0x9C11, 0xB005, 0x9C50, 0xAFB6, 0x9C8F, 0xAF68, 0x9CCE, 0xAF1A, 0x9D0E, 0xAECC, 0x9D4E, 0xAE7F, - 0x9D8E, 0xAE31, 0x9DCE, 0xADE4, 0x9E0F, 0xAD97, 0x9E50, 0xAD4A, 0x9E91, 0xACFD, 0x9ED2, 0xACB1, 0x9F14, 0xAC65, 0x9F56, 0xAC19, 0x9F98, 0xABCD, 0x9FDA, 0xAB81, - 0xA01C, 0xAB36, 0xA05F, 0xAAEB, 0xA0A2, 0xAAA0, 0xA0E5, 0xAA55, 0xA129, 0xAA0A, 0xA16C, 0xA9C0, 0xA1B0, 0xA976, 0xA1F4, 0xA92C, 0xA238, 0xA8E2, 0xA27D, 0xA899, - 0xA2C2, 0xA84F, 0xA307, 0xA806, 0xA34C, 0xA7BD, 0xA391, 0xA774, 0xA3D7, 0xA72C, 0xA41D, 0xA6E4, 0xA463, 0xA69C, 0xA4A9, 0xA654, 0xA4F0, 0xA60C, 0xA537, 0xA5C5, - 0xA57E, 0xA57E, 0xA5C5, 0xA537, 0xA60C, 0xA4F0, 0xA654, 0xA4A9, 0xA69C, 0xA463, 0xA6E4, 0xA41D, 0xA72C, 0xA3D7, 0xA774, 0xA391, 0xA7BD, 0xA34C, 0xA806, 0xA307, - 0xA84F, 0xA2C2, 0xA899, 0xA27D, 0xA8E2, 0xA238, 0xA92C, 0xA1F4, 0xA976, 0xA1B0, 0xA9C0, 0xA16C, 0xAA0A, 0xA129, 0xAA55, 0xA0E5, 0xAAA0, 0xA0A2, 0xAAEB, 0xA05F, - 0xAB36, 0xA01C, 0xAB81, 0x9FDA, 0xABCD, 0x9F98, 0xAC19, 0x9F56, 0xAC65, 0x9F14, 0xACB1, 0x9ED2, 0xACFD, 0x9E91, 0xAD4A, 0x9E50, 0xAD97, 0x9E0F, 0xADE4, 0x9DCE, - 0xAE31, 0x9D8E, 0xAE7F, 0x9D4E, 0xAECC, 0x9D0E, 0xAF1A, 0x9CCE, 0xAF68, 0x9C8F, 0xAFB6, 0x9C50, 0xB005, 0x9C11, 0xB053, 0x9BD2, 0xB0A2, 0x9B94, 0xB0F1, 0x9B55, - 0xB140, 0x9B17, 0xB18F, 0x9ADA, 0xB1DF, 0x9A9C, 0xB22F, 0x9A5F, 0xB27F, 0x9A22, 0xB2CF, 0x99E5, 0xB31F, 0x99A9, 0xB36F, 0x996D, 0xB3C0, 0x9930, 0xB411, 0x98F5, - 0xB462, 0x98B9, 0xB4B3, 0x987E, 0xB505, 0x9843, 0xB556, 0x9808, 0xB5A8, 0x97CE, 0xB5FA, 0x9793, 0xB64C, 0x9759, 0xB69E, 0x9720, 0xB6F1, 0x96E6, 0xB743, 0x96AD, - 0xB796, 0x9674, 0xB7E9, 0x963B, 0xB83C, 0x9603, 0xB890, 0x95CA, 0xB8E3, 0x9592, 0xB937, 0x955B, 0xB98B, 0x9523, 0xB9DF, 0x94EC, 0xBA33, 0x94B5, 0xBA87, 0x947E, - 0xBADC, 0x9448, 0xBB30, 0x9412, 0xBB85, 0x93DC, 0xBBDA, 0x93A6, 0xBC2F, 0x9371, 0xBC85, 0x933C, 0xBCDA, 0x9307, 0xBD30, 0x92D2, 0xBD86, 0x929E, 0xBDDC, 0x926A, - 0xBE32, 0x9236, 0xBE88, 0x9202, 0xBEDF, 0x91CF, 0xBF35, 0x919C, 0xBF8C, 0x9169, 0xBFE3, 0x9137, 0xC03A, 0x9105, 0xC091, 0x90D3, 0xC0E9, 0x90A1, 0xC140, 0x9070, - 0xC198, 0x903E, 0xC1F0, 0x900E, 0xC248, 0x8FDD, 0xC2A0, 0x8FAD, 0xC2F8, 0x8F7D, 0xC351, 0x8F4D, 0xC3A9, 0x8F1D, 0xC402, 0x8EEE, 0xC45B, 0x8EBF, 0xC4B4, 0x8E90, - 0xC50D, 0x8E62, 0xC566, 0x8E34, 0xC5C0, 0x8E06, 0xC619, 0x8DD8, 0xC673, 0x8DAB, 0xC6CD, 0x8D7E, 0xC727, 0x8D51, 0xC781, 0x8D24, 0xC7DB, 0x8CF8, 0xC836, 0x8CCC, - 0xC890, 0x8CA1, 0xC8EB, 0x8C75, 0xC946, 0x8C4A, 0xC9A1, 0x8C1F, 0xC9FC, 0x8BF5, 0xCA57, 0x8BCA, 0xCAB2, 0x8BA0, 0xCB0E, 0x8B77, 0xCB69, 0x8B4D, 0xCBC5, 0x8B24, - 0xCC21, 0x8AFB, 0xCC7D, 0x8AD3, 0xCCD9, 0x8AAA, 0xCD35, 0x8A82, 0xCD92, 0x8A5A, 0xCDEE, 0x8A33, 0xCE4B, 0x8A0C, 0xCEA7, 0x89E5, 0xCF04, 0x89BE, 0xCF61, 0x8998, - 0xCFBE, 0x8972, 0xD01B, 0x894C, 0xD079, 0x8927, 0xD0D6, 0x8902, 0xD134, 0x88DD, 0xD191, 0x88B8, 0xD1EF, 0x8894, 0xD24D, 0x8870, 0xD2AB, 0x884C, 0xD309, 0x8828, - 0xD367, 0x8805, 0xD3C5, 0x87E2, 0xD424, 0x87C0, 0xD482, 0x879D, 0xD4E1, 0x877B, 0xD53F, 0x875A, 0xD59E, 0x8738, 0xD5FD, 0x8717, 0xD65C, 0x86F6, 0xD6BB, 0x86D6, - 0xD71B, 0x86B6, 0xD77A, 0x8696, 0xD7D9, 0x8676, 0xD839, 0x8656, 0xD898, 0x8637, 0xD8F8, 0x8619, 0xD958, 0x85FA, 0xD9B8, 0x85DC, 0xDA18, 0x85BE, 0xDA78, 0x85A0, - 0xDAD8, 0x8583, 0xDB38, 0x8566, 0xDB99, 0x8549, 0xDBF9, 0x852D, 0xDC59, 0x8511, 0xDCBA, 0x84F5, 0xDD1B, 0x84D9, 0xDD7C, 0x84BE, 0xDDDC, 0x84A3, 0xDE3D, 0x8488, - 0xDE9E, 0x846E, 0xDEFF, 0x8454, 0xDF61, 0x843A, 0xDFC2, 0x8421, 0xE023, 0x8407, 0xE085, 0x83EF, 0xE0E6, 0x83D6, 0xE148, 0x83BE, 0xE1A9, 0x83A6, 0xE20B, 0x838E, - 0xE26D, 0x8377, 0xE2CF, 0x8360, 0xE330, 0x8349, 0xE392, 0x8332, 0xE3F4, 0x831C, 0xE457, 0x8306, 0xE4B9, 0x82F1, 0xE51B, 0x82DB, 0xE57D, 0x82C6, 0xE5E0, 0x82B2, - 0xE642, 0x829D, 0xE6A5, 0x8289, 0xE707, 0x8276, 0xE76A, 0x8262, 0xE7CD, 0x824F, 0xE82F, 0x823C, 0xE892, 0x822A, 0xE8F5, 0x8217, 0xE958, 0x8205, 0xE9BB, 0x81F4, - 0xEA1E, 0x81E2, 0xEA81, 0x81D1, 0xEAE4, 0x81C1, 0xEB47, 0x81B0, 0xEBAB, 0x81A0, 0xEC0E, 0x8190, 0xEC71, 0x8181, 0xECD5, 0x8172, 0xED38, 0x8163, 0xED9B, 0x8154, - 0xEDFF, 0x8146, 0xEE62, 0x8138, 0xEEC6, 0x812A, 0xEF2A, 0x811D, 0xEF8D, 0x8110, 0xEFF1, 0x8103, 0xF055, 0x80F6, 0xF0B9, 0x80EA, 0xF11C, 0x80DE, 0xF180, 0x80D3, - 0xF1E4, 0x80C8, 0xF248, 0x80BD, 0xF2AC, 0x80B2, 0xF310, 0x80A8, 0xF374, 0x809E, 0xF3D8, 0x8094, 0xF43C, 0x808B, 0xF4A0, 0x8082, 0xF505, 0x8079, 0xF569, 0x8070, - 0xF5CD, 0x8068, 0xF631, 0x8060, 0xF695, 0x8059, 0xF6FA, 0x8052, 0xF75E, 0x804B, 0xF7C2, 0x8044, 0xF827, 0x803E, 0xF88B, 0x8038, 0xF8EF, 0x8032, 0xF954, 0x802D, - 0xF9B8, 0x8027, 0xFA1D, 0x8023, 0xFA81, 0x801E, 0xFAE5, 0x801A, 0xFB4A, 0x8016, 0xFBAE, 0x8013, 0xFC13, 0x800F, 0xFC77, 0x800C, 0xFCDC, 0x800A, 0xFD40, 0x8008, - 0xFDA5, 0x8006, 0xFE09, 0x8004, 0xFE6E, 0x8002, 0xFED2, 0x8001, 0xFF37, 0x8001, 0xFF9B, 0x8000, 0x0000, 0x8000, 0x0065, 0x8000, 0x00C9, 0x8001, 0x012E, 0x8001, - 0x0192, 0x8002, 0x01F7, 0x8004, 0x025B, 0x8006, 0x02C0, 0x8008, 0x0324, 0x800A, 0x0389, 0x800C, 0x03ED, 0x800F, 0x0452, 0x8013, 0x04B6, 0x8016, 0x051B, 0x801A, - 0x057F, 0x801E, 0x05E3, 0x8023, 0x0648, 0x8027, 0x06AC, 0x802D, 0x0711, 0x8032, 0x0775, 0x8038, 0x07D9, 0x803E, 0x083E, 0x8044, 0x08A2, 0x804B, 0x0906, 0x8052, - 0x096B, 0x8059, 0x09CF, 0x8060, 0x0A33, 0x8068, 0x0A97, 0x8070, 0x0AFB, 0x8079, 0x0B60, 0x8082, 0x0BC4, 0x808B, 0x0C28, 0x8094, 0x0C8C, 0x809E, 0x0CF0, 0x80A8, - 0x0D54, 0x80B2, 0x0DB8, 0x80BD, 0x0E1C, 0x80C8, 0x0E80, 0x80D3, 0x0EE4, 0x80DE, 0x0F47, 0x80EA, 0x0FAB, 0x80F6, 0x100F, 0x8103, 0x1073, 0x8110, 0x10D6, 0x811D, - 0x113A, 0x812A, 0x119E, 0x8138, 0x1201, 0x8146, 0x1265, 0x8154, 0x12C8, 0x8163, 0x132B, 0x8172, 0x138F, 0x8181, 0x13F2, 0x8190, 0x1455, 0x81A0, 0x14B9, 0x81B0, - 0x151C, 0x81C1, 0x157F, 0x81D1, 0x15E2, 0x81E2, 0x1645, 0x81F4, 0x16A8, 0x8205, 0x170B, 0x8217, 0x176E, 0x822A, 0x17D1, 0x823C, 0x1833, 0x824F, 0x1896, 0x8262, - 0x18F9, 0x8276, 0x195B, 0x8289, 0x19BE, 0x829D, 0x1A20, 0x82B2, 0x1A83, 0x82C6, 0x1AE5, 0x82DB, 0x1B47, 0x82F1, 0x1BA9, 0x8306, 0x1C0C, 0x831C, 0x1C6E, 0x8332, - 0x1CD0, 0x8349, 0x1D31, 0x8360, 0x1D93, 0x8377, 0x1DF5, 0x838E, 0x1E57, 0x83A6, 0x1EB8, 0x83BE, 0x1F1A, 0x83D6, 0x1F7B, 0x83EF, 0x1FDD, 0x8407, 0x203E, 0x8421, - 0x209F, 0x843A, 0x2101, 0x8454, 0x2162, 0x846E, 0x21C3, 0x8488, 0x2224, 0x84A3, 0x2284, 0x84BE, 0x22E5, 0x84D9, 0x2346, 0x84F5, 0x23A7, 0x8511, 0x2407, 0x852D, - 0x2467, 0x8549, 0x24C8, 0x8566, 0x2528, 0x8583, 0x2588, 0x85A0, 0x25E8, 0x85BE, 0x2648, 0x85DC, 0x26A8, 0x85FA, 0x2708, 0x8619, 0x2768, 0x8637, 0x27C7, 0x8656, - 0x2827, 0x8676, 0x2886, 0x8696, 0x28E5, 0x86B6, 0x2945, 0x86D6, 0x29A4, 0x86F6, 0x2A03, 0x8717, 0x2A62, 0x8738, 0x2AC1, 0x875A, 0x2B1F, 0x877B, 0x2B7E, 0x879D, - 0x2BDC, 0x87C0, 0x2C3B, 0x87E2, 0x2C99, 0x8805, 0x2CF7, 0x8828, 0x2D55, 0x884C, 0x2DB3, 0x8870, 0x2E11, 0x8894, 0x2E6F, 0x88B8, 0x2ECC, 0x88DD, 0x2F2A, 0x8902, - 0x2F87, 0x8927, 0x2FE5, 0x894C, 0x3042, 0x8972, 0x309F, 0x8998, 0x30FC, 0x89BE, 0x3159, 0x89E5, 0x31B5, 0x8A0C, 0x3212, 0x8A33, 0x326E, 0x8A5A, 0x32CB, 0x8A82, - 0x3327, 0x8AAA, 0x3383, 0x8AD3, 0x33DF, 0x8AFB, 0x343B, 0x8B24, 0x3497, 0x8B4D, 0x34F2, 0x8B77, 0x354E, 0x8BA0, 0x35A9, 0x8BCA, 0x3604, 0x8BF5, 0x365F, 0x8C1F, - 0x36BA, 0x8C4A, 0x3715, 0x8C75, 0x3770, 0x8CA1, 0x37CA, 0x8CCC, 0x3825, 0x8CF8, 0x387F, 0x8D24, 0x38D9, 0x8D51, 0x3933, 0x8D7E, 0x398D, 0x8DAB, 0x39E7, 0x8DD8, - 0x3A40, 0x8E06, 0x3A9A, 0x8E34, 0x3AF3, 0x8E62, 0x3B4C, 0x8E90, 0x3BA5, 0x8EBF, 0x3BFE, 0x8EEE, 0x3C57, 0x8F1D, 0x3CAF, 0x8F4D, 0x3D08, 0x8F7D, 0x3D60, 0x8FAD, - 0x3DB8, 0x8FDD, 0x3E10, 0x900E, 0x3E68, 0x903E, 0x3EC0, 0x9070, 0x3F17, 0x90A1, 0x3F6F, 0x90D3, 0x3FC6, 0x9105, 0x401D, 0x9137, 0x4074, 0x9169, 0x40CB, 0x919C, - 0x4121, 0x91CF, 0x4178, 0x9202, 0x41CE, 0x9236, 0x4224, 0x926A, 0x427A, 0x929E, 0x42D0, 0x92D2, 0x4326, 0x9307, 0x437B, 0x933C, 0x43D1, 0x9371, 0x4426, 0x93A6, - 0x447B, 0x93DC, 0x44D0, 0x9412, 0x4524, 0x9448, 0x4579, 0x947E, 0x45CD, 0x94B5, 0x4621, 0x94EC, 0x4675, 0x9523, 0x46C9, 0x955B, 0x471D, 0x9592, 0x4770, 0x95CA, - 0x47C4, 0x9603, 0x4817, 0x963B, 0x486A, 0x9674, 0x48BD, 0x96AD, 0x490F, 0x96E6, 0x4962, 0x9720, 0x49B4, 0x9759, 0x4A06, 0x9793, 0x4A58, 0x97CE, 0x4AAA, 0x9808, - 0x4AFB, 0x9843, 0x4B4D, 0x987E, 0x4B9E, 0x98B9, 0x4BEF, 0x98F5, 0x4C40, 0x9930, 0x4C91, 0x996D, 0x4CE1, 0x99A9, 0x4D31, 0x99E5, 0x4D81, 0x9A22, 0x4DD1, 0x9A5F, - 0x4E21, 0x9A9C, 0x4E71, 0x9ADA, 0x4EC0, 0x9B17, 0x4F0F, 0x9B55, 0x4F5E, 0x9B94, 0x4FAD, 0x9BD2, 0x4FFB, 0x9C11, 0x504A, 0x9C50, 0x5098, 0x9C8F, 0x50E6, 0x9CCE, - 0x5134, 0x9D0E, 0x5181, 0x9D4E, 0x51CF, 0x9D8E, 0x521C, 0x9DCE, 0x5269, 0x9E0F, 0x52B6, 0x9E50, 0x5303, 0x9E91, 0x534F, 0x9ED2, 0x539B, 0x9F14, 0x53E7, 0x9F56, - 0x5433, 0x9F98, 0x547F, 0x9FDA, 0x54CA, 0xA01C, 0x5515, 0xA05F, 0x5560, 0xA0A2, 0x55AB, 0xA0E5, 0x55F6, 0xA129, 0x5640, 0xA16C, 0x568A, 0xA1B0, 0x56D4, 0xA1F4, - 0x571E, 0xA238, 0x5767, 0xA27D, 0x57B1, 0xA2C2, 0x57FA, 0xA307, 0x5843, 0xA34C, 0x588C, 0xA391, 0x58D4, 0xA3D7, 0x591C, 0xA41D, 0x5964, 0xA463, 0x59AC, 0xA4A9, - 0x59F4, 0xA4F0, 0x5A3B, 0xA537, 0x5A82, 0xA57E, 0x5AC9, 0xA5C5, 0x5B10, 0xA60C, 0x5B57, 0xA654, 0x5B9D, 0xA69C, 0x5BE3, 0xA6E4, 0x5C29, 0xA72C, 0x5C6F, 0xA774, - 0x5CB4, 0xA7BD, 0x5CF9, 0xA806, 0x5D3E, 0xA84F, 0x5D83, 0xA899, 0x5DC8, 0xA8E2, 0x5E0C, 0xA92C, 0x5E50, 0xA976, 0x5E94, 0xA9C0, 0x5ED7, 0xAA0A, 0x5F1B, 0xAA55, - 0x5F5E, 0xAAA0, 0x5FA1, 0xAAEB, 0x5FE4, 0xAB36, 0x6026, 0xAB81, 0x6068, 0xABCD, 0x60AA, 0xAC19, 0x60EC, 0xAC65, 0x612E, 0xACB1, 0x616F, 0xACFD, 0x61B0, 0xAD4A, - 0x61F1, 0xAD97, 0x6232, 0xADE4, 0x6272, 0xAE31, 0x62B2, 0xAE7F, 0x62F2, 0xAECC, 0x6332, 0xAF1A, 0x6371, 0xAF68, 0x63B0, 0xAFB6, 0x63EF, 0xB005, 0x642E, 0xB053, - 0x646C, 0xB0A2, 0x64AB, 0xB0F1, 0x64E9, 0xB140, 0x6526, 0xB18F, 0x6564, 0xB1DF, 0x65A1, 0xB22F, 0x65DE, 0xB27F, 0x661B, 0xB2CF, 0x6657, 0xB31F, 0x6693, 0xB36F, - 0x66D0, 0xB3C0, 0x670B, 0xB411, 0x6747, 0xB462, 0x6782, 0xB4B3, 0x67BD, 0xB505, 0x67F8, 0xB556, 0x6832, 0xB5A8, 0x686D, 0xB5FA, 0x68A7, 0xB64C, 0x68E0, 0xB69E, - 0x691A, 0xB6F1, 0x6953, 0xB743, 0x698C, 0xB796, 0x69C5, 0xB7E9, 0x69FD, 0xB83C, 0x6A36, 0xB890, 0x6A6E, 0xB8E3, 0x6AA5, 0xB937, 0x6ADD, 0xB98B, 0x6B14, 0xB9DF, - 0x6B4B, 0xBA33, 0x6B82, 0xBA87, 0x6BB8, 0xBADC, 0x6BEE, 0xBB30, 0x6C24, 0xBB85, 0x6C5A, 0xBBDA, 0x6C8F, 0xBC2F, 0x6CC4, 0xBC85, 0x6CF9, 0xBCDA, 0x6D2E, 0xBD30, - 0x6D62, 0xBD86, 0x6D96, 0xBDDC, 0x6DCA, 0xBE32, 0x6DFE, 0xBE88, 0x6E31, 0xBEDF, 0x6E64, 0xBF35, 0x6E97, 0xBF8C, 0x6EC9, 0xBFE3, 0x6EFB, 0xC03A, 0x6F2D, 0xC091, - 0x6F5F, 0xC0E9, 0x6F90, 0xC140, 0x6FC2, 0xC198, 0x6FF2, 0xC1F0, 0x7023, 0xC248, 0x7053, 0xC2A0, 0x7083, 0xC2F8, 0x70B3, 0xC351, 0x70E3, 0xC3A9, 0x7112, 0xC402, - 0x7141, 0xC45B, 0x7170, 0xC4B4, 0x719E, 0xC50D, 0x71CC, 0xC566, 0x71FA, 0xC5C0, 0x7228, 0xC619, 0x7255, 0xC673, 0x7282, 0xC6CD, 0x72AF, 0xC727, 0x72DC, 0xC781, - 0x7308, 0xC7DB, 0x7334, 0xC836, 0x735F, 0xC890, 0x738B, 0xC8EB, 0x73B6, 0xC946, 0x73E1, 0xC9A1, 0x740B, 0xC9FC, 0x7436, 0xCA57, 0x7460, 0xCAB2, 0x7489, 0xCB0E, - 0x74B3, 0xCB69, 0x74DC, 0xCBC5, 0x7505, 0xCC21, 0x752D, 0xCC7D, 0x7556, 0xCCD9, 0x757E, 0xCD35, 0x75A6, 0xCD92, 0x75CD, 0xCDEE, 0x75F4, 0xCE4B, 0x761B, 0xCEA7, - 0x7642, 0xCF04, 0x7668, 0xCF61, 0x768E, 0xCFBE, 0x76B4, 0xD01B, 0x76D9, 0xD079, 0x76FE, 0xD0D6, 0x7723, 0xD134, 0x7748, 0xD191, 0x776C, 0xD1EF, 0x7790, 0xD24D, - 0x77B4, 0xD2AB, 0x77D8, 0xD309, 0x77FB, 0xD367, 0x781E, 0xD3C5, 0x7840, 0xD424, 0x7863, 0xD482, 0x7885, 0xD4E1, 0x78A6, 0xD53F, 0x78C8, 0xD59E, 0x78E9, 0xD5FD, - 0x790A, 0xD65C, 0x792A, 0xD6BB, 0x794A, 0xD71B, 0x796A, 0xD77A, 0x798A, 0xD7D9, 0x79AA, 0xD839, 0x79C9, 0xD898, 0x79E7, 0xD8F8, 0x7A06, 0xD958, 0x7A24, 0xD9B8, - 0x7A42, 0xDA18, 0x7A60, 0xDA78, 0x7A7D, 0xDAD8, 0x7A9A, 0xDB38, 0x7AB7, 0xDB99, 0x7AD3, 0xDBF9, 0x7AEF, 0xDC59, 0x7B0B, 0xDCBA, 0x7B27, 0xDD1B, 0x7B42, 0xDD7C, - 0x7B5D, 0xDDDC, 0x7B78, 0xDE3D, 0x7B92, 0xDE9E, 0x7BAC, 0xDEFF, 0x7BC6, 0xDF61, 0x7BDF, 0xDFC2, 0x7BF9, 0xE023, 0x7C11, 0xE085, 0x7C2A, 0xE0E6, 0x7C42, 0xE148, - 0x7C5A, 0xE1A9, 0x7C72, 0xE20B, 0x7C89, 0xE26D, 0x7CA0, 0xE2CF, 0x7CB7, 0xE330, 0x7CCE, 0xE392, 0x7CE4, 0xE3F4, 0x7CFA, 0xE457, 0x7D0F, 0xE4B9, 0x7D25, 0xE51B, - 0x7D3A, 0xE57D, 0x7D4E, 0xE5E0, 0x7D63, 0xE642, 0x7D77, 0xE6A5, 0x7D8A, 0xE707, 0x7D9E, 0xE76A, 0x7DB1, 0xE7CD, 0x7DC4, 0xE82F, 0x7DD6, 0xE892, 0x7DE9, 0xE8F5, - 0x7DFB, 0xE958, 0x7E0C, 0xE9BB, 0x7E1E, 0xEA1E, 0x7E2F, 0xEA81, 0x7E3F, 0xEAE4, 0x7E50, 0xEB47, 0x7E60, 0xEBAB, 0x7E70, 0xEC0E, 0x7E7F, 0xEC71, 0x7E8E, 0xECD5, - 0x7E9D, 0xED38, 0x7EAC, 0xED9B, 0x7EBA, 0xEDFF, 0x7EC8, 0xEE62, 0x7ED6, 0xEEC6, 0x7EE3, 0xEF2A, 0x7EF0, 0xEF8D, 0x7EFD, 0xEFF1, 0x7F0A, 0xF055, 0x7F16, 0xF0B9, - 0x7F22, 0xF11C, 0x7F2D, 0xF180, 0x7F38, 0xF1E4, 0x7F43, 0xF248, 0x7F4E, 0xF2AC, 0x7F58, 0xF310, 0x7F62, 0xF374, 0x7F6C, 0xF3D8, 0x7F75, 0xF43C, 0x7F7E, 0xF4A0, - 0x7F87, 0xF505, 0x7F90, 0xF569, 0x7F98, 0xF5CD, 0x7FA0, 0xF631, 0x7FA7, 0xF695, 0x7FAE, 0xF6FA, 0x7FB5, 0xF75E, 0x7FBC, 0xF7C2, 0x7FC2, 0xF827, 0x7FC8, 0xF88B, - 0x7FCE, 0xF8EF, 0x7FD3, 0xF954, 0x7FD9, 0xF9B8, 0x7FDD, 0xFA1D, 0x7FE2, 0xFA81, 0x7FE6, 0xFAE5, 0x7FEA, 0xFB4A, 0x7FED, 0xFBAE, 0x7FF1, 0xFC13, 0x7FF4, 0xFC77, - 0x7FF6, 0xFCDC, 0x7FF8, 0xFD40, 0x7FFA, 0xFDA5, 0x7FFC, 0xFE09, 0x7FFE, 0xFE6E, 0x7FFF, 0xFED2, 0x7FFF, 0xFF37, 0x7FFF, 0xFF9B, }; - -#elif defined(MAX_FFT_SIZE_1024) -const INT16 radix4FftTwiddleArr[2*MAX_FFT_SIZE] = {0X7FFF,0X0000,0X7FFF,0X00C9,0X7FFE,0X0192,0X7FFA,0X025B,0X7FF6,0X0324,0X7FF1,0X03ED,0X7FEA,0X04B6,0X7FE2,0X057F,0X7FD9,0X0648,0X7FCE,0X0711, - 0X7FC2,0X07D9,0X7FB5,0X08A2,0X7FA7,0X096B,0X7F98,0X0A33,0X7F87,0X0AFB,0X7F75,0X0BC4,0X7F62,0X0C8C,0X7F4E,0X0D54,0X7F38,0X0E1C,0X7F22,0X0EE4, - 0X7F0A,0X0FAB,0X7EF0,0X1073,0X7ED6,0X113A,0X7EBA,0X1201,0X7E9D,0X12C8,0X7E7F,0X138F,0X7E60,0X1455,0X7E3F,0X151C,0X7E1E,0X15E2,0X7DFB,0X16A8, - 0X7DD6,0X176E,0X7DB1,0X1833,0X7D8A,0X18F9,0X7D63,0X19BE,0X7D3A,0X1A83,0X7D0F,0X1B47,0X7CE4,0X1C0C,0X7CB7,0X1CD0,0X7C89,0X1D93,0X7C5A,0X1E57, - 0X7C2A,0X1F1A,0X7BF9,0X1FDD,0X7BC6,0X209F,0X7B92,0X2162,0X7B5D,0X2224,0X7B27,0X22E5,0X7AEF,0X23A7,0X7AB7,0X2467,0X7A7D,0X2528,0X7A42,0X25E8, - 0X7A06,0X26A8,0X79C9,0X2768,0X798A,0X2827,0X794A,0X28E5,0X790A,0X29A4,0X78C8,0X2A62,0X7885,0X2B1F,0X7840,0X2BDC,0X77FB,0X2C99,0X77B4,0X2D55, - 0X776C,0X2E11,0X7723,0X2ECC,0X76D9,0X2F87,0X768E,0X3042,0X7642,0X30FC,0X75F4,0X31B5,0X75A6,0X326E,0X7556,0X3327,0X7505,0X33DF,0X74B3,0X3497, - 0X7460,0X354E,0X740B,0X3604,0X73B6,0X36BA,0X735F,0X3770,0X7308,0X3825,0X72AF,0X38D9,0X7255,0X398D,0X71FA,0X3A40,0X719E,0X3AF3,0X7141,0X3BA5, - 0X70E3,0X3C57,0X7083,0X3D08,0X7023,0X3DB8,0X6FC2,0X3E68,0X6F5F,0X3F17,0X6EFB,0X3FC6,0X6E97,0X4074,0X6E31,0X4121,0X6DCA,0X41CE,0X6D62,0X427A, - 0X6CF9,0X4326,0X6C8F,0X43D1,0X6C24,0X447B,0X6BB8,0X4524,0X6B4B,0X45CD,0X6ADD,0X4675,0X6A6E,0X471D,0X69FD,0X47C4,0X698C,0X486A,0X691A,0X490F, - 0X68A7,0X49B4,0X6832,0X4A58,0X67BD,0X4AFB,0X6747,0X4B9E,0X66D0,0X4C40,0X6657,0X4CE1,0X65DE,0X4D81,0X6564,0X4E21,0X64E9,0X4EC0,0X646C,0X4F5E, - 0X63EF,0X4FFB,0X6371,0X5098,0X62F2,0X5134,0X6272,0X51CF,0X61F1,0X5269,0X616F,0X5303,0X60EC,0X539B,0X6068,0X5433,0X5FE4,0X54CA,0X5F5E,0X5560, - 0X5ED7,0X55F6,0X5E50,0X568A,0X5DC8,0X571E,0X5D3E,0X57B1,0X5CB4,0X5843,0X5C29,0X58D4,0X5B9D,0X5964,0X5B10,0X59F4,0X5A82,0X5A82,0X59F4,0X5B10, - 0X5964,0X5B9D,0X58D4,0X5C29,0X5843,0X5CB4,0X57B1,0X5D3E,0X571E,0X5DC8,0X568A,0X5E50,0X55F6,0X5ED7,0X5560,0X5F5E,0X54CA,0X5FE4,0X5433,0X6068, - 0X539B,0X60EC,0X5303,0X616F,0X5269,0X61F1,0X51CF,0X6272,0X5134,0X62F2,0X5098,0X6371,0X4FFB,0X63EF,0X4F5E,0X646C,0X4EC0,0X64E9,0X4E21,0X6564, - 0X4D81,0X65DE,0X4CE1,0X6657,0X4C40,0X66D0,0X4B9E,0X6747,0X4AFB,0X67BD,0X4A58,0X6832,0X49B4,0X68A7,0X490F,0X691A,0X486A,0X698C,0X47C4,0X69FD, - 0X471D,0X6A6E,0X4675,0X6ADD,0X45CD,0X6B4B,0X4524,0X6BB8,0X447B,0X6C24,0X43D1,0X6C8F,0X4326,0X6CF9,0X427A,0X6D62,0X41CE,0X6DCA,0X4121,0X6E31, - 0X4074,0X6E97,0X3FC6,0X6EFB,0X3F17,0X6F5F,0X3E68,0X6FC2,0X3DB8,0X7023,0X3D08,0X7083,0X3C57,0X70E3,0X3BA5,0X7141,0X3AF3,0X719E,0X3A40,0X71FA, - 0X398D,0X7255,0X38D9,0X72AF,0X3825,0X7308,0X3770,0X735F,0X36BA,0X73B6,0X3604,0X740B,0X354E,0X7460,0X3497,0X74B3,0X33DF,0X7505,0X3327,0X7556, - 0X326E,0X75A6,0X31B5,0X75F4,0X30FC,0X7642,0X3042,0X768E,0X2F87,0X76D9,0X2ECC,0X7723,0X2E11,0X776C,0X2D55,0X77B4,0X2C99,0X77FB,0X2BDC,0X7840, - 0X2B1F,0X7885,0X2A62,0X78C8,0X29A4,0X790A,0X28E5,0X794A,0X2827,0X798A,0X2768,0X79C9,0X26A8,0X7A06,0X25E8,0X7A42,0X2528,0X7A7D,0X2467,0X7AB7, - 0X23A7,0X7AEF,0X22E5,0X7B27,0X2224,0X7B5D,0X2162,0X7B92,0X209F,0X7BC6,0X1FDD,0X7BF9,0X1F1A,0X7C2A,0X1E57,0X7C5A,0X1D93,0X7C89,0X1CD0,0X7CB7, - 0X1C0C,0X7CE4,0X1B47,0X7D0F,0X1A83,0X7D3A,0X19BE,0X7D63,0X18F9,0X7D8A,0X1833,0X7DB1,0X176E,0X7DD6,0X16A8,0X7DFB,0X15E2,0X7E1E,0X151C,0X7E3F, - 0X1455,0X7E60,0X138F,0X7E7F,0X12C8,0X7E9D,0X1201,0X7EBA,0X113A,0X7ED6,0X1073,0X7EF0,0X0FAB,0X7F0A,0X0EE4,0X7F22,0X0E1C,0X7F38,0X0D54,0X7F4E, - 0X0C8C,0X7F62,0X0BC4,0X7F75,0X0AFB,0X7F87,0X0A33,0X7F98,0X096B,0X7FA7,0X08A2,0X7FB5,0X07D9,0X7FC2,0X0711,0X7FCE,0X0648,0X7FD9,0X057F,0X7FE2, - 0X04B6,0X7FEA,0X03ED,0X7FF1,0X0324,0X7FF6,0X025B,0X7FFA,0X0192,0X7FFE,0X00C9,0X7FFF,0X0000,0X7FFF,0XFF37,0X7FFF,0XFE6E,0X7FFE,0XFDA5,0X7FFA, - 0XFCDC,0X7FF6,0XFC13,0X7FF1,0XFB4A,0X7FEA,0XFA81,0X7FE2,0XF9B8,0X7FD9,0XF8EF,0X7FCE,0XF827,0X7FC2,0XF75E,0X7FB5,0XF695,0X7FA7,0XF5CD,0X7F98, - 0XF505,0X7F87,0XF43C,0X7F75,0XF374,0X7F62,0XF2AC,0X7F4E,0XF1E4,0X7F38,0XF11C,0X7F22,0XF055,0X7F0A,0XEF8D,0X7EF0,0XEEC6,0X7ED6,0XEDFF,0X7EBA, - 0XED38,0X7E9D,0XEC71,0X7E7F,0XEBAB,0X7E60,0XEAE4,0X7E3F,0XEA1E,0X7E1E,0XE958,0X7DFB,0XE892,0X7DD6,0XE7CD,0X7DB1,0XE707,0X7D8A,0XE642,0X7D63, - 0XE57D,0X7D3A,0XE4B9,0X7D0F,0XE3F4,0X7CE4,0XE330,0X7CB7,0XE26D,0X7C89,0XE1A9,0X7C5A,0XE0E6,0X7C2A,0XE023,0X7BF9,0XDF61,0X7BC6,0XDE9E,0X7B92, - 0XDDDC,0X7B5D,0XDD1B,0X7B27,0XDC59,0X7AEF,0XDB99,0X7AB7,0XDAD8,0X7A7D,0XDA18,0X7A42,0XD958,0X7A06,0XD898,0X79C9,0XD7D9,0X798A,0XD71B,0X794A, - 0XD65C,0X790A,0XD59E,0X78C8,0XD4E1,0X7885,0XD424,0X7840,0XD367,0X77FB,0XD2AB,0X77B4,0XD1EF,0X776C,0XD134,0X7723,0XD079,0X76D9,0XCFBE,0X768E, - 0XCF04,0X7642,0XCE4B,0X75F4,0XCD92,0X75A6,0XCCD9,0X7556,0XCC21,0X7505,0XCB69,0X74B3,0XCAB2,0X7460,0XC9FC,0X740B,0XC946,0X73B6,0XC890,0X735F, - 0XC7DB,0X7308,0XC727,0X72AF,0XC673,0X7255,0XC5C0,0X71FA,0XC50D,0X719E,0XC45B,0X7141,0XC3A9,0X70E3,0XC2F8,0X7083,0XC248,0X7023,0XC198,0X6FC2, - 0XC0E9,0X6F5F,0XC03A,0X6EFB,0XBF8C,0X6E97,0XBEDF,0X6E31,0XBE32,0X6DCA,0XBD86,0X6D62,0XBCDA,0X6CF9,0XBC2F,0X6C8F,0XBB85,0X6C24,0XBADC,0X6BB8, - 0XBA33,0X6B4B,0XB98B,0X6ADD,0XB8E3,0X6A6E,0XB83C,0X69FD,0XB796,0X698C,0XB6F1,0X691A,0XB64C,0X68A7,0XB5A8,0X6832,0XB505,0X67BD,0XB462,0X6747, - 0XB3C0,0X66D0,0XB31F,0X6657,0XB27F,0X65DE,0XB1DF,0X6564,0XB140,0X64E9,0XB0A2,0X646C,0XB005,0X63EF,0XAF68,0X6371,0XAECC,0X62F2,0XAE31,0X6272, - 0XAD97,0X61F1,0XACFD,0X616F,0XAC65,0X60EC,0XABCD,0X6068,0XAB36,0X5FE4,0XAAA0,0X5F5E,0XAA0A,0X5ED7,0XA976,0X5E50,0XA8E2,0X5DC8,0XA84F,0X5D3E, - 0XA7BD,0X5CB4,0XA72C,0X5C29,0XA69C,0X5B9D,0XA60C,0X5B10,0XA57E,0X5A82,0XA4F0,0X59F4,0XA463,0X5964,0XA3D7,0X58D4,0XA34C,0X5843,0XA2C2,0X57B1, - 0XA238,0X571E,0XA1B0,0X568A,0XA129,0X55F6,0XA0A2,0X5560,0XA01C,0X54CA,0X9F98,0X5433,0X9F14,0X539B,0X9E91,0X5303,0X9E0F,0X5269,0X9D8E,0X51CF, - 0X9D0E,0X5134,0X9C8F,0X5098,0X9C11,0X4FFB,0X9B94,0X4F5E,0X9B17,0X4EC0,0X9A9C,0X4E21,0X9A22,0X4D81,0X99A9,0X4CE1,0X9930,0X4C40,0X98B9,0X4B9E, - 0X9843,0X4AFB,0X97CE,0X4A58,0X9759,0X49B4,0X96E6,0X490F,0X9674,0X486A,0X9603,0X47C4,0X9592,0X471D,0X9523,0X4675,0X94B5,0X45CD,0X9448,0X4524, - 0X93DC,0X447B,0X9371,0X43D1,0X9307,0X4326,0X929E,0X427A,0X9236,0X41CE,0X91CF,0X4121,0X9169,0X4074,0X9105,0X3FC6,0X90A1,0X3F17,0X903E,0X3E68, - 0X8FDD,0X3DB8,0X8F7D,0X3D08,0X8F1D,0X3C57,0X8EBF,0X3BA5,0X8E62,0X3AF3,0X8E06,0X3A40,0X8DAB,0X398D,0X8D51,0X38D9,0X8CF8,0X3825,0X8CA1,0X3770, - 0X8C4A,0X36BA,0X8BF5,0X3604,0X8BA0,0X354E,0X8B4D,0X3497,0X8AFB,0X33DF,0X8AAA,0X3327,0X8A5A,0X326E,0X8A0C,0X31B5,0X89BE,0X30FC,0X8972,0X3042, - 0X8927,0X2F87,0X88DD,0X2ECC,0X8894,0X2E11,0X884C,0X2D55,0X8805,0X2C99,0X87C0,0X2BDC,0X877B,0X2B1F,0X8738,0X2A62,0X86F6,0X29A4,0X86B6,0X28E5, - 0X8676,0X2827,0X8637,0X2768,0X85FA,0X26A8,0X85BE,0X25E8,0X8583,0X2528,0X8549,0X2467,0X8511,0X23A7,0X84D9,0X22E5,0X84A3,0X2224,0X846E,0X2162, - 0X843A,0X209F,0X8407,0X1FDD,0X83D6,0X1F1A,0X83A6,0X1E57,0X8377,0X1D93,0X8349,0X1CD0,0X831C,0X1C0C,0X82F1,0X1B47,0X82C6,0X1A83,0X829D,0X19BE, - 0X8276,0X18F9,0X824F,0X1833,0X822A,0X176E,0X8205,0X16A8,0X81E2,0X15E2,0X81C1,0X151C,0X81A0,0X1455,0X8181,0X138F,0X8163,0X12C8,0X8146,0X1201, - 0X812A,0X113A,0X8110,0X1073,0X80F6,0X0FAB,0X80DE,0X0EE4,0X80C8,0X0E1C,0X80B2,0X0D54,0X809E,0X0C8C,0X808B,0X0BC4,0X8079,0X0AFB,0X8068,0X0A33, - 0X8059,0X096B,0X804B,0X08A2,0X803E,0X07D9,0X8032,0X0711,0X8027,0X0648,0X801E,0X057F,0X8016,0X04B6,0X800F,0X03ED,0X800A,0X0324,0X8006,0X025B, - 0X8002,0X0192,0X8001,0X00C9,0X8001,0X0000,0X8001,0XFF37,0X8002,0XFE6E,0X8006,0XFDA5,0X800A,0XFCDC,0X800F,0XFC13,0X8016,0XFB4A,0X801E,0XFA81, - 0X8027,0XF9B8,0X8032,0XF8EF,0X803E,0XF827,0X804B,0XF75E,0X8059,0XF695,0X8068,0XF5CD,0X8079,0XF505,0X808B,0XF43C,0X809E,0XF374,0X80B2,0XF2AC, - 0X80C8,0XF1E4,0X80DE,0XF11C,0X80F6,0XF055,0X8110,0XEF8D,0X812A,0XEEC6,0X8146,0XEDFF,0X8163,0XED38,0X8181,0XEC71,0X81A0,0XEBAB,0X81C1,0XEAE4, - 0X81E2,0XEA1E,0X8205,0XE958,0X822A,0XE892,0X824F,0XE7CD,0X8276,0XE707,0X829D,0XE642,0X82C6,0XE57D,0X82F1,0XE4B9,0X831C,0XE3F4,0X8349,0XE330, - 0X8377,0XE26D,0X83A6,0XE1A9,0X83D6,0XE0E6,0X8407,0XE023,0X843A,0XDF61,0X846E,0XDE9E,0X84A3,0XDDDC,0X84D9,0XDD1B,0X8511,0XDC59,0X8549,0XDB99, - 0X8583,0XDAD8,0X85BE,0XDA18,0X85FA,0XD958,0X8637,0XD898,0X8676,0XD7D9,0X86B6,0XD71B,0X86F6,0XD65C,0X8738,0XD59E,0X877B,0XD4E1,0X87C0,0XD424, - 0X8805,0XD367,0X884C,0XD2AB,0X8894,0XD1EF,0X88DD,0XD134,0X8927,0XD079,0X8972,0XCFBE,0X89BE,0XCF04,0X8A0C,0XCE4B,0X8A5A,0XCD92,0X8AAA,0XCCD9, - 0X8AFB,0XCC21,0X8B4D,0XCB69,0X8BA0,0XCAB2,0X8BF5,0XC9FC,0X8C4A,0XC946,0X8CA1,0XC890,0X8CF8,0XC7DB,0X8D51,0XC727,0X8DAB,0XC673,0X8E06,0XC5C0, - 0X8E62,0XC50D,0X8EBF,0XC45B,0X8F1D,0XC3A9,0X8F7D,0XC2F8,0X8FDD,0XC248,0X903E,0XC198,0X90A1,0XC0E9,0X9105,0XC03A,0X9169,0XBF8C,0X91CF,0XBEDF, - 0X9236,0XBE32,0X929E,0XBD86,0X9307,0XBCDA,0X9371,0XBC2F,0X93DC,0XBB85,0X9448,0XBADC,0X94B5,0XBA33,0X9523,0XB98B,0X9592,0XB8E3,0X9603,0XB83C, - 0X9674,0XB796,0X96E6,0XB6F1,0X9759,0XB64C,0X97CE,0XB5A8,0X9843,0XB505,0X98B9,0XB462,0X9930,0XB3C0,0X99A9,0XB31F,0X9A22,0XB27F,0X9A9C,0XB1DF, - 0X9B17,0XB140,0X9B94,0XB0A2,0X9C11,0XB005,0X9C8F,0XAF68,0X9D0E,0XAECC,0X9D8E,0XAE31,0X9E0F,0XAD97,0X9E91,0XACFD,0X9F14,0XAC65,0X9F98,0XABCD, - 0XA01C,0XAB36,0XA0A2,0XAAA0,0XA129,0XAA0A,0XA1B0,0XA976,0XA238,0XA8E2,0XA2C2,0XA84F,0XA34C,0XA7BD,0XA3D7,0XA72C,0XA463,0XA69C,0XA4F0,0XA60C, - 0XA57E,0XA57E,0XA60C,0XA4F0,0XA69C,0XA463,0XA72C,0XA3D7,0XA7BD,0XA34C,0XA84F,0XA2C2,0XA8E2,0XA238,0XA976,0XA1B0,0XAA0A,0XA129,0XAAA0,0XA0A2, - 0XAB36,0XA01C,0XABCD,0X9F98,0XAC65,0X9F14,0XACFD,0X9E91,0XAD97,0X9E0F,0XAE31,0X9D8E,0XAECC,0X9D0E,0XAF68,0X9C8F,0XB005,0X9C11,0XB0A2,0X9B94, - 0XB140,0X9B17,0XB1DF,0X9A9C,0XB27F,0X9A22,0XB31F,0X99A9,0XB3C0,0X9930,0XB462,0X98B9,0XB505,0X9843,0XB5A8,0X97CE,0XB64C,0X9759,0XB6F1,0X96E6, - 0XB796,0X9674,0XB83C,0X9603,0XB8E3,0X9592,0XB98B,0X9523,0XBA33,0X94B5,0XBADC,0X9448,0XBB85,0X93DC,0XBC2F,0X9371,0XBCDA,0X9307,0XBD86,0X929E, - 0XBE32,0X9236,0XBEDF,0X91CF,0XBF8C,0X9169,0XC03A,0X9105,0XC0E9,0X90A1,0XC198,0X903E,0XC248,0X8FDD,0XC2F8,0X8F7D,0XC3A9,0X8F1D,0XC45B,0X8EBF, - 0XC50D,0X8E62,0XC5C0,0X8E06,0XC673,0X8DAB,0XC727,0X8D51,0XC7DB,0X8CF8,0XC890,0X8CA1,0XC946,0X8C4A,0XC9FC,0X8BF5,0XCAB2,0X8BA0,0XCB69,0X8B4D, - 0XCC21,0X8AFB,0XCCD9,0X8AAA,0XCD92,0X8A5A,0XCE4B,0X8A0C,0XCF04,0X89BE,0XCFBE,0X8972,0XD079,0X8927,0XD134,0X88DD,0XD1EF,0X8894,0XD2AB,0X884C, - 0XD367,0X8805,0XD424,0X87C0,0XD4E1,0X877B,0XD59E,0X8738,0XD65C,0X86F6,0XD71B,0X86B6,0XD7D9,0X8676,0XD898,0X8637,0XD958,0X85FA,0XDA18,0X85BE, - 0XDAD8,0X8583,0XDB99,0X8549,0XDC59,0X8511,0XDD1B,0X84D9,0XDDDC,0X84A3,0XDE9E,0X846E,0XDF61,0X843A,0XE023,0X8407,0XE0E6,0X83D6,0XE1A9,0X83A6, - 0XE26D,0X8377,0XE330,0X8349,0XE3F4,0X831C,0XE4B9,0X82F1,0XE57D,0X82C6,0XE642,0X829D,0XE707,0X8276,0XE7CD,0X824F,0XE892,0X822A,0XE958,0X8205, - 0XEA1E,0X81E2,0XEAE4,0X81C1,0XEBAB,0X81A0,0XEC71,0X8181,0XED38,0X8163,0XEDFF,0X8146,0XEEC6,0X812A,0XEF8D,0X8110,0XF055,0X80F6,0XF11C,0X80DE, - 0XF1E4,0X80C8,0XF2AC,0X80B2,0XF374,0X809E,0XF43C,0X808B,0XF505,0X8079,0XF5CD,0X8068,0XF695,0X8059,0XF75E,0X804B,0XF827,0X803E,0XF8EF,0X8032, - 0XF9B8,0X8027,0XFA81,0X801E,0XFB4A,0X8016,0XFC13,0X800F,0XFCDC,0X800A,0XFDA5,0X8006,0XFE6E,0X8002,0XFF37,0X8001,0X0000,0X8001,0X00C9,0X8001, - 0X0192,0X8002,0X025B,0X8006,0X0324,0X800A,0X03ED,0X800F,0X04B6,0X8016,0X057F,0X801E,0X0648,0X8027,0X0711,0X8032,0X07D9,0X803E,0X08A2,0X804B, - 0X096B,0X8059,0X0A33,0X8068,0X0AFB,0X8079,0X0BC4,0X808B,0X0C8C,0X809E,0X0D54,0X80B2,0X0E1C,0X80C8,0X0EE4,0X80DE,0X0FAB,0X80F6,0X1073,0X8110, - 0X113A,0X812A,0X1201,0X8146,0X12C8,0X8163,0X138F,0X8181,0X1455,0X81A0,0X151C,0X81C1,0X15E2,0X81E2,0X16A8,0X8205,0X176E,0X822A,0X1833,0X824F, - 0X18F9,0X8276,0X19BE,0X829D,0X1A83,0X82C6,0X1B47,0X82F1,0X1C0C,0X831C,0X1CD0,0X8349,0X1D93,0X8377,0X1E57,0X83A6,0X1F1A,0X83D6,0X1FDD,0X8407, - 0X209F,0X843A,0X2162,0X846E,0X2224,0X84A3,0X22E5,0X84D9,0X23A7,0X8511,0X2467,0X8549,0X2528,0X8583,0X25E8,0X85BE,0X26A8,0X85FA,0X2768,0X8637, - 0X2827,0X8676,0X28E5,0X86B6,0X29A4,0X86F6,0X2A62,0X8738,0X2B1F,0X877B,0X2BDC,0X87C0,0X2C99,0X8805,0X2D55,0X884C,0X2E11,0X8894,0X2ECC,0X88DD, - 0X2F87,0X8927,0X3042,0X8972,0X30FC,0X89BE,0X31B5,0X8A0C,0X326E,0X8A5A,0X3327,0X8AAA,0X33DF,0X8AFB,0X3497,0X8B4D,0X354E,0X8BA0,0X3604,0X8BF5, - 0X36BA,0X8C4A,0X3770,0X8CA1,0X3825,0X8CF8,0X38D9,0X8D51,0X398D,0X8DAB,0X3A40,0X8E06,0X3AF3,0X8E62,0X3BA5,0X8EBF,0X3C57,0X8F1D,0X3D08,0X8F7D, - 0X3DB8,0X8FDD,0X3E68,0X903E,0X3F17,0X90A1,0X3FC6,0X9105,0X4074,0X9169,0X4121,0X91CF,0X41CE,0X9236,0X427A,0X929E,0X4326,0X9307,0X43D1,0X9371, - 0X447B,0X93DC,0X4524,0X9448,0X45CD,0X94B5,0X4675,0X9523,0X471D,0X9592,0X47C4,0X9603,0X486A,0X9674,0X490F,0X96E6,0X49B4,0X9759,0X4A58,0X97CE, - 0X4AFB,0X9843,0X4B9E,0X98B9,0X4C40,0X9930,0X4CE1,0X99A9,0X4D81,0X9A22,0X4E21,0X9A9C,0X4EC0,0X9B17,0X4F5E,0X9B94,0X4FFB,0X9C11,0X5098,0X9C8F, - 0X5134,0X9D0E,0X51CF,0X9D8E,0X5269,0X9E0F,0X5303,0X9E91,0X539B,0X9F14,0X5433,0X9F98,0X54CA,0XA01C,0X5560,0XA0A2,0X55F6,0XA129,0X568A,0XA1B0, - 0X571E,0XA238,0X57B1,0XA2C2,0X5843,0XA34C,0X58D4,0XA3D7,0X5964,0XA463,0X59F4,0XA4F0,0X5A82,0XA57E,0X5B10,0XA60C,0X5B9D,0XA69C,0X5C29,0XA72C, - 0X5CB4,0XA7BD,0X5D3E,0XA84F,0X5DC8,0XA8E2,0X5E50,0XA976,0X5ED7,0XAA0A,0X5F5E,0XAAA0,0X5FE4,0XAB36,0X6068,0XABCD,0X60EC,0XAC65,0X616F,0XACFD, - 0X61F1,0XAD97,0X6272,0XAE31,0X62F2,0XAECC,0X6371,0XAF68,0X63EF,0XB005,0X646C,0XB0A2,0X64E9,0XB140,0X6564,0XB1DF,0X65DE,0XB27F,0X6657,0XB31F, - 0X66D0,0XB3C0,0X6747,0XB462,0X67BD,0XB505,0X6832,0XB5A8,0X68A7,0XB64C,0X691A,0XB6F1,0X698C,0XB796,0X69FD,0XB83C,0X6A6E,0XB8E3,0X6ADD,0XB98B, - 0X6B4B,0XBA33,0X6BB8,0XBADC,0X6C24,0XBB85,0X6C8F,0XBC2F,0X6CF9,0XBCDA,0X6D62,0XBD86,0X6DCA,0XBE32,0X6E31,0XBEDF,0X6E97,0XBF8C,0X6EFB,0XC03A, - 0X6F5F,0XC0E9,0X6FC2,0XC198,0X7023,0XC248,0X7083,0XC2F8,0X70E3,0XC3A9,0X7141,0XC45B,0X719E,0XC50D,0X71FA,0XC5C0,0X7255,0XC673,0X72AF,0XC727, - 0X7308,0XC7DB,0X735F,0XC890,0X73B6,0XC946,0X740B,0XC9FC,0X7460,0XCAB2,0X74B3,0XCB69,0X7505,0XCC21,0X7556,0XCCD9,0X75A6,0XCD92,0X75F4,0XCE4B, - 0X7642,0XCF04,0X768E,0XCFBE,0X76D9,0XD079,0X7723,0XD134,0X776C,0XD1EF,0X77B4,0XD2AB,0X77FB,0XD367,0X7840,0XD424,0X7885,0XD4E1,0X78C8,0XD59E, - 0X790A,0XD65C,0X794A,0XD71B,0X798A,0XD7D9,0X79C9,0XD898,0X7A06,0XD958,0X7A42,0XDA18,0X7A7D,0XDAD8,0X7AB7,0XDB99,0X7AEF,0XDC59,0X7B27,0XDD1B, - 0X7B5D,0XDDDC,0X7B92,0XDE9E,0X7BC6,0XDF61,0X7BF9,0XE023,0X7C2A,0XE0E6,0X7C5A,0XE1A9,0X7C89,0XE26D,0X7CB7,0XE330,0X7CE4,0XE3F4,0X7D0F,0XE4B9, - 0X7D3A,0XE57D,0X7D63,0XE642,0X7D8A,0XE707,0X7DB1,0XE7CD,0X7DD6,0XE892,0X7DFB,0XE958,0X7E1E,0XEA1E,0X7E3F,0XEAE4,0X7E60,0XEBAB,0X7E7F,0XEC71, - 0X7E9D,0XED38,0X7EBA,0XEDFF,0X7ED6,0XEEC6,0X7EF0,0XEF8D,0X7F0A,0XF055,0X7F22,0XF11C,0X7F38,0XF1E4,0X7F4E,0XF2AC,0X7F62,0XF374,0X7F75,0XF43C, - 0X7F87,0XF505,0X7F98,0XF5CD,0X7FA7,0XF695,0X7FB5,0XF75E,0X7FC2,0XF827,0X7FCE,0XF8EF,0X7FD9,0XF9B8,0X7FE2,0XFA81,0X7FEA,0XFB4A,0X7FF1,0XFC13, - 0X7FF6,0XFCDC,0X7FFA,0XFDA5,0X7FFE,0XFE6E,0X7FFF,0XFF37}; -#elif defined(MAX_FFT_SIZE_512) -const INT16 radix4FftTwiddleArr[2*MAX_FFT_SIZE] = {0X7FFF,0x0000,0x7FFE,0x0192,0x7FF6,0x0324,0x7FEA,0x04B6,0x7FD9,0x0648,0x7FC2,0x07D9,0x7FA7,0x096B,0x7F87,0x0AFB,0x7F62,0x0C8C,0x7F38,0x0E1C,0x7F0A,0x0FAB,0x7ED6,0x113A,0x7E9D,0x12C8,0x7E60,0x1455,0x7E1E,0x15E2,0x7DD6,0x176E,0x7D8A,0x18F9,0x7D3A,0x1A83,0x7CE4,0x1C0C,0x7C89,0x1D93,0x7C2A,0x1F1A,0x7BC6,0x209F,0x7B5D,0x2224,0x7AEF,0x23A7,0x7A7D,0x2528,0x7A06,0x26A8,0x798A,0x2827,0x790A,0x29A4,0x7885,0x2B1F,0x77FB,0x2C99,0x776C,0x2E11,0x76D9,0x2F87,0x7642,0x30FC,0x75A6,0x326E,0x7505,0x33DF,0x7460,0x354E,0x73B6,0x36BA,0x7308,0x3825,0x7255,0x398D,0x719E,0x3AF3,0x70E3,0x3C57,0x7023,0x3DB8,0x6F5F,0x3F17,0x6E97,0x4074,0x6DCA,0x41CE,0x6CF9,0x4326,0x6C24,0x447B,0x6B4B,0x45CD,0x6A6E,0x471D,0x698C,0x486A,0x68A7,0x49B4,0x67BD,0x4AFB,0x66D0,0x4C40,0x65DE,0x4D81,0x64E9,0x4EC0,0x63EF,0x4FFB,0x62F2,0x5134,0x61F1,0x5269,0x60EC,0x539B,0x5FE4,0x54CA,0x5ED7,0x55F6,0x5DC8,0x571E,0x5CB4,0x5843,0x5B9D,0x5964,0x5A82,0x5A82,0x5964,0x5B9D,0x5843,0x5CB4,0x571E,0x5DC8,0x55F6,0x5ED7,0x54CA,0x5FE4,0x539B,0x60EC,0x5269,0x61F1,0x5134,0x62F2,0x4FFB,0x63EF,0x4EC0,0x64E9,0x4D81,0x65DE,0x4C40,0x66D0,0x4AFB,0x67BD,0x49B4,0x68A7,0x486A,0x698C,0x471D,0x6A6E,0x45CD,0x6B4B,0x447B,0x6C24,0x4326,0x6CF9,0x41CE,0x6DCA,0x4074,0x6E97,0x3F17,0x6F5F,0x3DB8,0x7023,0x3C57,0x70E3,0x3AF3,0x719E,0x398D,0x7255,0x3825,0x7308,0x36BA,0x73B6,0x354E,0x7460,0x33DF,0x7505,0x326E,0x75A6,0x30FC,0x7642,0x2F87,0x76D9,0x2E11,0x776C,0x2C99,0x77FB,0x2B1F,0x7885,0x29A4,0x790A,0x2827,0x798A,0x26A8,0x7A06,0x2528,0x7A7D,0x23A7,0x7AEF,0x2224,0x7B5D,0x209F,0x7BC6,0x1F1A,0x7C2A,0x1D93,0x7C89,0x1C0C,0x7CE4,0x1A83,0x7D3A,0x18F9,0x7D8A,0x176E,0x7DD6,0x15E2,0x7E1E,0x1455,0x7E60,0x12C8,0x7E9D,0x113A,0x7ED6,0x0FAB,0x7F0A,0x0E1C,0x7F38,0x0C8C,0x7F62,0x0AFB,0x7F87,0x096B,0x7FA7,0x07D9,0x7FC2,0x0648,0x7FD9,0x04B6,0x7FEA,0x0324,0x7FF6,0x0192,0x7FFE,0x0000,0x7FFF,0xFE6E,0x7FFE,0xFCDC,0x7FF6,0xFB4A,0x7FEA,0xF9B8,0x7FD9,0xF827,0x7FC2,0xF695,0x7FA7,0xF505,0x7F87,0xF374,0x7F62,0xF1E4,0x7F38,0xF055,0x7F0A,0xEEC6,0x7ED6,0xED38,0x7E9D,0xEBAB,0x7E60,0xEA1E,0x7E1E,0xE892,0x7DD6,0xE707,0x7D8A,0xE57D,0x7D3A,0xE3F4,0x7CE4,0xE26D,0x7C89,0xE0E6,0x7C2A,0xDF61,0x7BC6,0xDDDC,0x7B5D,0xDC59,0x7AEF,0xDAD8,0x7A7D,0xD958,0x7A06,0xD7D9,0x798A,0xD65C,0x790A,0xD4E1,0x7885,0xD367,0x77FB,0xD1EF,0x776C,0xD079,0x76D9,0xCF04,0x7642,0xCD92,0x75A6,0xCC21,0x7505,0xCAB2,0x7460,0xC946,0x73B6,0xC7DB,0x7308,0xC673,0x7255,0xC50D,0x719E,0xC3A9,0x70E3,0xC248,0x7023,0xC0E9,0x6F5F,0xBF8C,0x6E97,0xBE32,0x6DCA,0xBCDA,0x6CF9,0xBB85,0x6C24,0xBA33,0x6B4B,0xB8E3,0x6A6E,0xB796,0x698C,0xB64C,0x68A7,0xB505,0x67BD,0xB3C0,0x66D0,0xB27F,0x65DE,0xB140,0x64E9,0xB005,0x63EF,0xAECC,0x62F2,0xAD97,0x61F1,0xAC65,0x60EC,0xAB36,0x5FE4,0xAA0A,0x5ED7,0xA8E2,0x5DC8,0xA7BD,0x5CB4,0xA69C,0x5B9D,0xA57E,0x5A82,0xA463,0x5964,0xA34C,0x5843,0xA238,0x571E,0xA129,0x55F6,0xA01C,0x54CA,0x9F14,0x539B,0x9E0F,0x5269,0x9D0E,0x5134,0x9C11,0x4FFB,0x9B17,0x4EC0,0x9A22,0x4D81,0x9930,0x4C40,0x9843,0x4AFB,0x9759,0x49B4,0x9674,0x486A,0x9592,0x471D,0x94B5,0x45CD,0x93DC,0x447B,0x9307,0x4326,0x9236,0x41CE,0x9169,0x4074,0x90A1,0x3F17,0x8FDD,0x3DB8,0x8F1D,0x3C57,0x8E62,0x3AF3,0x8DAB,0x398D,0x8CF8,0x3825,0x8C4A,0x36BA,0x8BA0,0x354E,0x8AFB,0x33DF,0x8A5A,0x326E,0x89BE,0x30FC,0x8927,0x2F87,0x8894,0x2E11,0x8805,0x2C99,0x877B,0x2B1F,0x86F6,0x29A4,0x8676,0x2827,0x85FA,0x26A8,0x8583,0x2528,0x8511,0x23A7,0x84A3,0x2224,0x843A,0x209F,0x83D6,0x1F1A,0x8377,0x1D93,0x831C,0x1C0C,0x82C6,0x1A83,0x8276,0x18F9,0x822A,0x176E,0x81E2,0x15E2,0x81A0,0x1455,0x8163,0x12C8,0x812A,0x113A,0x80F6,0x0FAB,0x80C8,0x0E1C,0x809E,0x0C8C,0x8079,0x0AFB,0x8059,0x096B,0x803E,0x07D9,0x8027,0x0648,0x8016,0x04B6,0x800A,0x0324,0x8002,0x0192,0x8000,0x0000,0x8002,0xFE6E,0x800A,0xFCDC,0x8016,0xFB4A,0x8027,0xF9B8,0x803E,0xF827,0x8059,0xF695,0x8079,0xF505,0x809E,0xF374,0x80C8,0xF1E4,0x80F6,0xF055,0x812A,0xEEC6,0x8163,0xED38,0x81A0,0xEBAB,0x81E2,0xEA1E,0x822A,0xE892,0x8276,0xE707,0x82C6,0xE57D,0x831C,0xE3F4,0x8377,0xE26D,0x83D6,0xE0E6,0x843A,0xDF61,0x84A3,0xDDDC,0x8511,0xDC59,0x8583,0xDAD8,0x85FA,0xD958,0x8676,0xD7D9,0x86F6,0xD65C,0x877B,0xD4E1,0x8805,0xD367,0x8894,0xD1EF,0x8927,0xD079,0x89BE,0xCF04,0x8A5A,0xCD92,0x8AFB,0xCC21,0x8BA0,0xCAB2,0x8C4A,0xC946,0x8CF8,0xC7DB,0x8DAB,0xC673,0x8E62,0xC50D,0x8F1D,0xC3A9,0x8FDD,0xC248,0x90A1,0xC0E9,0x9169,0xBF8C,0x9236,0xBE32,0x9307,0xBCDA,0x93DC,0xBB85,0x94B5,0xBA33,0x9592,0xB8E3,0x9674,0xB796,0x9759,0xB64C,0x9843,0xB505,0x9930,0xB3C0,0x9A22,0xB27F,0x9B17,0xB140,0x9C11,0xB005,0x9D0E,0xAECC,0x9E0F,0xAD97,0x9F14,0xAC65,0xA01C,0xAB36,0xA129,0xAA0A,0xA238,0xA8E2,0xA34C,0xA7BD,0xA463,0xA69C,0xA57E,0xA57E,0xA69C,0xA463,0xA7BD,0xA34C,0xA8E2,0xA238,0xAA0A,0xA129,0xAB36,0xA01C,0xAC65,0x9F14,0xAD97,0x9E0F,0xAECC,0x9D0E,0xB005,0x9C11,0xB140,0x9B17,0xB27F,0x9A22,0xB3C0,0x9930,0xB505,0x9843,0xB64C,0x9759,0xB796,0x9674,0xB8E3,0x9592,0xBA33,0x94B5,0xBB85,0x93DC,0xBCDA,0x9307,0xBE32,0x9236,0xBF8C,0x9169,0xC0E9,0x90A1,0xC248,0x8FDD,0xC3A9,0x8F1D,0xC50D,0x8E62,0xC673,0x8DAB,0xC7DB,0x8CF8,0xC946,0x8C4A,0xCAB2,0x8BA0,0xCC21,0x8AFB,0xCD92,0x8A5A,0xCF04,0x89BE,0xD079,0x8927,0xD1EF,0x8894,0xD367,0x8805,0xD4E1,0x877B,0xD65C,0x86F6,0xD7D9,0x8676,0xD958,0x85FA,0xDAD8,0x8583,0xDC59,0x8511,0xDDDC,0x84A3,0xDF61,0x843A,0xE0E6,0x83D6,0xE26D,0x8377,0xE3F4,0x831C,0xE57D,0x82C6,0xE707,0x8276,0xE892,0x822A,0xEA1E,0x81E2,0xEBAB,0x81A0,0xED38,0x8163,0xEEC6,0x812A,0xF055,0x80F6,0xF1E4,0x80C8,0xF374,0x809E,0xF505,0x8079,0xF695,0x8059,0xF827,0x803E,0xF9B8,0x8027,0xFB4A,0x8016,0xFCDC,0x800A,0xFE6E,0x8002,0x0000,0x8000,0x0192,0x8002,0x0324,0x800A,0x04B6,0x8016,0x0648,0x8027,0x07D9,0x803E,0x096B,0x8059,0x0AFB,0x8079,0x0C8C,0x809E,0x0E1C,0x80C8,0x0FAB,0x80F6,0x113A,0x812A,0x12C8,0x8163,0x1455,0x81A0,0x15E2,0x81E2,0x176E,0x822A,0x18F9,0x8276,0x1A83,0x82C6,0x1C0C,0x831C,0x1D93,0x8377,0x1F1A,0x83D6,0x209F,0x843A,0x2224,0x84A3,0x23A7,0x8511,0x2528,0x8583,0x26A8,0x85FA,0x2827,0x8676,0x29A4,0x86F6,0x2B1F,0x877B,0x2C99,0x8805,0x2E11,0x8894,0x2F87,0x8927,0x30FC,0x89BE,0x326E,0x8A5A,0x33DF,0x8AFB,0x354E,0x8BA0,0x36BA,0x8C4A,0x3825,0x8CF8,0x398D,0x8DAB,0x3AF3,0x8E62,0x3C57,0x8F1D,0x3DB8,0x8FDD,0x3F17,0x90A1,0x4074,0x9169,0x41CE,0x9236,0x4326,0x9307,0x447B,0x93DC,0x45CD,0x94B5,0x471D,0x9592,0x486A,0x9674,0x49B4,0x9759,0x4AFB,0x9843,0x4C40,0x9930,0x4D81,0x9A22,0x4EC0,0x9B17,0x4FFB,0x9C11,0x5134,0x9D0E,0x5269,0x9E0F,0x539B,0x9F14,0x54CA,0xA01C,0x55F6,0xA129,0x571E,0xA238,0x5843,0xA34C,0x5964,0xA463,0x5A82,0xA57E,0x5B9D,0xA69C,0x5CB4,0xA7BD,0x5DC8,0xA8E2,0x5ED7,0xAA0A,0x5FE4,0xAB36,0x60EC,0xAC65,0x61F1,0xAD97,0x62F2,0xAECC,0x63EF,0xB005,0x64E9,0xB140,0x65DE,0xB27F,0x66D0,0xB3C0,0x67BD,0xB505,0x68A7,0xB64C,0x698C,0xB796,0x6A6E,0xB8E3,0x6B4B,0xBA33,0x6C24,0xBB85,0x6CF9,0xBCDA,0x6DCA,0xBE32,0x6E97,0xBF8C,0x6F5F,0xC0E9,0x7023,0xC248,0x70E3,0xC3A9,0x719E,0xC50D,0x7255,0xC673,0x7308,0xC7DB,0x73B6,0xC946,0x7460,0xCAB2,0x7505,0xCC21,0x75A6,0xCD92,0x7642,0xCF04,0x76D9,0xD079,0x776C,0xD1EF,0x77FB,0xD367,0x7885,0xD4E1,0x790A,0xD65C,0x798A,0xD7D9,0x7A06,0xD958,0x7A7D,0xDAD8,0x7AEF,0xDC59,0x7B5D,0xDDDC,0x7BC6,0xDF61,0x7C2A,0xE0E6,0x7C89,0xE26D,0x7CE4,0xE3F4,0x7D3A,0xE57D,0x7D8A,0xE707,0x7DD6,0xE892,0x7E1E,0xEA1E,0x7E60,0xEBAB,0x7E9D,0xED38,0x7ED6,0xEEC6,0x7F0A,0xF055,0x7F38,0xF1E4,0x7F62,0xF374,0x7F87,0xF505,0x7FA7,0xF695,0x7FC2,0xF827,0x7FD9,0xF9B8,0x7FEA,0xFB4A,0x7FF6,0xFCDC,0x7FFE,0xFE6E}; -#elif defined(MAX_FFT_SIZE_256) -#ifdef TWIDDLE_HALF_SIZE -const INT16 radix4FftTwiddleArr[MAX_FFT_SIZE] = {0X7FFF,0x0000,0x7FF6,0x0324,0x7FD9,0x0648,0x7FA7,0x096B,0x7F62,0x0C8C,0x7F0A,0x0FAB,0x7E9D,0x12C8,0x7E1E,0x15E2,0x7D8A,0x18F9,0x7CE4,0x1C0C,0x7C2A,0x1F1A,0x7B5D,0x2224,0x7A7D,0x2528,0x798A,0x2827,0x7885,0x2B1F,0x776C,0x2E11,0x7642,0x30FC,0x7505,0x33DF,0x73B6,0x36BA,0x7255,0x398D,0x70E3,0x3C57,0x6F5F,0x3F17,0x6DCA,0x41CE,0x6C24,0x447B,0x6A6E,0x471D,0x68A7,0x49B4,0x66D0,0x4C40,0x64E9,0x4EC0,0x62F2,0x5134,0x60EC,0x539B,0x5ED7,0x55F6,0x5CB4,0x5843,0x5A82,0x5A82,0x5843,0x5CB4,0x55F6,0x5ED7,0x539B,0x60EC,0x5134,0x62F2,0x4EC0,0x64E9,0x4C40,0x66D0,0x49B4,0x68A7,0x471D,0x6A6E,0x447B,0x6C24,0x41CE,0x6DCA,0x3F17,0x6F5F,0x3C57,0x70E3,0x398D,0x7255,0x36BA,0x73B6,0x33DF,0x7505,0x30FC,0x7642,0x2E11,0x776C,0x2B1F,0x7885,0x2827,0x798A,0x2528,0x7A7D,0x2224,0x7B5D,0x1F1A,0x7C2A,0x1C0C,0x7CE4,0x18F9,0x7D8A,0x15E2,0x7E1E,0x12C8,0x7E9D,0x0FAB,0x7F0A,0x0C8C,0x7F62,0x096B,0x7FA7,0x0648,0x7FD9,0x0324,0x7FF6,0x0000,0x7FFF,0xFCDC,0x7FF6,0xF9B8,0x7FD9,0xF695,0x7FA7,0xF374,0x7F62,0xF055,0x7F0A,0xED38,0x7E9D,0xEA1E,0x7E1E,0xE707,0x7D8A,0xE3F4,0x7CE4,0xE0E6,0x7C2A,0xDDDC,0x7B5D,0xDAD8,0x7A7D,0xD7D9,0x798A,0xD4E1,0x7885,0xD1EF,0x776C,0xCF04,0x7642,0xCC21,0x7505,0xC946,0x73B6,0xC673,0x7255,0xC3A9,0x70E3,0xC0E9,0x6F5F,0xBE32,0x6DCA,0xBB85,0x6C24,0xB8E3,0x6A6E,0xB64C,0x68A7,0xB3C0,0x66D0,0xB140,0x64E9,0xAECC,0x62F2,0xAC65,0x60EC,0xAA0A,0x5ED7,0xA7BD,0x5CB4,0xA57E,0x5A82,0xA34C,0x5843,0xA129,0x55F6,0x9F14,0x539B,0x9D0E,0x5134,0x9B17,0x4EC0,0x9930,0x4C40,0x9759,0x49B4,0x9592,0x471D,0x93DC,0x447B,0x9236,0x41CE,0x90A1,0x3F17,0x8F1D,0x3C57,0x8DAB,0x398D,0x8C4A,0x36BA,0x8AFB,0x33DF,0x89BE,0x30FC,0x8894,0x2E11,0x877B,0x2B1F,0x8676,0x2827,0x8583,0x2528,0x84A3,0x2224,0x83D6,0x1F1A,0x831C,0x1C0C,0x8276,0x18F9,0x81E2,0x15E2,0x8163,0x12C8,0x80F6,0x0FAB,0x809E,0x0C8C,0x8059,0x096B,0x8027,0x0648,0x800A,0x0324}; -#else -const INT16 radix4FftTwiddleArr[2*MAX_FFT_SIZE] = {0X7FFF,0x0000,0x7FF6,0x0324,0x7FD9,0x0648,0x7FA7,0x096B,0x7F62,0x0C8C,0x7F0A,0x0FAB,0x7E9D,0x12C8,0x7E1E,0x15E2,0x7D8A,0x18F9,0x7CE4,0x1C0C,0x7C2A,0x1F1A,0x7B5D,0x2224,0x7A7D,0x2528,0x798A,0x2827,0x7885,0x2B1F,0x776C,0x2E11,0x7642,0x30FC,0x7505,0x33DF,0x73B6,0x36BA,0x7255,0x398D,0x70E3,0x3C57,0x6F5F,0x3F17,0x6DCA,0x41CE,0x6C24,0x447B,0x6A6E,0x471D,0x68A7,0x49B4,0x66D0,0x4C40,0x64E9,0x4EC0,0x62F2,0x5134,0x60EC,0x539B,0x5ED7,0x55F6,0x5CB4,0x5843,0x5A82,0x5A82,0x5843,0x5CB4,0x55F6,0x5ED7,0x539B,0x60EC,0x5134,0x62F2,0x4EC0,0x64E9,0x4C40,0x66D0,0x49B4,0x68A7,0x471D,0x6A6E,0x447B,0x6C24,0x41CE,0x6DCA,0x3F17,0x6F5F,0x3C57,0x70E3,0x398D,0x7255,0x36BA,0x73B6,0x33DF,0x7505,0x30FC,0x7642,0x2E11,0x776C,0x2B1F,0x7885,0x2827,0x798A,0x2528,0x7A7D,0x2224,0x7B5D,0x1F1A,0x7C2A,0x1C0C,0x7CE4,0x18F9,0x7D8A,0x15E2,0x7E1E,0x12C8,0x7E9D,0x0FAB,0x7F0A,0x0C8C,0x7F62,0x096B,0x7FA7,0x0648,0x7FD9,0x0324,0x7FF6,0x0000,0x7FFF,0xFCDC,0x7FF6,0xF9B8,0x7FD9,0xF695,0x7FA7,0xF374,0x7F62,0xF055,0x7F0A,0xED38,0x7E9D,0xEA1E,0x7E1E,0xE707,0x7D8A,0xE3F4,0x7CE4,0xE0E6,0x7C2A,0xDDDC,0x7B5D,0xDAD8,0x7A7D,0xD7D9,0x798A,0xD4E1,0x7885,0xD1EF,0x776C,0xCF04,0x7642,0xCC21,0x7505,0xC946,0x73B6,0xC673,0x7255,0xC3A9,0x70E3,0xC0E9,0x6F5F,0xBE32,0x6DCA,0xBB85,0x6C24,0xB8E3,0x6A6E,0xB64C,0x68A7,0xB3C0,0x66D0,0xB140,0x64E9,0xAECC,0x62F2,0xAC65,0x60EC,0xAA0A,0x5ED7,0xA7BD,0x5CB4,0xA57E,0x5A82,0xA34C,0x5843,0xA129,0x55F6,0x9F14,0x539B,0x9D0E,0x5134,0x9B17,0x4EC0,0x9930,0x4C40,0x9759,0x49B4,0x9592,0x471D,0x93DC,0x447B,0x9236,0x41CE,0x90A1,0x3F17,0x8F1D,0x3C57,0x8DAB,0x398D,0x8C4A,0x36BA,0x8AFB,0x33DF,0x89BE,0x30FC,0x8894,0x2E11,0x877B,0x2B1F,0x8676,0x2827,0x8583,0x2528,0x84A3,0x2224,0x83D6,0x1F1A,0x831C,0x1C0C,0x8276,0x18F9,0x81E2,0x15E2,0x8163,0x12C8,0x80F6,0x0FAB,0x809E,0x0C8C,0x8059,0x096B,0x8027,0x0648,0x800A,0x0324,0x8000,0x0000,0x800A,0xFCDC,0x8027,0xF9B8,0x8059,0xF695,0x809E,0xF374,0x80F6,0xF055,0x8163,0xED38,0x81E2,0xEA1E,0x8276,0xE707,0x831C,0xE3F4,0x83D6,0xE0E6,0x84A3,0xDDDC,0x8583,0xDAD8,0x8676,0xD7D9,0x877B,0xD4E1,0x8894,0xD1EF,0x89BE,0xCF04,0x8AFB,0xCC21,0x8C4A,0xC946,0x8DAB,0xC673,0x8F1D,0xC3A9,0x90A1,0xC0E9,0x9236,0xBE32,0x93DC,0xBB85,0x9592,0xB8E3,0x9759,0xB64C,0x9930,0xB3C0,0x9B17,0xB140,0x9D0E,0xAECC,0x9F14,0xAC65,0xA129,0xAA0A,0xA34C,0xA7BD,0xA57E,0xA57E,0xA7BD,0xA34C,0xAA0A,0xA129,0xAC65,0x9F14,0xAECC,0x9D0E,0xB140,0x9B17,0xB3C0,0x9930,0xB64C,0x9759,0xB8E3,0x9592,0xBB85,0x93DC,0xBE32,0x9236,0xC0E9,0x90A1,0xC3A9,0x8F1D,0xC673,0x8DAB,0xC946,0x8C4A,0xCC21,0x8AFB,0xCF04,0x89BE,0xD1EF,0x8894,0xD4E1,0x877B,0xD7D9,0x8676,0xDAD8,0x8583,0xDDDC,0x84A3,0xE0E6,0x83D6,0xE3F4,0x831C,0xE707,0x8276,0xEA1E,0x81E2,0xED38,0x8163,0xF055,0x80F6,0xF374,0x809E,0xF695,0x8059,0xF9B8,0x8027,0xFCDC,0x800A,0x0000,0x8000,0x0324,0x800A,0x0648,0x8027,0x096B,0x8059,0x0C8C,0x809E,0x0FAB,0x80F6,0x12C8,0x8163,0x15E2,0x81E2,0x18F9,0x8276,0x1C0C,0x831C,0x1F1A,0x83D6,0x2224,0x84A3,0x2528,0x8583,0x2827,0x8676,0x2B1F,0x877B,0x2E11,0x8894,0x30FC,0x89BE,0x33DF,0x8AFB,0x36BA,0x8C4A,0x398D,0x8DAB,0x3C57,0x8F1D,0x3F17,0x90A1,0x41CE,0x9236,0x447B,0x93DC,0x471D,0x9592,0x49B4,0x9759,0x4C40,0x9930,0x4EC0,0x9B17,0x5134,0x9D0E,0x539B,0x9F14,0x55F6,0xA129,0x5843,0xA34C,0x5A82,0xA57E,0x5CB4,0xA7BD,0x5ED7,0xAA0A,0x60EC,0xAC65,0x62F2,0xAECC,0x64E9,0xB140,0x66D0,0xB3C0,0x68A7,0xB64C,0x6A6E,0xB8E3,0x6C24,0xBB85,0x6DCA,0xBE32,0x6F5F,0xC0E9,0x70E3,0xC3A9,0x7255,0xC673,0x73B6,0xC946,0x7505,0xCC21,0x7642,0xCF04,0x776C,0xD1EF,0x7885,0xD4E1,0x798A,0xD7D9,0x7A7D,0xDAD8,0x7B5D,0xDDDC,0x7C2A,0xE0E6,0x7CE4,0xE3F4,0x7D8A,0xE707,0x7E1E,0xEA1E,0x7E9D,0xED38,0x7F0A,0xF055,0x7F62,0xF374,0x7FA7,0xF695,0x7FD9,0xF9B8,0x7FF6,0xFCDC}; -#endif -#endif - -#ifdef DOUBLE -#include -#define PI 3.141592653589793 -#endif - -#if !(defined(ARM_DS5) || defined(W8987)) -unsigned int __clz(uint32_t x){ -#ifdef ARM_GCC - int ret; - asm ("clz %0, %1 ; \n" - : "=r"(ret) :"r"(x)); - return ret; -#else - int ret = 0; - while(!(x&0x80000000) && (ret<32)){ - x<<=1; - ret++; - } - return ret; -#endif -} -#endif - - - -#if defined(ARM_GCC) || defined(ARM_DS5) -#define REV_IDX(x) reverse(x) -#define IDX_LEN 32 -unsigned int reverse(register unsigned int x){ - int ret; -#ifdef ARM_DS5 - __asm ("rbit ret, x"); -#else - asm ("rbit %0, %1 ; \n" - : "=r"(ret) :"r"(x)); -#endif - return ret; -} -#else -#if (MAX_FFT_SIZE > (1<<8)) -#define REV_IDX(x) reverse(x) -#define IDX_LEN 32 -#else -#define REV_IDX(x) reverse8(x) -#define IDX_LEN 8 -#endif -unsigned int reverse(register unsigned int x){ - x = (((x & 0xaaaaaaaa) >> 1) | ((x & 0x55555555) << 1)); - x = (((x & 0xcccccccc) >> 2) | ((x & 0x33333333) << 2)); - x = (((x & 0xf0f0f0f0) >> 4) | ((x & 0x0f0f0f0f) << 4)); - x = (((x & 0xff00ff00) >> 8) | ((x & 0x00ff00ff) << 8)); - return((x >> 16) | (x << 16)); -} - -unsigned char reverse8(unsigned char b){ -#ifdef _MSC_VER // 64 bit - b = (unsigned char) ((b * 0x0202020202ULL & 0x010884422010ULL) % 1023); -#else // 32 bit - b = ((b * 0x0802LU & 0x22110LU) | (b * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16; -#endif - return b; -} - -#endif - -#if !(defined(ARM_GCC) || defined(ARM_DS5)) - -static const char my_msb_lut[32] = {0, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8, 31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9}; -int myMsb(unsigned int v){ - v |= v >> 1; - v |= v >> 2; - v |= v >> 4; - v |= v >> 8; - v |= v >> 16; - v = (v >> 1) + 1; - return my_msb_lut[((unsigned int)(v * 0x077CB531UL)) >> 27]; -} - -#ifdef ARM_DEBUG -#define ROUND_CONST15BIT 0x4000 -#define MAX_POS16BIT 0x7fff -#define MIN_NEQ16BIT -0x7fff -#define MAX_POS32BIT 0x7fffffff -#define MIN_NEQ32BIT -0x7fffffff - -INT32 radix4Fft16BitTo32BitMultiplier(INT16 x, INT16 y){ - INT32 temp = x; - temp *=y; - - if(temp>MAX_POS32BIT) - return MAX_POS32BIT; - else{ - if(tempMAX_POS32BIT) - return MAX_POS32BIT; - else{ - if(tempMAX_POS32BIT) - return MAX_POS32BIT; - else{ - if(temp>15)&0x1); -#endif - temp >>=15; - - if(temp>MAX_POS16BIT) - return MAX_POS16BIT; - else{ - if(temp>=(15+extraShift); - - if(temp>0xffff) - return 0xffff; - else{ - if(temp<0) - return 0; - else - return (unsigned short)temp; - } -} - -INT16 radix4Fft16BitAdd(INT16 x, INT16 y){ - INT32 temp = x; - temp +=y; - - if(temp>MAX_POS16BIT) - return MAX_POS16BIT; - else{ - if(tempMAX_POS16BIT) - return MAX_POS16BIT; - else{ - if(temp>1); -} - -INT16 radix4Fft16BitSubtractShift(INT16 x, INT16 y){ - INT32 temp = x; - temp -=y; - - return (INT16)(temp>>1); -} -#endif -#else -#pragma arm -#endif - -void radix4FftRadix4ButterflyTwiddle(INT16* pSrc, INT16* pDst, int stride, const INT16* pCoeff, int coeffStride){ -#if defined(ARM_GCC) || defined(ARM_DS5) -#ifdef ARM_DS5 - int reg5, reg6, reg7, reg8, reg9, reg10, reg11; - int strideDw = 4*stride; - int coeffStrideDw = 4*coeffStride; - - __asm volatile - { - LDR reg11, [pCoeff, coeffStrideDw]! - LDR reg5, [pSrc], strideDw - LDR reg7, [pSrc], strideDw - LDR reg6, [pSrc], strideDw - SMUAD reg10, reg11, reg6 - SMUSDX reg9, reg11, reg6 - LDR reg11, [pCoeff, coeffStrideDw]! - LSL reg9, reg9, #1 - PKHTB reg6, reg9, reg10, ASR #15 - SMUAD reg10, reg11, reg7 - SMUSDX reg9, reg11, reg7 - LDR reg8, [pSrc], strideDw - LDR reg11, [pCoeff, coeffStrideDw] - LSL reg9, reg9, #1 - PKHTB reg7, reg9, reg10, ASR #15 - SMUAD reg10, reg11, reg8 - SMUSDX reg8, reg11, reg8 - LSL reg8, reg8, #1 - PKHTB reg9, reg8, reg10, ASR #15 - QADD16 reg10, reg5, reg7 - QSUB16 reg11, reg5, reg7 - QADD16 reg8, reg6, reg9 - QSUB16 reg9, reg6, reg9 - QADD16 reg5, reg10, reg8 - QSUB16 reg7, reg10, reg8 - QSAX reg6, reg11, reg9 - QASX reg8, reg11, reg9 - STR reg5, [pDst], strideDw - STR reg6, [pDst], strideDw - STR reg7, [pDst], strideDw - STR reg8, [pDst], strideDw - } -#else - asm volatile( - "LDR r11, [%[pCoeff], %[coeffStrideDw]]! \n\t" - "LDR r5, [%[pSrc]], %[strideDw] \n\t" - "LDR r7, [%[pSrc]], %[strideDw] \n\t" - "LDR r6, [%[pSrc]], %[strideDw] \n\t" - "SMUAD r10, r11, r6 \n\t" - "SMUSDX r9, r11, r6 \n\t" - "LDR r11, [%[pCoeff], %[coeffStrideDw]]! \n\t" - "LSL r9, r9, #1 \n\t" - "PKHTB r6, r9, r10, ASR #15 \n\t" - "SMUAD r10, r11, r7 \n\t" - "SMUSDX r9, r11, r7 \n\t" - "LDR r8, [%[pSrc]], %[strideDw] \n\t" - "LDR r11, [%[pCoeff], %[coeffStrideDw]] \n\t" - "LSL r9, r9, #1 \n\t" - "PKHTB r7, r9, r10, ASR #15 \n\t" - "SMUAD r10, r11, r8 \n\t" - "SMUSDX r8, r11, r8 \n\t" - "LSL r8, r8, #1 \n\t" - "PKHTB r9, r8, r10, ASR #15 \n\t" - "QADD16 r10, r5, r7 \n\t" - "QSUB16 r11, r5, r7 \n\t" - "QADD16 r8, r6, r9 \n\t" - "QSUB16 r9, r6, r9 \n\t" - "QADD16 r5, r10, r8 \n\t" - "QSUB16 r7, r10, r8 \n\t" - "QSAX r6, r11, r9 \n\t" - "QASX r8, r11, r9 \n\t" - "STR r5, [%[pDst]], %[strideDw] \n\t" - "STR r6, [%[pDst]], %[strideDw] \n\t" - "STR r7, [%[pDst]], %[strideDw] \n\t" - "STR r8, [%[pDst]], %[strideDw] \n\t" - : [pSrc]"+r"(pSrc), [pDst]"+r"(pDst) : [pCoeff]"r"(pCoeff), [strideDw]"r"(4*stride), [coeffStrideDw]"r"(4*coeffStride) - : "r5", "r6", "r7", "r8", "r9", "r10","r11"); -/* - "SMULBT r8, r6, r11 \n\t" - "SMULTB r9, r6, r11 \n\t" - "QSUB r9, r9, r8 \n\t" -*/ -#endif -#else - INT16 x1I, x1Q, x2I, x2Q, x3I, x3Q; // x0I, x0Q, - INT16 z0I, z0Q, z1I, z1Q, z2I, z2Q, z3I, z3Q; - INT16 y0I, y0Q, y1I, y1Q, y2I, y2Q, y3I, y3Q; - INT16 AI, AQ, BI, BQ, CI, CQ, DI, DQ; - INT16 W1I, W1Q, W2I, W2Q, W3I, W3Q; // W0I, W0Q, - - unsigned int *loadPtr = (unsigned int *)pSrc; - unsigned int loadTemp; - unsigned int *coeffPtr = (unsigned int *)(pCoeff +2*coeffStride); - unsigned int loadCoeff; - unsigned int *storePtr = (unsigned int *)pDst; - unsigned int storeTemp; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - loadTemp = *loadPtr; loadPtr +=stride; - z0I = (loadTemp&0xffff); z0Q = (loadTemp>>16); - loadTemp = *loadPtr; loadPtr +=stride; - x2I = (loadTemp&0xffff); x2Q = (loadTemp>>16); - loadTemp = *loadPtr; loadPtr +=stride; - x1I = (loadTemp&0xffff); x1Q = (loadTemp>>16); - loadTemp = *loadPtr; loadPtr +=stride; - x3I = (loadTemp&0xffff); x3Q = (loadTemp>>16); - - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W1I = (loadCoeff&0xffff); W1Q = (loadCoeff>>16); - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W2I = (loadCoeff&0xffff); W2Q = (loadCoeff>>16); - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W3I = (loadCoeff&0xffff); W3Q = (loadCoeff>>16); - - // z0 = W0*x0, z1 = W1*x1, z2 = W2*x2, z3 = W3*x3 assuming W0Q = -exp(1i*2*pi*nk/N) -#ifdef ARM_DEBUG - z1I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W1I, x1I), radix4Fft16BitTo32BitMultiplier(W1Q, x1Q))); // r6 - z1Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W1I, x1Q), radix4Fft16BitTo32BitMultiplier(W1Q, x1I))); - z2I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W2I, x2I), radix4Fft16BitTo32BitMultiplier(W2Q, x2Q))); // r7 - z2Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W2I, x2Q), radix4Fft16BitTo32BitMultiplier(W2Q, x2I))); - z3I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W3I, x3I), radix4Fft16BitTo32BitMultiplier(W3Q, x3Q))); // r9 - z3Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W3I, x3Q), radix4Fft16BitTo32BitMultiplier(W3Q, x3I))); -#else - z1I = (INT16)((((INT32)W1I*x1I)+((INT32)W1Q*x1Q))>>15); - z1Q = (INT16)((((INT32)W1I*x1Q)-((INT32)W1Q*x1I))>>15); - z2I = (INT16)((((INT32)W2I*x2I)+((INT32)W2Q*x2Q))>>15); - z2Q = (INT16)((((INT32)W2I*x2Q)-((INT32)W2Q*x2I))>>15); - z3I = (INT16)((((INT32)W3I*x3I)+((INT32)W3Q*x3Q))>>15); - z3Q = (INT16)((((INT32)W3I*x3Q)-((INT32)W3Q*x3I))>>15); -#endif - // calculate using A = (z0+z2), B = (z0-z2), C = (z1+z3), D = (z1-z3); - // y0 = z0 + z1 + z2 + z3 = A + C - // y1 = z0 -jz1 - z2 +jz3 = B -jD - // y2 = z0 - z1 + z2 - z3 = A - C - // y3 = z0 +jz1 - z2 -jz3 = B +jD -#ifdef ARM_DEBUG - AI = radix4Fft16BitAdd(z0I, z2I); AQ = radix4Fft16BitAdd(z0Q, z2Q); - BI = radix4Fft16BitSubtract(z0I, z2I); BQ = radix4Fft16BitSubtract(z0Q, z2Q); - CI = radix4Fft16BitAdd(z1I, z3I); CQ = radix4Fft16BitAdd(z1Q, z3Q); - DI = radix4Fft16BitSubtract(z1I, z3I); DQ = radix4Fft16BitSubtract(z1Q, z3Q); - - y0I = radix4Fft16BitAdd(AI, CI); y0Q = radix4Fft16BitAdd(AQ, CQ); - y1I = radix4Fft16BitAdd(BI, DQ); y1Q = radix4Fft16BitSubtract(BQ, DI); - y2I = radix4Fft16BitSubtract(AI, CI); y2Q = radix4Fft16BitSubtract(AQ, CQ); - y3I = radix4Fft16BitSubtract(BI, DQ); y3Q = radix4Fft16BitAdd(BQ, DI); -#else - AI = z0I+z2I; AQ = z0Q+z2Q; - BI = z0I-z2I; BQ = z0Q-z2Q; - CI = z1I+z3I; CQ = z1Q+z3Q; - DI = z1I-z3I; DQ = z1Q-z3Q; - - y0I = AI+CI; y0Q = AQ+CQ; - y1I = BI+DQ; y1Q = BQ-DI; - y2I = AI-CI; y2Q = AQ-CQ; - y3I = BI-DQ; y3Q = BQ+DI; -#endif - storeTemp = (unsigned short)y0Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y0I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y1Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y1I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y2Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y2I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y3Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y3I; - *storePtr = storeTemp; storePtr+=stride; -#endif -} - -void radix4IfftRadix4ButterflyTwiddle(INT16* pSrc, INT16* pDst, int stride, const INT16* pCoeff, int coeffStride){ -#if defined(ARM_GCC) || defined(ARM_DS5) -#ifdef ARM_DS5 - int coeffStrideMult4 = 4*coeffStride; - int strideMult4 = 4*stride; - int reg5, reg6, reg7, reg8, reg9, reg10, reg11; - __asm volatile - { - LDR reg11, [pCoeff, coeffStrideMult4]! - LDR reg5, [pSrc], strideMult4 - LDR reg7, [pSrc], strideMult4 - LDR reg6, [pSrc], strideMult4 - SMUSD reg10, reg11, reg6 - SMUADX reg9, reg11, reg6 - LDR reg11, [pCoeff, coeffStrideMult4]! - LSL reg9, reg9, #1 - PKHTB reg6, reg9, reg10, ASR #15 - SMUSD reg10, reg11, reg7 - SMUADX reg9, reg11, reg7 - LDR reg8, [pSrc], strideMult4 - LDR reg11, [pCoeff, coeffStrideMult4] - LSL reg9, reg9, #1 - PKHTB reg7, reg9, reg10, ASR #15 - SMUSD reg10, reg11, reg8 - SMUADX reg8, reg11, reg8 - LSL reg8, reg8, #1 - PKHTB reg9, reg8, reg10, ASR #15 - SHADD16 reg10, reg5, reg7 - SHSUB16 reg11, reg5, reg7 - SHADD16 reg8, reg6, reg9 - SHSUB16 reg9, reg6, reg9 - SHADD16 reg5, reg10, reg8 - SHSUB16 reg7, reg10, reg8 - SHASX reg6, reg11, reg9 - SHSAX reg8, reg11, reg9 - STR reg5, [pDst], strideMult4 - STR reg6, [pDst], strideMult4 - STR reg7, [pDst], strideMult4 - STR reg8, [pDst], strideMult4 - } -#else - asm volatile( - "LDR r11, [%[pCoeff], %[coeffStrideDw]]! \n\t" - "LDR r5, [%[pSrc]], %[strideDw] \n\t" - "LDR r7, [%[pSrc]], %[strideDw] \n\t" - "LDR r6, [%[pSrc]], %[strideDw] \n\t" - "SMUSD r10, r11, r6 \n\t" - "SMUADX r9, r11, r6 \n\t" - "LDR r11, [%[pCoeff], %[coeffStrideDw]]! \n\t" - "LSL r9, r9, #1 \n\t" - "PKHTB r6, r9, r10, ASR #15 \n\t" - "SMUSD r10, r11, r7 \n\t" - "SMUADX r9, r11, r7 \n\t" - "LDR r8, [%[pSrc]], %[strideDw] \n\t" - "LDR r11, [%[pCoeff], %[coeffStrideDw]] \n\t" - "LSL r9, r9, #1 \n\t" - "PKHTB r7, r9, r10, ASR #15 \n\t" - "SMUSD r10, r11, r8 \n\t" - "SMUADX r8, r11, r8 \n\t" - "LSL r8, r8, #1 \n\t" - "PKHTB r9, r8, r10, ASR #15 \n\t" - "SHADD16 r10, r5, r7 \n\t" - "SHSUB16 r11, r5, r7 \n\t" - "SHADD16 r8, r6, r9 \n\t" - "SHSUB16 r9, r6, r9 \n\t" - "SHADD16 r5, r10, r8 \n\t" - "SHSUB16 r7, r10, r8 \n\t" - "SHASX r6, r11, r9 \n\t" - "SHSAX r8, r11, r9 \n\t" - "STR r5, [%[pDst]], %[strideDw] \n\t" - "STR r6, [%[pDst]], %[strideDw] \n\t" - "STR r7, [%[pDst]], %[strideDw] \n\t" - "STR r8, [%[pDst]], %[strideDw] \n\t" - : [pSrc]"+r"(pSrc), [pDst]"+r"(pDst) : [pCoeff]"r"(pCoeff), [strideDw]"r"(4*stride), [coeffStrideDw]"r"(4*coeffStride) - : "r5", "r6", "r7", "r8", "r9", "r10","r11"); -/* - "SMULBT r8, r6, r11 \n\t" - "SMULTB r9, r6, r11 \n\t" - "QSUB r9, r9, r8 \n\t" -*/ -#endif -#else - INT16 x1I, x1Q, x2I, x2Q, x3I, x3Q; // x0I, x0Q, - INT16 z0I, z0Q, z1I, z1Q, z2I, z2Q, z3I, z3Q; - INT16 y0I, y0Q, y1I, y1Q, y2I, y2Q, y3I, y3Q; - INT16 AI, AQ, BI, BQ, CI, CQ, DI, DQ; - INT16 W1I, W1Q, W2I, W2Q, W3I, W3Q; // W0I, W0Q, - - unsigned int *loadPtr = (unsigned int *)pSrc; - unsigned int loadTemp; - unsigned int *coeffPtr = (unsigned int *)(pCoeff +2*coeffStride); - unsigned int loadCoeff; - unsigned int *storePtr = (unsigned int *)pDst; - unsigned int storeTemp; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - loadTemp = *loadPtr; loadPtr +=stride; - z0I = (loadTemp&0xffff); z0Q = (loadTemp>>16); - loadTemp = *loadPtr; loadPtr +=stride; - x2I = (loadTemp&0xffff); x2Q = (loadTemp>>16); - loadTemp = *loadPtr; loadPtr +=stride; - x1I = (loadTemp&0xffff); x1Q = (loadTemp>>16); - loadTemp = *loadPtr; loadPtr +=stride; - x3I = (loadTemp&0xffff); x3Q = (loadTemp>>16); - - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W1I = (loadCoeff&0xffff); W1Q = (loadCoeff>>16); - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W2I = (loadCoeff&0xffff); W2Q = (loadCoeff>>16); - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W3I = (loadCoeff&0xffff); W3Q = (loadCoeff>>16); - -#ifdef ARM_DEBUG - z1I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W1I, x1I), radix4Fft16BitTo32BitMultiplier(W1Q, x1Q))); // r6 - z1Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W1I, x1Q), radix4Fft16BitTo32BitMultiplier(W1Q, x1I))); - z2I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W2I, x2I), radix4Fft16BitTo32BitMultiplier(W2Q, x2Q))); // r7 - z2Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W2I, x2Q), radix4Fft16BitTo32BitMultiplier(W2Q, x2I))); - z3I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W3I, x3I), radix4Fft16BitTo32BitMultiplier(W3Q, x3Q))); // r9 - z3Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W3I, x3Q), radix4Fft16BitTo32BitMultiplier(W3Q, x3I))); -#else - z1I = (INT16)( (((INT32)W1I*x1I)-((INT32)W1Q*x1Q))>>15 ); - z1Q = (INT16)( (((INT32)W1I*x1Q)+((INT32)W1Q*x1I))>>15 ); - z2I = (INT16)( (((INT32)W2I*x2I)-((INT32)W2Q*x2Q))>>15 ); - z2Q = (INT16)( (((INT32)W2I*x2Q)+((INT32)W2Q*x2I))>>15 ); - z3I = (INT16)( (((INT32)W3I*x3I)-((INT32)W3Q*x3Q))>>15 ); - z3Q = (INT16)( (((INT32)W3I*x3Q)+((INT32)W3Q*x3I))>>15 ); -#endif - // calculate using A = (z0+z2), B = (z0-z2), C = (z1+z3), D = (z1-z3); - // y0 = z0 + z1 + z2 + z3 = A + C - // y1 = z0 -jz1 - z2 +jz3 = B -jD - // y2 = z0 - z1 + z2 - z3 = A - C - // y3 = z0 +jz1 - z2 -jz3 = B +jD -#ifdef ARM_DEBUG - AI = radix4Fft16BitAddShift(z0I, z2I); AQ = radix4Fft16BitAddShift(z0Q, z2Q); - BI = radix4Fft16BitSubtractShift(z0I, z2I); BQ = radix4Fft16BitSubtractShift(z0Q, z2Q); - CI = radix4Fft16BitAddShift(z1I, z3I); CQ = radix4Fft16BitAddShift(z1Q, z3Q); - DI = radix4Fft16BitSubtractShift(z1I, z3I); DQ = radix4Fft16BitSubtractShift(z1Q, z3Q); - - y0I = radix4Fft16BitAddShift(AI, CI); y0Q = radix4Fft16BitAddShift(AQ, CQ); - y1I = radix4Fft16BitSubtractShift(BI, DQ); y1Q = radix4Fft16BitAddShift(BQ, DI); - y2I = radix4Fft16BitSubtractShift(AI, CI); y2Q = radix4Fft16BitSubtractShift(AQ, CQ); - y3I = radix4Fft16BitAddShift(BI, DQ); y3Q = radix4Fft16BitSubtractShift(BQ, DI); -#else - AI = (z0I+z2I)>>1; AQ = (z0Q+z2Q)>>1; - BI = (z0I-z2I)>>1; BQ = (z0Q-z2Q)>>1; - CI = (z1I+z3I)>>1; CQ = (z1Q+z3Q)>>1; - DI = (z1I-z3I)>>1; DQ = (z1Q-z3Q)>>1; - - y0I = (AI+CI)>>1; y0Q = (AQ+CQ)>>1; - y1I = (BI-DQ)>>1; y1Q = (BQ+DI)>>1; - y2I = (AI-CI)>>1; y2Q = (AQ-CQ)>>1; - y3I = (BI+DQ)>>1; y3Q = (BQ-DI)>>1; -#endif - storeTemp = (unsigned short)y0Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y0I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y1Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y1I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y2Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y2I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y3Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y3I; - *storePtr = storeTemp; storePtr+=stride; -#endif -} - -#ifdef FFT_PARALLEL -void radix4IfftRadix4ButterflyTwiddleParallel(INT16* pSrc, INT16* pDst, int stride, const INT16* pCoeff, int coeffStride) { - - int qq; - INT16 x1I[NUM_PARALLEL], x1Q[NUM_PARALLEL], x2I[NUM_PARALLEL], x2Q[NUM_PARALLEL], x3I[NUM_PARALLEL], x3Q[NUM_PARALLEL]; // x0I, x0Q, - INT16 z0I[NUM_PARALLEL], z0Q[NUM_PARALLEL], z1I[NUM_PARALLEL], z1Q[NUM_PARALLEL], z2I[NUM_PARALLEL], z2Q[NUM_PARALLEL], z3I[NUM_PARALLEL], z3Q[NUM_PARALLEL]; - INT16 y0I[NUM_PARALLEL], y0Q[NUM_PARALLEL], y1I[NUM_PARALLEL], y1Q[NUM_PARALLEL], y2I[NUM_PARALLEL], y2Q[NUM_PARALLEL], y3I[NUM_PARALLEL], y3Q[NUM_PARALLEL]; - INT16 AI[NUM_PARALLEL], AQ[NUM_PARALLEL], BI[NUM_PARALLEL], BQ[NUM_PARALLEL], CI[NUM_PARALLEL], CQ[NUM_PARALLEL], DI[NUM_PARALLEL], DQ[NUM_PARALLEL]; - INT16 W1I, W1Q, W2I, W2Q, W3I, W3Q; // W0I, W0Q, - - unsigned int *loadPtr = (unsigned int *)pSrc; - unsigned int loadTemp; - unsigned int *coeffPtr = (unsigned int *)(pCoeff + 2 * coeffStride); - unsigned int loadCoeff; - unsigned int *storePtr = (unsigned int *)pDst; - unsigned int storeTemp; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - for (qq = 0;qq> 16); - } - loadPtr += stride; - for (qq = 0;qq> 16); - } - loadPtr += stride; - for (qq = 0;qq> 16); - } - loadPtr += stride; - for (qq = 0;qq> 16); - } - - loadCoeff = *coeffPtr; coeffPtr += coeffStride; - W1I = (loadCoeff & 0xffff); W1Q = (loadCoeff >> 16); - loadCoeff = *coeffPtr; coeffPtr += coeffStride; - W2I = (loadCoeff & 0xffff); W2Q = (loadCoeff >> 16); - loadCoeff = *coeffPtr; coeffPtr += coeffStride; - W3I = (loadCoeff & 0xffff); W3Q = (loadCoeff >> 16); - - for (qq = 0;qq> 15); - z1Q[qq] = (INT16)((((INT32)W1I*x1Q[qq]) + ((INT32)W1Q*x1I[qq])) >> 15); - z2I[qq] = (INT16)((((INT32)W2I*x2I[qq]) - ((INT32)W2Q*x2Q[qq])) >> 15); - z2Q[qq] = (INT16)((((INT32)W2I*x2Q[qq]) + ((INT32)W2Q*x2I[qq])) >> 15); - z3I[qq] = (INT16)((((INT32)W3I*x3I[qq]) - ((INT32)W3Q*x3Q[qq])) >> 15); - z3Q[qq] = (INT16)((((INT32)W3I*x3Q[qq]) + ((INT32)W3Q*x3I[qq])) >> 15); - } - - // calculate using A = (z0+z2), B = (z0-z2), C = (z1+z3), D = (z1-z3); - // y0 = z0 + z1 + z2 + z3 = A + C - // y1 = z0 -jz1 - z2 +jz3 = B -jD - // y2 = z0 - z1 + z2 - z3 = A - C - // y3 = z0 +jz1 - z2 -jz3 = B +jD - for (qq = 0;qq> 1; AQ[qq] = (z0Q[qq] + z2Q[qq]) >> 1; - BI[qq] = (z0I[qq] - z2I[qq]) >> 1; BQ[qq] = (z0Q[qq] - z2Q[qq]) >> 1; - CI[qq] = (z1I[qq] + z3I[qq]) >> 1; CQ[qq] = (z1Q[qq] + z3Q[qq]) >> 1; - DI[qq] = (z1I[qq] - z3I[qq]) >> 1; DQ[qq] = (z1Q[qq] - z3Q[qq]) >> 1; - } - - for (qq = 0;qq> 1; y0Q[qq] = (AQ[qq] + CQ[qq]) >> 1; - y1I[qq] = (BI[qq] - DQ[qq]) >> 1; y1Q[qq] = (BQ[qq] + DI[qq]) >> 1; - y2I[qq] = (AI[qq] - CI[qq]) >> 1; y2Q[qq] = (AQ[qq] - CQ[qq]) >> 1; - y3I[qq] = (BI[qq] + DQ[qq]) >> 1; y3Q[qq] = (BQ[qq] - DI[qq]) >> 1; - } - - for (qq = 0;qq>16); - loadTemp = *loadPtr; loadPtr +=stride; - x1I = (loadTemp&0xffff); x1Q = (loadTemp>>16); - - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W1I = (loadCoeff&0xffff); W1Q = (loadCoeff>>16); -#ifdef ARM_DEBUG - z1I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W1I, x1I), radix4Fft16BitTo32BitMultiplier(W1Q, x1Q))); // r6 - z1Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W1I, x1Q), radix4Fft16BitTo32BitMultiplier(W1Q, x1I))); - y0I = radix4Fft16BitAdd(z0I, z1I); y0Q = radix4Fft16BitAdd(z0Q, z1Q); - y1I = radix4Fft16BitSubtract(z0I, z1I); y1Q = radix4Fft16BitSubtract(z0Q, z1Q); -#else - z1I = (INT16)( (((INT32)W1I*x1I)+((INT32)W1Q*x1Q))>>15 ); - z1Q = (INT16)( (((INT32)W1I*x1Q)-((INT32)W1Q*x1I))>>15 ); - y0I = (z0I+z1I); y0Q = (z0Q+z1Q); - y1I = (z0I-z1I); y1Q = (z0Q-z1Q); -#endif - storeTemp = (unsigned short)y0Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y0I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y1Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y1I; - *storePtr = storeTemp; storePtr+=stride; -#endif -} - -void radix4IfftRadix2ButterflyTwiddle(INT16* pSrc, INT16* pDst, int stride, const INT16* pCoeff, int coeffStride){ -#if defined(ARM_GCC) || defined(ARM_DS5) -#ifdef ARM_DS5 - int reg5, reg6, reg7, reg8, reg9; - __asm - { - LDR reg7, [pCoeff, (4*coeffStride)] - LDR reg5, [pSrc] - LDR reg6, [pSrc, (4*stride)] - SMUSD reg8, reg7, reg6 - SMUADX reg9, reg7, reg6 - LSL reg7, reg9, #1 - PKHTB reg6, reg7, reg8, ASR #15 - SHADD16 reg7, reg5, reg6 - SHSUB16 reg8, reg5, reg6 - STR reg7, [pDst] - STR reg8, [pDst, (4*stride)] - } - -#else - asm( - "LDR r7, [%[pCoeff], %[coeffStrideDw]] \n\t" - "LDR r5, [%[pSrc]] \n\t" - "LDR r6, [%[pSrc], %[strideDw]] \n\t" - "SMUSD r8, r7, r6 \n\t" - "SMUADX r9, r7, r6 \n\t" - "LSL r7, r9, #1 \n\t" - "PKHTB r6, r7, r8, ASR #15 \n\t" - "SHADD16 r7, r5, r6 \n\t" - "SHSUB16 r8, r5, r6 \n\t" - "STR r7, [%[pDst]] \n\t" - "STR r8, [%[pDst], %[strideDw]] \n\t" - :: [pSrc]"r"(pSrc), [pDst]"r"(pDst), [pCoeff]"r"(pCoeff), [strideDw]"r"(4*stride), [coeffStrideDw]"r"(4*coeffStride) - : "r5", "r6", "r7", "r8", "r9"); -#endif -#else - INT16 x1I, x1Q; // x0I, x0Q, - INT16 z0I, z0Q, z1I, z1Q; - INT16 y0I, y0Q, y1I, y1Q; - INT16 W1I, W1Q; // W0I, W0Q, - - unsigned int *loadPtr = (unsigned int *)pSrc; - unsigned int loadTemp; - unsigned int *coeffPtr = (unsigned int *)(pCoeff +2*coeffStride); - unsigned int loadCoeff; - unsigned int *storePtr = (unsigned int *)pDst; - unsigned int storeTemp; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - loadTemp = *loadPtr; loadPtr +=stride; - z0I = (loadTemp&0xffff); z0Q = (loadTemp>>16); - loadTemp = *loadPtr; loadPtr +=stride; - x1I = (loadTemp&0xffff); x1Q = (loadTemp>>16); - - loadCoeff = *coeffPtr; coeffPtr +=coeffStride; - W1I = (loadCoeff&0xffff); W1Q = (loadCoeff>>16); -#ifdef ARM_DEBUG - z1I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W1I, x1I), radix4Fft16BitTo32BitMultiplier(W1Q, x1Q))); // r6 - z1Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W1I, x1Q), radix4Fft16BitTo32BitMultiplier(W1Q, x1I))); - - y0I = radix4Fft16BitAddShift(z0I, z1I); y0Q = radix4Fft16BitAddShift(z0Q, z1Q); - y1I = radix4Fft16BitSubtractShift(z0I, z1I); y1Q = radix4Fft16BitSubtractShift(z0Q, z1Q); -#else - z1I = ( (((INT32)W1I*x1I)-((INT32)W1Q*x1Q))>>15 ); - z1Q = ( (((INT32)W1I*x1Q)+((INT32)W1Q*x1I))>>15 ); - - y0I = (z0I+z1I)>>1; y0Q = (z0Q+z1Q)>>1; - y1I = (z0I-z1I)>>1; y1Q = (z0Q-z1Q)>>1; - -#endif - storeTemp = (unsigned short)y0Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y0I; - *storePtr = storeTemp; storePtr+=stride; - storeTemp = (unsigned short)y1Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y1I; - *storePtr = storeTemp; storePtr+=stride; -#endif -} - -#ifdef FFT_PARALLEL -void radix4IfftRadix2ButterflyTwiddleParallel(INT16* pSrc, INT16* pDst, int stride, const INT16* pCoeff, int coeffStride) { - - int qq; - INT16 x1I[NUM_PARALLEL], x1Q[NUM_PARALLEL]; // x0I, x0Q, - INT16 z0I[NUM_PARALLEL], z0Q[NUM_PARALLEL], z1I[NUM_PARALLEL], z1Q[NUM_PARALLEL]; - INT16 y0I[NUM_PARALLEL], y0Q[NUM_PARALLEL], y1I[NUM_PARALLEL], y1Q[NUM_PARALLEL]; - INT16 W1I, W1Q; // W0I, W0Q, - - unsigned int *loadPtr = (unsigned int *)pSrc; - unsigned int loadTemp; - unsigned int *coeffPtr = (unsigned int *)(pCoeff + 2 * coeffStride); - unsigned int loadCoeff; - unsigned int *storePtr = (unsigned int *)pDst; - unsigned int storeTemp; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - for (qq = 0;qq> 16); - } - loadPtr += stride; - for (qq = 0;qq> 16); - } - loadPtr += stride; - - loadCoeff = *coeffPtr; coeffPtr += coeffStride; - W1I = (loadCoeff & 0xffff); W1Q = (loadCoeff >> 16); - - for (qq = 0;qq> 15); - z1Q[qq] = ((((INT32)W1I*x1Q[qq]) + ((INT32)W1Q*x1I[qq])) >> 15); - } - for (qq = 0;qq> 1; y0Q[qq] = (z0Q[qq] + z1Q[qq]) >> 1; - y1I[qq] = (z0I[qq] - z1I[qq]) >> 1; y1Q[qq] = (z0Q[qq] - z1Q[qq]) >> 1; - } - - for (qq = 0;qq>2; - for(nn=loopLen;nn>0;nn--){ - // first stage does not need saturation -#ifdef ARM_DS5 - __asm volatile - { - RBIT reg4, index1 - LDR reg5, [pSrc, reg4] - ADD index1, index1, revBitInc - RBIT reg4, index1 - LDR reg6, [pSrc, reg4] - ADD index1, index1, revBitInc - RBIT reg4, index1 - LDR reg7, [pSrc, reg4] - ADD index1, index1, revBitInc - RBIT reg4, index1 - LDR reg8, [pSrc, reg4] - ADD index1, index1, revBitInc - SADD16 reg9, reg5, reg6 - SSUB16 reg10, reg5, reg6 - SADD16 reg4, reg7, reg8 - SSUB16 reg5, reg7, reg8 - SADD16 reg6, reg9, reg4 - SSUB16 reg8, reg9, reg4 - SSAX reg7, reg10, reg5 - SASX reg9, reg10, reg5 - STR reg6, [pDst], #4 - STR reg7, [pDst], #4 - STR reg8, [pDst], #4 - STR reg9, [pDst], #4 - } -/// STM pDst!, {reg6-reg9} -#else - asm volatile ( - "RBIT r4, %[index1] \n\t" - "LDR r5, [%[pSrc], r4] \n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "RBIT r4, %[index1] \n\t" - "LDR r6, [%[pSrc], r4]\n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "RBIT r4, %[index1] \n\t" - "LDR r7, [%[pSrc], r4] \n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "RBIT r4, %[index1] \n\t" - "LDR r8, [%[pSrc], r4] \n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "SADD16 r9, r5, r6 \n\t" - "SSUB16 r10, r5, r6 \n\t" - "SADD16 r4, r7, r8 \n\t" - "SSUB16 r5, r7, r8 \n\t" - "SADD16 r6, r9, r4 \n\t" - "SSUB16 r8, r9, r4 \n\t" - "SSAX r7, r10, r5 \n\t" - "SASX r9, r10, r5 \n\t" - "STM %[pDst]!, {r6-r9} \n\t" - : [pDst]"+r"(pDst), [index1]"+r"(index1) : [pSrc]"r"(pSrc), [revBitInc]"r"(revBitInc) - : "r4", "r5", "r6", "r7", "r8", "r9","r10","memory"); -#endif - } -} -/* - "STRD r6, r7, [%[pDst]], #8 \n\t" - "STRD r8, r9, [%[pDst]], #8 \n\t" -*/ -void radix4IfftStageOne(INT16* pSrc, INT16* pDst, int Nfft){ - -#ifdef ARM_DS5 - int reg4, reg5, reg6, reg7, reg8, reg9, reg10; -#endif - int nn; - int loopLen; - unsigned int index1 = 0; // bit-reversal permutation - int revBitInc = 1<<(1+__clz(Nfft)-2); // -2 for 4 byte LD - - loopLen = Nfft>>2; - for(nn=loopLen;nn>0;nn--){ - // first stage does not need saturation -#ifdef ARM_DS5 - __asm volatile - { - RBIT reg4, index1 - LDR reg5, [pSrc, reg4] - ADD index1, index1, revBitInc - RBIT reg4, index1 - LDR reg6, [pSrc, reg4] - ADD index1, index1, revBitInc - RBIT reg4, index1 - LDR reg7, [pSrc, reg4] - ADD index1, index1, revBitInc - RBIT reg4, index1 - LDR reg8, [pSrc, reg4] - ADD index1, index1, revBitInc - SHADD16 reg9, reg5, reg6 - SHSUB16 reg10, reg5, reg6 - SHADD16 reg4, reg7, reg8 - SHSUB16 reg5, reg7, reg8 - SHADD16 reg6, reg9, reg4 - SHSUB16 reg8, reg9, reg4 - SHASX reg7, reg10, reg5 - SHSAX reg9, reg10, reg5 - STR reg6, [pDst], #4 - STR reg7, [pDst], #4 - STR reg8, [pDst], #4 - STR reg9, [pDst], #4 - } -// STM pDst!, {reg6, reg7, reg8, reg9} -#else - asm volatile ( - "RBIT r4, %[index1] \n\t" - "LDR r5, [%[pSrc], r4] \n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "RBIT r4, %[index1] \n\t" - "LDR r6, [%[pSrc], r4]\n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "RBIT r4, %[index1] \n\t" - "LDR r7, [%[pSrc], r4] \n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "RBIT r4, %[index1] \n\t" - "LDR r8, [%[pSrc], r4] \n\t" - "ADD %[index1], %[index1], %[revBitInc] \n\t" - "SHADD16 r9, r5, r6 \n\t" - "SHSUB16 r10, r5, r6 \n\t" - "SHADD16 r4, r7, r8 \n\t" - "SHSUB16 r5, r7, r8 \n\t" - "SHADD16 r6, r9, r4 \n\t" - "SHSUB16 r8, r9, r4 \n\t" - "SHASX r7, r10, r5 \n\t" - "SHSAX r9, r10, r5 \n\t" - "STM %[pDst]!, {r6-r9} \n\t" - : [pDst]"+r"(pDst), [index1]"+r"(index1) : [pSrc]"r"(pSrc), [revBitInc]"r"(revBitInc) - : "r4", "r5", "r6", "r7", "r8", "r9","r10","memory"); -#endif - } -} -#else -void radix4FftRadix4ButterflyBrev(INT16* pSrc, INT16* pDst, unsigned int index1, int log2Nfft){ - INT16 x0I, x0Q, x1I, x1Q, x2I, x2Q, x3I, x3Q; - INT16 y0I, y0Q, y1I, y1Q, y2I, y2Q, y3I, y3Q; - INT16 AI, AQ, BI, BQ, CI, CQ, DI, DQ; - unsigned int index2; - unsigned int *loadPtr = (unsigned int*)pSrc; - unsigned int *storePtr = (unsigned int*)pDst; - unsigned int loadTemp, storeTemp; - int bitShift = IDX_LEN-log2Nfft; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x0I = (loadTemp&0xffff); x0Q = (loadTemp>>16); - //x0I = pSrc[2*index2]; x0Q = pSrc[2*index2+1]; // r5 - - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x2I = (loadTemp&0xffff); x2Q = (loadTemp>>16); - //x2I = pSrc[2*index2]; x2Q = pSrc[2*index2+1]; // r6 - - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x1I = (loadTemp&0xffff); x1Q = (loadTemp>>16); - //x1I = pSrc[2*index2]; x1Q = pSrc[2*index2+1]; // r7 - - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x3I = (loadTemp&0xffff); x3Q = (loadTemp>>16); - //x3I = pSrc[2*index2]; x3Q = pSrc[2*index2+1]; // r8 - -#ifdef ARM_DEBUG - AI = radix4Fft16BitAdd(x0I, x2I); AQ = radix4Fft16BitAdd(x0Q, x2Q); // r4 - BI = radix4Fft16BitSubtract(x0I, x2I); BQ = radix4Fft16BitSubtract(x0Q, x2Q); // r5 - CI = radix4Fft16BitAdd(x1I, x3I); CQ = radix4Fft16BitAdd(x1Q, x3Q); // r6 - DI = radix4Fft16BitSubtract(x1I, x3I); DQ = radix4Fft16BitSubtract(x1Q, x3Q); // r7 - - y0I = radix4Fft16BitAdd(AI, CI); y0Q = radix4Fft16BitAdd(AQ, CQ); // r8 - y2I = radix4Fft16BitSubtract(AI, CI); y2Q = radix4Fft16BitSubtract(AQ, CQ); // r4 - y3I = radix4Fft16BitSubtract(BI, DQ); y3Q = radix4Fft16BitAdd(BQ, DI); // r6 - y1I = radix4Fft16BitAdd(BI, DQ); y1Q = radix4Fft16BitSubtract(BQ, DI); // r5 -#else - AI = x0I+x2I; AQ = x0Q+x2Q; - BI = x0I-x2I; BQ = x0Q-x2Q; - CI = x1I+x3I; CQ = x1Q+x3Q; - DI = x1I-x3I; DQ = x1Q-x3Q; - - y0I = AI+CI; y0Q = AQ+CQ; - y2I = AI-CI; y2Q = AQ-CQ; - y3I = BI-DQ; y3Q = BQ+DI; - y1I = BI+DQ; y1Q = BQ-DI; -#endif - - storeTemp = (unsigned short)y0Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y0I; - *storePtr++ = storeTemp; - storeTemp = (unsigned short)y1Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y1I; - *storePtr++ = storeTemp; - storeTemp = (unsigned short)y2Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y2I; - *storePtr++ = storeTemp; - storeTemp = (unsigned short)y3Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y3I; - *storePtr++ = storeTemp; -} - -void radix4IfftRadix4ButterflyBrev(INT16* pSrc, INT16* pDst, unsigned int index1, int log2Nfft){ - INT16 x0I, x0Q, x1I, x1Q, x2I, x2Q, x3I, x3Q; - INT16 y0I, y0Q, y1I, y1Q, y2I, y2Q, y3I, y3Q; - INT16 AI, AQ, BI, BQ, CI, CQ, DI, DQ; - - unsigned int index2; - unsigned int *loadPtr = (unsigned int*)pSrc; - unsigned int *storePtr = (unsigned int*)pDst; - unsigned int loadTemp, storeTemp; - int bitShift = IDX_LEN-log2Nfft; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x0I = (loadTemp&0xffff); x0Q = (loadTemp>>16); - //x0I = pSrc[2*index2]; x0Q = pSrc[2*index2+1]; // r5 - - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x2I = (loadTemp&0xffff); x2Q = (loadTemp>>16); - //x2I = pSrc[2*index2]; x2Q = pSrc[2*index2+1]; // r6 - - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x1I = (loadTemp&0xffff); x1Q = (loadTemp>>16); - //x1I = pSrc[2*index2]; x1Q = pSrc[2*index2+1]; // r7 - - index2 = REV_IDX(index1++); - index2>>=bitShift; - loadTemp = loadPtr[index2]; - x3I = (loadTemp&0xffff); x3Q = (loadTemp>>16); - //x3I = pSrc[2*index2]; x3Q = pSrc[2*index2+1]; // r8 -#ifdef ARM_DEBUG - AI = radix4Fft16BitAddShift(x0I, x2I); AQ = radix4Fft16BitAddShift(x0Q, x2Q); // r4 - BI = radix4Fft16BitSubtractShift(x0I, x2I); BQ = radix4Fft16BitSubtractShift(x0Q, x2Q); // r5 - CI = radix4Fft16BitAddShift(x1I, x3I); CQ = radix4Fft16BitAddShift(x1Q, x3Q); // r6 - DI = radix4Fft16BitSubtractShift(x1I, x3I); DQ = radix4Fft16BitSubtractShift(x1Q, x3Q); // r7 - y0I = radix4Fft16BitAddShift(AI, CI); y0Q = radix4Fft16BitAddShift(AQ, CQ); // r8 - y2I = radix4Fft16BitSubtractShift(AI, CI); y2Q = radix4Fft16BitSubtractShift(AQ, CQ); // r4 - y3I = radix4Fft16BitAddShift(BI, DQ); y3Q = radix4Fft16BitSubtractShift(BQ, DI); // r6 - y1I = radix4Fft16BitSubtractShift(BI, DQ); y1Q = radix4Fft16BitAddShift(BQ, DI); // r5 -#else - AI = (x0I+x2I)>>1; AQ = (x0Q+x2Q)>>1; - BI = (x0I-x2I)>>1; BQ = (x0Q-x2Q)>>1; - CI = (x1I+x3I)>>1; CQ = (x1Q+x3Q)>>1; - DI = (x1I-x3I)>>1; DQ = (x1Q-x3Q)>>1; - y0I = (AI+CI)>>1; y0Q = (AQ+CQ)>>1; - y2I = (AI-CI)>>1; y2Q = (AQ-CQ)>>1; - y3I = (BI+DQ)>>1; y3Q = (BQ-DI)>>1; - y1I = (BI-DQ)>>1; y1Q = (BQ+DI)>>1; -#endif - - storeTemp = (unsigned short)y0Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y0I; - *storePtr++ = storeTemp; - storeTemp = (unsigned short)y1Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y1I; - *storePtr++ = storeTemp; - storeTemp = (unsigned short)y2Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y2I; - *storePtr++ = storeTemp; - storeTemp = (unsigned short)y3Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y3I; - *storePtr++ = storeTemp; -} - -void radix4FftStageOne(INT16* pSrc, INT16* pDst, int Nfft){ - - int nn; - int loopLen; - INT16 *pSrcLoop, *pDstLoop; - // bit-reversal permutation - unsigned int index1; - - int log2Nfft = myMsb(Nfft); - - loopLen = Nfft>>2; - pSrcLoop = pSrc; - pDstLoop = pDst; - index1 = 0; - for(nn=0;nn>2; - pSrcLoop = pSrc; - pDstLoop = pDst; - index1 = 0; - for(nn=0;nn>= bitShift; - loadTemp = loadPtr[NUM_PARALLEL * index2]; - x0I = (loadTemp & 0xffff); x0Q = (loadTemp >> 16); - - index2 = REV_IDX(index1++); - index2 >>= bitShift; - loadTemp = loadPtr[NUM_PARALLEL * index2]; - x2I = (loadTemp & 0xffff); x2Q = (loadTemp >> 16); - - index2 = REV_IDX(index1++); - index2 >>= bitShift; - loadTemp = loadPtr[NUM_PARALLEL * index2]; - x1I = (loadTemp & 0xffff); x1Q = (loadTemp >> 16); - - index2 = REV_IDX(index1++); - index2 >>= bitShift; - loadTemp = loadPtr[NUM_PARALLEL * index2]; - x3I = (loadTemp & 0xffff); x3Q = (loadTemp >> 16); - - AI = (x0I + x2I) >> 1; AQ = (x0Q + x2Q) >> 1; - BI = (x0I - x2I) >> 1; BQ = (x0Q - x2Q) >> 1; - CI = (x1I + x3I) >> 1; CQ = (x1Q + x3Q) >> 1; - DI = (x1I - x3I) >> 1; DQ = (x1Q - x3Q) >> 1; - y0I = (AI + CI) >> 1; y0Q = (AQ + CQ) >> 1; - y2I = (AI - CI) >> 1; y2Q = (AQ - CQ) >> 1; - y3I = (BI + DQ) >> 1; y3Q = (BQ - DI) >> 1; - y1I = (BI - DQ) >> 1; y1Q = (BQ + DI) >> 1; - - storeTemp = (unsigned short)y0Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y0I; - *storePtr = storeTemp; storePtr+=NUM_PARALLEL; - storeTemp = (unsigned short)y1Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y1I; - *storePtr = storeTemp; storePtr+= NUM_PARALLEL; - storeTemp = (unsigned short)y2Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y2I; - *storePtr = storeTemp; storePtr+= NUM_PARALLEL; - storeTemp = (unsigned short)y3Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y3I; - *storePtr = storeTemp; storePtr+= NUM_PARALLEL; -} -#endif - -#ifdef FFT_PARALLEL -void radix4IfftRadix4ButterflyBrevParallel(INT16* pSrc, INT16* pDst, unsigned int index1, int log2Nfft) { - - int qq; - INT16 x0I[NUM_PARALLEL], x0Q[NUM_PARALLEL], x1I[NUM_PARALLEL], x1Q[NUM_PARALLEL], x2I[NUM_PARALLEL], x2Q[NUM_PARALLEL], x3I[NUM_PARALLEL], x3Q[NUM_PARALLEL]; - INT16 y0I[NUM_PARALLEL], y0Q[NUM_PARALLEL], y1I[NUM_PARALLEL], y1Q[NUM_PARALLEL], y2I[NUM_PARALLEL], y2Q[NUM_PARALLEL], y3I[NUM_PARALLEL], y3Q[NUM_PARALLEL]; - INT16 AI[NUM_PARALLEL], AQ[NUM_PARALLEL], BI[NUM_PARALLEL], BQ[NUM_PARALLEL], CI[NUM_PARALLEL], CQ[NUM_PARALLEL], DI[NUM_PARALLEL], DQ[NUM_PARALLEL]; - - unsigned int index2; - unsigned int *loadPtr = (unsigned int*)pSrc; - unsigned int *storePtr = (unsigned int*)pDst; - unsigned int loadTemp, storeTemp; - int bitShift = 32 - log2Nfft; - - // re-order due to L^4_2 - because of using radix-2 bit-reversal - index2 = REV_IDX(index1++); - index2 >>= bitShift; - for (qq = 0;qq> 16); - } - index2 = REV_IDX(index1++); - index2 >>= bitShift; - for (qq = 0;qq> 16); - } - index2 = REV_IDX(index1++); - index2 >>= bitShift; - for (qq = 0;qq> 16); - } - index2 = REV_IDX(index1++); - index2 >>= bitShift; - for (qq = 0;qq> 16); - } - - for (qq = 0;qq> 1; AQ[qq] = (x0Q[qq] + x2Q[qq]) >> 1; - BI[qq] = (x0I[qq] - x2I[qq]) >> 1; BQ[qq] = (x0Q[qq] - x2Q[qq]) >> 1; - CI[qq] = (x1I[qq] + x3I[qq]) >> 1; CQ[qq] = (x1Q[qq] + x3Q[qq]) >> 1; - DI[qq] = (x1I[qq] - x3I[qq]) >> 1; DQ[qq] = (x1Q[qq] - x3Q[qq]) >> 1; - } - for (qq = 0;qq> 1; y0Q[qq] = (AQ[qq] + CQ[qq]) >> 1; - y2I[qq] = (AI[qq] - CI[qq]) >> 1; y2Q[qq] = (AQ[qq] - CQ[qq]) >> 1; - y3I[qq] = (BI[qq] + DQ[qq]) >> 1; y3Q[qq] = (BQ[qq] - DI[qq]) >> 1; - y1I[qq] = (BI[qq] - DQ[qq]) >> 1; y1Q[qq] = (BQ[qq] + DI[qq]) >> 1; - } - - for (qq = 0;qq> 2; - pSrcLoop = pSrc; - pDstLoop = pDst; - index1 = 0; - for (nn = 0;nn> 2; - pSrcLoop = pSrc; - pDstLoop = pDst; - index1 = 0; - for (nn = 0;nn>1; - int radix2 = log2Nfft&0x1; - - INT16 *pSrcLoop, *pDstLoop; - int coeffStride, coeffStrideTemp; - int ii, mm, nn; - - loop1 = Nfft>>2; - loop2 = 1; - - // next stages - coeffStrideTemp = lenCoeff>>2; - for(ii=1;ii>=2; - loop2 <<=2; - coeffStrideTemp>>=2; - pSrcLoop = pDst; - pDstLoop = pDst; - - coeffStride = 0; - for(mm=0;mm0;nn--){ - radix4FftRadix4ButterflyTwiddle(pSrcLoop, pDstLoop, loop2, pCoeff, coeffStride); - pSrcLoop += 2*4*loop2; - pDstLoop += 2*4*loop2; - } - pSrcLoop += 2 -2*4*loop2*loop1; - pDstLoop += 2 -2*4*loop2*loop1; - coeffStride += coeffStrideTemp; // tabel size is fixed - } - } -} - -void radix4IfftMainStages(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff){ - - int loop1, loop2; -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31-__clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int log4Nfft = log2Nfft>>1; - int radix2 = log2Nfft&0x1; - - INT16 *pSrcLoop, *pDstLoop; - int coeffStride, coeffStrideTemp; - int ii, mm, nn; - - loop1 = Nfft>>2; - loop2 = 1; - - // next stages - coeffStrideTemp = lenCoeff>>2; - for(ii=1;ii>=2; - loop2 <<=2; - coeffStrideTemp>>=2; - pSrcLoop = pDst; - pDstLoop = pDst; - - coeffStride = 0; - for(mm=0;mm0;nn--){ - radix4IfftRadix4ButterflyTwiddle(pSrcLoop, pDstLoop, NUM_PARALLEL*loop2, pCoeff, coeffStride); - pSrcLoop += NUM_PARALLEL * 2*4*loop2; - pDstLoop += NUM_PARALLEL * 2*4*loop2; - } - pSrcLoop += NUM_PARALLEL * (2 -2*4*loop2*loop1); - pDstLoop += NUM_PARALLEL * (2 -2*4*loop2*loop1); - coeffStride += coeffStrideTemp; // tabel size is fixed - } - } -} - -#ifdef FFT_PARALLEL -void radix4IfftMainStagesParallel(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff) { - - int loop1, loop2; -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31 - __clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int log4Nfft = log2Nfft >> 1; - int radix2 = log2Nfft & 0x1; - - INT16 *pSrcLoop, *pDstLoop; - int coeffStride, coeffStrideTemp; - int ii, mm, nn; - - loop1 = Nfft >> 2; - loop2 = 1; - - // next stages - coeffStrideTemp = lenCoeff >> 2; - for (ii = 1;ii>= 2; - loop2 <<= 2; - coeffStrideTemp >>= 2; - pSrcLoop = pDst; - pDstLoop = pDst; - - coeffStride = 0; - for (mm = 0;mm0;nn--) { - radix4IfftRadix4ButterflyTwiddleParallel(pSrcLoop, pDstLoop, NUM_PARALLEL*loop2, pCoeff, coeffStride); - pSrcLoop += NUM_PARALLEL * 2 * 4 * loop2; - pDstLoop += NUM_PARALLEL * 2 * 4 * loop2; - } - pSrcLoop += NUM_PARALLEL * (2 - 2 * 4 * loop2*loop1); - pDstLoop += NUM_PARALLEL * (2 - 2 * 4 * loop2*loop1); - coeffStride += coeffStrideTemp; // tabel size is fixed - } - } -} -#endif - -void myBitRev(INT16* pSrc, int Nfft){ - - int ii, jj; // , mm - unsigned int tempVal; - unsigned int *dataPtr = (unsigned int *)pSrc; -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31-__clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int bitShift = IDX_LEN-log2Nfft; - - //jj = 0; - for(ii=0;ii>bitShift; - if(jj > ii){ - tempVal = dataPtr[jj]; - dataPtr[jj] = dataPtr[ii]; - dataPtr[ii] = tempVal; - } - } -} - -void radix2IfftMainStages(INT16* pBfr, int Nfft, const INT16* pCoeff, int lenCoeff){ - - int loop1, loop2; -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31-__clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - - INT16 *pSrcLoop; - int coeffStride, coeffStrideTemp; - int ii, mm, nn; - - loop1 = Nfft>>1; - loop2 = 1; - - // next stages - coeffStrideTemp = lenCoeff>>1; - for(ii=0;ii0;nn--){ - radix4IfftRadix2ButterflyTwiddle(pSrcLoop, pSrcLoop, loop2, pCoeff, coeffStride); - pSrcLoop += 2*2*loop2; - } - pSrcLoop += 2 -2*2*loop2*loop1; - coeffStride += coeffStrideTemp; // tabel size is fixed - } - loop1 >>=1; - loop2 <<=1; - coeffStrideTemp>>=1; - } -} - -void radix4FftLastStage(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff){ - - int mm, loopLen, coeffStride; -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31-__clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int log4Nfft = log2Nfft>>1; - int radix2 = log2Nfft&0x1; - int coeffStrideTemp = lenCoeff>>(2*log4Nfft+radix2); - INT16 *pSrcLoop = pDst; - INT16 *pDstLoop = pDst; - - if(radix2){ - loopLen = Nfft>>1; - - coeffStride = 0; - for(mm=loopLen;mm>0;mm--){ - radix4FftRadix2ButterflyTwiddle(pSrcLoop, pDstLoop, loopLen, pCoeff, coeffStride); - pSrcLoop += 2; - pDstLoop += 2; - coeffStride += coeffStrideTemp; - } - } - else{ // last iteration for even power of two - loopLen = Nfft>>2; - - coeffStride = 0; - for(mm=loopLen;mm>0;mm--){ - radix4FftRadix4ButterflyTwiddle(pSrcLoop, pDstLoop, loopLen, pCoeff, coeffStride); - pSrcLoop += 2; - pDstLoop += 2; - coeffStride += coeffStrideTemp; - } - } -} - -void radix4IfftLastStage(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff){ - - int mm, loopLen, coeffStride; -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31-__clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int log4Nfft = log2Nfft>>1; - int radix2 = log2Nfft&0x1; - int coeffStrideTemp = lenCoeff>>(2*log4Nfft+radix2); - INT16 *pSrcLoop = pDst; - INT16 *pDstLoop = pDst; - - if(radix2){ - loopLen = Nfft>>1; - - coeffStride = 0; - for(mm=loopLen;mm>0;mm--){ - radix4IfftRadix2ButterflyTwiddle(pSrcLoop, pDstLoop, NUM_PARALLEL*loopLen, pCoeff, coeffStride); - pSrcLoop += 2 * NUM_PARALLEL; - pDstLoop += 2 * NUM_PARALLEL; - coeffStride += coeffStrideTemp; - } - } - else{ // last iteration for even power of two - loopLen = Nfft>>2; - - coeffStride = 0; - for(mm=loopLen;mm>0;mm--){ - radix4IfftRadix4ButterflyTwiddle(pSrcLoop, pDstLoop, NUM_PARALLEL*loopLen, pCoeff, coeffStride); - pSrcLoop += 2 * NUM_PARALLEL; - pDstLoop += 2 * NUM_PARALLEL; - coeffStride += coeffStrideTemp; - } - } -} - -#ifdef FFT_PARALLEL -void radix4IfftLastStageParallel(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff) { - - int mm, loopLen, coeffStride; -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31 - __clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int log4Nfft = log2Nfft >> 1; - int radix2 = log2Nfft & 0x1; - int coeffStrideTemp = lenCoeff >> (2 * log4Nfft + radix2); - INT16 *pSrcLoop, *pDstLoop; - - pSrcLoop = pDst; - pDstLoop = pDst; - - if (radix2) { - loopLen = Nfft >> 1; - - coeffStride = 0; - for (mm = loopLen;mm>0;mm--) { - radix4IfftRadix2ButterflyTwiddleParallel(pSrcLoop, pDstLoop, NUM_PARALLEL*loopLen, pCoeff, coeffStride); - pSrcLoop += 2 * NUM_PARALLEL; - pDstLoop += 2 * NUM_PARALLEL; - coeffStride += coeffStrideTemp; - } - } - else { // last iteration for even power of two - loopLen = Nfft >> 2; - - coeffStride = 0; - for (mm = loopLen;mm>0;mm--) { - radix4IfftRadix4ButterflyTwiddleParallel(pSrcLoop, pDstLoop, NUM_PARALLEL*loopLen, pCoeff, coeffStride); - pSrcLoop += 2 * NUM_PARALLEL; - pDstLoop += 2 * NUM_PARALLEL; - coeffStride += coeffStrideTemp; - } - } -} -#endif - -// only uses two buffers, out-of-place in first stage (includes bit-reveral), then in-place -void radix4Fft(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff){ - - radix4FftStageOne(pSrc, pDst, Nfft); - - radix4FftMainStages(pSrc, pDst, Nfft, pCoeff, lenCoeff); - - radix4FftLastStage(pSrc, pDst, Nfft, pCoeff, lenCoeff); -} - -// same as FFT, inverse sign, scale by 2 to number of stages -void radix4Ifft(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff){ - - radix4IfftStageOne(pSrc, pDst, Nfft); - - radix4IfftMainStages(pSrc, pDst, Nfft, pCoeff, lenCoeff); - - radix4IfftLastStage(pSrc, pDst, Nfft, pCoeff, lenCoeff); -} - -#ifdef FFT_PARALLEL -void radix4IfftParallel(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff) { - - radix4IfftStageOneParallel(pSrc, pDst, Nfft); - - radix4IfftMainStagesParallel(pSrc, pDst, Nfft, pCoeff, lenCoeff); - - radix4IfftLastStageParallel(pSrc, pDst, Nfft, pCoeff, lenCoeff); -} -#endif - -#ifdef FFT_STRIDE -void radix4IfftStride(INT16* pSrc, INT16* pDst, int Nfft, const INT16* pCoeff, int lenCoeff) { - - radix4IfftStageOneStride(pSrc, pDst, Nfft); - - radix4IfftMainStages(pSrc, pDst, Nfft, pCoeff, lenCoeff); - - radix4IfftLastStage(pSrc, pDst, Nfft, pCoeff, lenCoeff); -} -#endif - -#define FFT_SCALE -void radix2Ifft(INT16* pBfr, int Nfft, const INT16* pCoeff, int lenCoeff){ - - int ii, jj, mm, nn; - int loop1, loop2, coeffStride; - - INT16 x1I, x1Q, W1I, W1Q; - INT16 z0I, z0Q, z1I, z1Q; - INT16 y0I, y0Q, y1I, y1Q; - - unsigned int loadCoeff; - unsigned int *coeffPtr; - unsigned int loadTemp, storeTemp; - unsigned int *loopPtr; - unsigned int tempVal0, tempVal1; - unsigned int *dataPtr = (unsigned int *)pBfr; - -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31-__clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int bitShift = IDX_LEN-log2Nfft; - - // bit-reversal permutation - for(ii=0;ii>bitShift; - if(jj > ii){ // swap [ii] for [jj] - tempVal0 = dataPtr[jj]; - tempVal1 = dataPtr[ii]; - dataPtr[ii] = tempVal0; - dataPtr[jj] = tempVal1; - } - } - - loop1 = Nfft>>1; - loop2 = 1; - - // radix-2 stages - coeffStride = lenCoeff>>1; - for(ii=0;ii>16); - for(nn=loop1;nn>0;nn--){ - loadTemp = loopPtr[0]; - z0I = (loadTemp&0xffff); z0Q = (loadTemp>>16); - loadTemp = loopPtr[loop2]; - x1I = (loadTemp&0xffff); x1Q = (loadTemp>>16); -#ifdef ARM_DEBUG - z1I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W1I, x1I), radix4Fft16BitTo32BitMultiplier(W1Q, x1Q))); // r6 - z1Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W1I, x1Q), radix4Fft16BitTo32BitMultiplier(W1Q, x1I))); - - y0I = radix4Fft16BitAddShift(z0I, z1I); y0Q = radix4Fft16BitAddShift(z0Q, z1Q); - y1I = radix4Fft16BitSubtractShift(z0I, z1I); y1Q = radix4Fft16BitSubtractShift(z0Q, z1Q); -#else - z1I = ( (((INT32)W1I*x1I)-((INT32)W1Q*x1Q))>>15 ); - z1Q = ( (((INT32)W1I*x1Q)+((INT32)W1Q*x1I))>>15 ); -#ifdef FFT_SCALE - y0I = (z0I+z1I)>>1; y0Q = (z0Q+z1Q)>>1; - y1I = (z0I-z1I)>>1; y1Q = (z0Q-z1Q)>>1; -#else - y0I = (z0I+z1I); y0Q = (z0Q+z1Q); - y1I = (z0I-z1I); y1Q = (z0Q-z1Q); -#endif -#endif - storeTemp = (unsigned short)y0Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y0I; - *loopPtr = storeTemp; loopPtr+=loop2; - storeTemp = (unsigned short)y1Q; - storeTemp<<=16; - storeTemp |=(unsigned short)y1I; - *loopPtr = storeTemp; loopPtr+=loop2; - - } - loopPtr += 1 - 2*loop2*loop1; - } - loop1 >>=1; - loop2 <<=1; - coeffStride>>=1; - } -} - -const float twiddleTableFlt[2 * MAX_FFT_FLT] = { - 1.00000000f, 0.00000000f, 0.995184720f, 0.0980171412f, 0.980785251f, 0.195090324f, 0.956940353f, 0.290284663f, 0.923879504f, 0.382683456f, 0.881921232f, 0.471396744f, - 0.831469595f, 0.555570245f, 0.773010433f, 0.634393334f, 0.707106769f, 0.707106769f, 0.634393275f, 0.773010433f, 0.555570185f, 0.831469655f, 0.471396655f, 0.881921291f, - 0.382683426f, 0.923879504f, 0.290284634f, 0.956940353f, 0.195090234f, 0.980785310f, 0.0980171338f, 0.995184720f, -4.37113883e-08f, 1.00000000f, -0.0980172232f, 0.995184720f, - -0.195090324f, 0.980785251f, -0.290284723f, 0.956940293f, -0.382683516f, 0.923879504f, -0.471396834f, 0.881921232f, -0.555570364f, 0.831469536f, -0.634393275f, 0.773010492f, - -0.707106769f, 0.707106769f, -0.773010492f, 0.634393275f, -0.831469655f, 0.555570185f, -0.881921351f, 0.471396625f, -0.923879623f, 0.382683277f, -0.956940353f, 0.290284723f, - -0.980785310f, 0.195090309f, -0.995184720f, 0.0980170965f, -1.00000000f, -8.74227766e-08f, -0.995184720f, -0.0980172679f, -0.980785251f, -0.195090488f, -0.956940293f, -0.290284872f, - -0.923879504f, -0.382683426f, -0.881921232f, -0.471396774f, -0.831469536f, -0.555570304f, -0.773010373f, -0.634393394f, -0.707106650f, -0.707106888f, -0.634393334f, -0.773010433f, - -0.555570006f, -0.831469774f, -0.471396685f, -0.881921291f, -0.382683128f, -0.923879683f, -0.290284544f, -0.956940353f, -0.195090383f, -0.980785251f, -0.0980169326f, -0.995184720f, - 1.19248806e-08f, -1.00000000f, 0.0980174318f, -0.995184720f, 0.195090413f, -0.980785251f, 0.290285021f, -0.956940234f, 0.382683605f, -0.923879445f, 0.471396714f, -0.881921291f, - 0.555570424f, -0.831469476f, 0.634393334f, -0.773010433f, 0.707107008f, -0.707106531f, 0.773010552f, -0.634393156f, 0.831469595f, -0.555570304f, 0.881921351f, -0.471396536f, - 0.923879564f, -0.382683426f, 0.956940413f, -0.290284395f, 0.980785310f, -0.195090234f, 0.995184779f, -0.0980167687f -}; - - -void radix2FftFlt(float* pBfr, int Nfft, const float* pCoeff, int lenCoeff) { - - int ii, jj, mm, nn; - int loop1, loop2, coeffStride; - - float x1I, x1Q, W1I, W1Q; - float z0I, z0Q, z1I, z1Q; - float y0I, y0Q, y1I, y1Q; - - const float *coeffPtr; - float *loopPtr; - unsigned long long tempVal0, tempVal1; - unsigned long long *dataPtr = (unsigned long long *)pBfr; - -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31 - __clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int bitShift = IDX_LEN - log2Nfft; - - // bit-reversal permutation - for (ii = 0;ii> bitShift; - if (jj > ii) { // swap [ii] for [jj] - tempVal0 = dataPtr[jj]; - tempVal1 = dataPtr[ii]; - dataPtr[ii] = tempVal0; - dataPtr[jj] = tempVal1; - } - } - - loop1 = Nfft >> 1; - loop2 = 1; - - // radix-2 stages - coeffStride = lenCoeff >> 1; - for (ii = 0;ii0;nn--) { - z0I = loopPtr[0]; - z0Q = loopPtr[1]; - x1I = loopPtr[2 * loop2]; - x1Q = loopPtr[2 * loop2 + 1]; - - z1I = ((W1I*x1I) + (W1Q*x1Q)); - z1Q = ((W1I*x1Q) - (W1Q*x1I)); - - y0I = (z0I + z1I); y0Q = (z0Q + z1Q); - y1I = (z0I - z1I); y1Q = (z0Q - z1Q); - - loopPtr[0] = y0I; - loopPtr[1] = y0Q; - loopPtr += 2 * loop2; - loopPtr[0] = y1I; - loopPtr[1] = y1Q; - loopPtr += 2 * loop2; - } - loopPtr += 2 - 4 * loop2*loop1; - } - loop1 >>= 1; - loop2 <<= 1; - coeffStride >>= 1; - } -} - -void radix2IfftFlt(float* pBfr, int Nfft, const float* pCoeff, int lenCoeff) { - - int ii, jj, mm, nn; - int loop1, loop2, coeffStride; - - float x1I, x1Q, W1I, W1Q; - float z0I, z0Q, z1I, z1Q; - float y0I, y0Q, y1I, y1Q; - - const float *coeffPtr; - float *loopPtr; - unsigned long long tempVal0, tempVal1; - unsigned long long *dataPtr = (unsigned long long *)pBfr; - -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31 - __clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int bitShift = IDX_LEN - log2Nfft; - - // bit-reversal permutation - for (ii = 0;ii> bitShift; - if (jj > ii) { // swap [ii] for [jj] - tempVal0 = dataPtr[jj]; - tempVal1 = dataPtr[ii]; - dataPtr[ii] = tempVal0; - dataPtr[jj] = tempVal1; - } - } - - loop1 = Nfft >> 1; - loop2 = 1; - - // radix-2 stages - coeffStride = lenCoeff >> 1; - for (ii = 0;ii0;nn--) { - z0I = loopPtr[0]; - z0Q = loopPtr[1]; - x1I = loopPtr[2 * loop2]; - x1Q = loopPtr[2 * loop2 + 1]; - - z1I = ((W1I*x1I) - (W1Q*x1Q)); - z1Q = ((W1I*x1Q) + (W1Q*x1I)); -#ifdef FFT_SCALE - y0I = (z0I + z1I)/2; y0Q = (z0Q + z1Q)/2; - y1I = (z0I - z1I)/2; y1Q = (z0Q - z1Q)/2; -#else - y0I = (z0I + z1I); y0Q = (z0Q + z1Q); - y1I = (z0I - z1I); y1Q = (z0Q - z1Q); -#endif - loopPtr[0] = y0I; - loopPtr[1] = y0Q; - loopPtr += 2*loop2; - loopPtr[0] = y1I; - loopPtr[1] = y1Q; - loopPtr += 2*loop2; - } - loopPtr += 2 - 4 * loop2*loop1; - } - loop1 >>= 1; - loop2 <<= 1; - coeffStride >>= 1; - } -} - -#ifdef FFT_STRIDE -void radix2IfftStride(INT16* pBfr, int Nfft, const INT16* pCoeff, int lenCoeff) { - - int ii, jj, mm, nn; - int loop1, loop2, coeffStride; - - INT16 x1I, x1Q, W1I, W1Q; - INT16 z0I, z0Q, z1I, z1Q; - INT16 y0I, y0Q, y1I, y1Q; - - unsigned int loadCoeff; - unsigned int *coeffPtr; - unsigned int loadTemp, storeTemp; - unsigned int *loopPtr; - unsigned int tempVal0, tempVal1; - unsigned int *dataPtr = (unsigned int *)pBfr; - -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31 - __clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int bitShift = IDX_LEN - log2Nfft; - - // bit-reversal permutation - for (ii = 0;ii> bitShift; - if (jj > ii) { // swap [ii] for [jj] - tempVal0 = dataPtr[NUM_PARALLEL*jj]; - tempVal1 = dataPtr[NUM_PARALLEL*ii]; - dataPtr[NUM_PARALLEL*ii] = tempVal0; - dataPtr[NUM_PARALLEL*jj] = tempVal1; - } - } - - loop1 = Nfft >> 1; - loop2 = 1; - - // radix-2 stages - coeffStride = lenCoeff >> 1; - for (ii = 0;ii> 16); - for (nn = loop1;nn>0;nn--) { - loadTemp = loopPtr[0]; - z0I = (loadTemp & 0xffff); z0Q = (loadTemp >> 16); - loadTemp = loopPtr[NUM_PARALLEL*loop2]; - x1I = (loadTemp & 0xffff); x1Q = (loadTemp >> 16); -#ifdef ARM_DEBUG - z1I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg(radix4Fft16BitTo32BitMultiplier(W1I, x1I), radix4Fft16BitTo32BitMultiplier(W1Q, x1Q))); // r6 - z1Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator(radix4Fft16BitTo32BitMultiplier(W1I, x1Q), radix4Fft16BitTo32BitMultiplier(W1Q, x1I))); - - y0I = radix4Fft16BitAddShift(z0I, z1I); y0Q = radix4Fft16BitAddShift(z0Q, z1Q); - y1I = radix4Fft16BitSubtractShift(z0I, z1I); y1Q = radix4Fft16BitSubtractShift(z0Q, z1Q); -#else - z1I = ((((INT32)W1I*x1I) - ((INT32)W1Q*x1Q)) >> 15); - z1Q = ((((INT32)W1I*x1Q) + ((INT32)W1Q*x1I)) >> 15); -#ifdef FFT_SCALE - y0I = (z0I + z1I) >> 1; y0Q = (z0Q + z1Q) >> 1; - y1I = (z0I - z1I) >> 1; y1Q = (z0Q - z1Q) >> 1; -#else - y0I = (z0I + z1I); y0Q = (z0Q + z1Q); - y1I = (z0I - z1I); y1Q = (z0Q - z1Q); -#endif -#endif - storeTemp = (unsigned short)y0Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y0I; - *loopPtr = storeTemp; loopPtr += NUM_PARALLEL*loop2; - storeTemp = (unsigned short)y1Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y1I; - *loopPtr = storeTemp; loopPtr += NUM_PARALLEL*loop2; - - } - loopPtr += NUM_PARALLEL * (1 - 2 * loop2*loop1); - } - loop1 >>= 1; - loop2 <<= 1; - coeffStride >>= 1; - } -} -#endif - -#ifdef FFT_PARALLEL -void radix2IfftParallel(INT16* pBfr, int Nfft, const INT16* pCoeff, int lenCoeff) { - - int ii, jj, qq, mm, nn; - int loop1, loop2, coeffStride; - - INT16 x1I, x1Q, W1I, W1Q; - INT16 z0I, z0Q, z1I, z1Q; - INT16 y0I, y0Q, y1I, y1Q; - - unsigned int loadCoeff; - unsigned int *coeffPtr; - unsigned int loadTemp, storeTemp; - unsigned int *loopPtr; - unsigned int tempVal0, tempVal1; - unsigned int *dataPtr = (unsigned int *)pBfr; - -#if defined(ARM_GCC) || defined(ARM_DS5) || defined(ARM_966) - int log2Nfft = 31 - __clz(Nfft); -#else - int log2Nfft = myMsb(Nfft); -#endif - int bitShift = IDX_LEN - log2Nfft; - - // bit-reversal permutation - for (ii = 0;ii> bitShift; - if (jj > ii) { // swap [ii] for [jj] - for (qq = 0;qq < NUM_PARALLEL;qq++) { - tempVal0 = dataPtr[NUM_PARALLEL*jj + qq]; - tempVal1 = dataPtr[NUM_PARALLEL*ii + qq]; - dataPtr[NUM_PARALLEL*ii + qq] = tempVal0; - dataPtr[NUM_PARALLEL*jj + qq] = tempVal1; - } - } - } - - loop1 = Nfft >> 1; - loop2 = 1; - - // radix-2 stages - coeffStride = lenCoeff >> 1; - for (ii = 0;ii> 16); - for (nn = loop1;nn>0;nn--) { - for (qq = 0;qq < NUM_PARALLEL;qq++) { - loadTemp = loopPtr[qq]; - z0I = (loadTemp & 0xffff); z0Q = (loadTemp >> 16); - loadTemp = loopPtr[NUM_PARALLEL*loop2 + qq]; - x1I = (loadTemp & 0xffff); x1Q = (loadTemp >> 16); - - z1I = ((((INT32)W1I*x1I) - ((INT32)W1Q*x1Q)) >> 15); - z1Q = ((((INT32)W1I*x1Q) + ((INT32)W1Q*x1I)) >> 15); - y0I = (z0I + z1I) >> 1; y0Q = (z0Q + z1Q) >> 1; - y1I = (z0I - z1I) >> 1; y1Q = (z0Q - z1Q) >> 1; - - storeTemp = (unsigned short)y0Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y0I; - loopPtr[qq] = storeTemp; - storeTemp = (unsigned short)y1Q; - storeTemp <<= 16; - storeTemp |= (unsigned short)y1I; - loopPtr[NUM_PARALLEL*loop2 + qq] = storeTemp; - } - loopPtr += 2*NUM_PARALLEL*loop2; - } - loopPtr += NUM_PARALLEL * (1 - 2 * loop2*loop1); - } - loop1 >>= 1; - loop2 <<= 1; - coeffStride >>= 1; - } -} -#endif - -// size 64 FFT with only four non-zero inputs, pruned down -void radix4Fft4in64(unsigned int *loadPtr, unsigned int *fftOutBfr, const INT16* pCoeff, int lenCoeff){ - - //int Nfft = 64; - - int mm, coeffStride = 0; - int coeffStrideTemp = lenCoeff>>(2*3); - unsigned short *pDst = (unsigned short*)fftOutBfr; -#if defined(ARM_GCC) || defined(ARM_DS5) - int coeffStrideDw; - //int coeffStrideTemp = lenCoeff>>3; - unsigned int z0 = loadPtr[0]; - unsigned int x1 = loadPtr[1]; - unsigned int x2 = loadPtr[2]; - unsigned int x3 = loadPtr[3]; -#ifdef ARM_DS5 - INT16* pCoeffTemp; - int reg5, reg6, reg7, reg8, reg9; -#endif -#else - INT16 *pSrc = (INT16*)loadPtr; - INT16 x1I, x1Q, x2I, x2Q, x3I, x3Q; // x0I, x0Q, - INT16 z0I, z0Q, z1I, z1Q, z2I, z2Q, z3I, z3Q; - INT16 y0I, y0Q, y1I, y1Q, y2I, y2Q, y3I, y3Q; - INT16 AI, AQ, BI, BQ, CI, CQ, DI, DQ; - INT16 W1I, W1Q, W2I, W2Q, W3I, W3Q; // W0I, W0Q, - - z0I = pSrc[0]; z0Q = pSrc[1]; - x1I = pSrc[2]; x1Q = pSrc[2+1]; - x2I = pSrc[4]; x2Q = pSrc[4+1]; - x3I = pSrc[6]; x3Q = pSrc[6+1]; -#endif - - for(mm=0;mm<16;mm++){ -#if defined(ARM_GCC) || defined(ARM_DS5) - coeffStrideDw = 4*coeffStride; -#ifdef ARM_GCC - asm volatile( - "LDR r5, [%[pCoeff], %[coeffStrideDw]]! \n\t" - "SMUAD r8, r5, %[x1] \n\t" - "SMUSDX r9, r5, %[x1] \n\t" - "LDR r5, [%[pCoeff], %[coeffStrideDw]]! \n\t" - "LSL r9, r9, #1 \n\t" - "PKHTB r6, r9, r8, ASR #15 \n\t" - "SMUAD r8, r5, %[x2] \n\t" - "SMUSDX r9, r5, %[x2] \n\t" - "LDR r5, [%[pCoeff], %[coeffStrideDw]] \n\t" - "LSL r9, r9, #1 \n\t" - "PKHTB r7, r9, r8, ASR #15 \n\t" - "SMUAD r8, r5, %[x3] \n\t" - "SMUSDX r9, r5, %[x3] \n\t" - "LSL r9, r9, #1 \n\t" - "PKHTB r5, r9, r8, ASR #15 \n\t" - "QADD16 r8, %[z0], r7 \n\t" - "QSUB16 r9, %[z0], r7 \n\t" - "QADD16 r7, r6, r5 \n\t" - "QSUB16 r6, r6, r5 \n\t" - "SHADD16 r5, r8, r7 \n\t" - "SHSUB16 r7, r8, r7 \n\t" - "SMUAD r5, r5, r5 \n\t" - "SMUAD r7, r7, r7 \n\t" - "ASR r5, r5, #13 \n\t" - "ASR r7, r7, #13 \n\t" - "SHASX r8, r9, r6 \n\t" - "SHSAX r6, r9, r6 \n\t" - "SMUAD r6, r6, r6 \n\t" - "SMUAD r8, r8, r8 \n\t" - "ASR r6, r6, #13 \n\t" - "ASR r8, r8, #13 \n\t" - "STRH r7, [%[pDst]], #32 \n\t" - "STRH r8, [%[pDst]], #32 \n\t" - "STRH r5, [%[pDst]], #32 \n\t" - "STRH r6, [%[pDst]], #32 \n\t" - : [pDst]"+r"(pDst) : [pCoeff]"r"(pCoeff), [coeffStrideDw]"r"(coeffStrideDw), [z0]"r"(z0), [x1]"r"(x1), [x2]"r"(x2), [x3]"r"(x3) - : "r5", "r6", "r7", "r8", "r9"); -#else - pCoeffTemp = pCoeff; - __asm volatile - { - LDR reg5, [pCoeffTemp, coeffStrideDw]! - SMUAD reg8, reg5, x1 - SMUSDX reg9, reg5, x1 - LDR reg5, [pCoeffTemp, coeffStrideDw]! - LSL reg9, reg9, #1 - PKHTB reg6, reg9, reg8, ASR #15 - SMUAD reg8, reg5, x2 - SMUSDX reg9, reg5, x2 - LDR reg5, [pCoeffTemp, coeffStrideDw] - LSL reg9, reg9, #1 - PKHTB reg7, reg9, reg8, ASR #15 - SMUAD reg8, reg5, x3 - SMUSDX reg9, reg5, x3 - LSL reg9, reg9, #1 - PKHTB reg5, reg9, reg8, ASR #15 - QADD16 reg8, z0, reg7 - QSUB16 reg9, z0, reg7 - QADD16 reg7, reg6, reg5 - QSUB16 reg6, reg6, reg5 - SHADD16 reg5, reg8, reg7 - SHSUB16 reg7, reg8, reg7 - SMUAD reg5, reg5, reg5 - SMUAD reg7, reg7, reg7 - ASR reg5, reg5, #13 - ASR reg7, reg7, #13 - SHASX reg8, reg9, reg6 - SHSAX reg6, reg9, reg6 - SMUAD reg6, reg6, reg6 - SMUAD reg8, reg8, reg8 - ASR reg6, reg6, #13 - ASR reg8, reg8, #13 - STRH reg7, [pDst], #32 - STRH reg8, [pDst], #32 - STRH reg5, [pDst], #32 - STRH reg6, [pDst], #32 - } -#endif - pDst -= 4*16; -#else - //W0I = pCoeff[0]; W0Q = pCoeff[1]; - W1I = pCoeff[2*coeffStride]; W1Q = pCoeff[2*coeffStride+1]; - W2I = pCoeff[4*coeffStride]; W2Q = pCoeff[4*coeffStride+1]; - W3I = pCoeff[6*coeffStride]; W3Q = pCoeff[6*coeffStride+1]; - // z0 = W0*x0, z1 = W1*x1, z2 = W2*x2, z3 = W3*x3 assuming W0Q = -exp(1i*2*pi*nk/N) - -#ifdef ARM_DEBUG - z1I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W1I, x1I), radix4Fft16BitTo32BitMultiplier(W1Q, x1Q))); - z1Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W1I, x1Q), radix4Fft16BitTo32BitMultiplier(W1Q, x1I))); - z2I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W2I, x2I), radix4Fft16BitTo32BitMultiplier(W2Q, x2Q))); - z2Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W2I, x2Q), radix4Fft16BitTo32BitMultiplier(W2Q, x2I))); - z3I = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(W3I, x3I), radix4Fft16BitTo32BitMultiplier(W3Q, x3Q))); - z3Q = radix4Fft32BitTo16BitRounding(radix4Fft32BitAccumulatorNeg( radix4Fft16BitTo32BitMultiplier(W3I, x3Q), radix4Fft16BitTo32BitMultiplier(W3Q, x3I))); - - AI = radix4Fft16BitAdd(z0I, z2I); AQ = radix4Fft16BitAdd(z0Q, z2Q); - BI = radix4Fft16BitSubtract(z0I, z2I); BQ = radix4Fft16BitSubtract(z0Q, z2Q); - CI = radix4Fft16BitAdd(z1I, z3I); CQ = radix4Fft16BitAdd(z1Q, z3Q); - DI = radix4Fft16BitSubtract(z1I, z3I); DQ = radix4Fft16BitSubtract(z1Q, z3Q); - - y0I = radix4Fft16BitAddShift(AI, CI); y0Q = radix4Fft16BitAddShift(AQ, CQ); - y1I = radix4Fft16BitAddShift(BI, DQ); y1Q = radix4Fft16BitSubtractShift(BQ, DI); - y2I = radix4Fft16BitSubtractShift(AI, CI); y2Q = radix4Fft16BitSubtractShift(AQ, CQ); - y3I = radix4Fft16BitSubtractShift(BI, DQ); y3Q = radix4Fft16BitAddShift(BQ, DI); - - pDst[2*16] = radix4Fft32BitTo16BitUnsignedRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(y0I, y0I), radix4Fft16BitTo32BitMultiplier(y0Q, y0Q)),-2); - pDst[3*16] = radix4Fft32BitTo16BitUnsignedRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(y1I, y1I), radix4Fft16BitTo32BitMultiplier(y1Q, y1Q)),-2); - pDst[0] = radix4Fft32BitTo16BitUnsignedRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(y2I, y2I), radix4Fft16BitTo32BitMultiplier(y2Q, y2Q)),-2); - pDst[16] = radix4Fft32BitTo16BitUnsignedRounding(radix4Fft32BitAccumulator( radix4Fft16BitTo32BitMultiplier(y3I, y3I), radix4Fft16BitTo32BitMultiplier(y3Q, y3Q)),-2); -#else - z1I = (INT16)( (((INT32)W1I*x1I)+((INT32)W1Q*x1Q))>>15 ); - z1Q = (INT16)( (((INT32)W1I*x1Q)-((INT32)W1Q*x1I))>>15 ); - z2I = (INT16)( (((INT32)W2I*x2I)+((INT32)W2Q*x2Q))>>15 ); - z2Q = (INT16)( (((INT32)W2I*x2Q)-((INT32)W2Q*x2I))>>15 ); - z3I = (INT16)( (((INT32)W3I*x3I)+((INT32)W3Q*x3Q))>>15 ); - z3Q = (INT16)( (((INT32)W3I*x3Q)-((INT32)W3Q*x3I))>>15 ); - - AI = (z0I+z2I); AQ = (z0Q+z2Q); - BI = (z0I-z2I); BQ = (z0Q-z2Q); - CI = (z1I+z3I); CQ = (z1Q+z3Q); - DI = (z1I-z3I); DQ = (z1Q-z3Q); - - y0I = (AI+CI)>>1; y0Q = (AQ+CQ)>>1; - y1I = (BI+DQ)>>1; y1Q = (BQ-DI)>>1; - y2I = (AI-CI)>>1; y2Q = (AQ-CQ)>>1; - y3I = (BI-DQ)>>1; y3Q = (BQ+DI)>>1; - - pDst[2*16] = (INT16) ( (((INT32)y0I*y0I)+((INT32)y0Q*y0Q))>>13 ); - pDst[3*16] = (INT16) ( (((INT32)y1I*y1I)+((INT32)y1Q*y1Q))>>13 ); - pDst[0] = (INT16) ( (((INT32)y2I*y2I)+((INT32)y2Q*y2Q))>>13 ); - pDst[16] = (INT16) ( (((INT32)y3I*y3I)+((INT32)y3Q*y3Q))>>13 ); -#endif - -#endif - //printf("%d | %d | %d | %d\n",pDst[0],pDst[16],pDst[2*16],pDst[3*16]); - //printf("%d\n",(unsigned int)pDst); - pDst ++; - coeffStride += coeffStrideTemp; - } -} - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_radix4Fft.h b/mcux/middleware/wifi_nxp/wls/wls_radix4Fft.h deleted file mode 100644 index c7c6123318..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_radix4Fft.h +++ /dev/null @@ -1,75 +0,0 @@ -#if CONFIG_WLS_CSI_PROC -/** @file wls_radix4Fft.h - * - * @brief This file contains header file for fixed-point FFT functions - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * DFW header file for fixed-point FFT functions - ************************************************************************/ - -#ifndef WLS_RADIX4_FFT -#define WLS_RADIX4_FFT - -#include "wls_param_defines.h" - -#define INT8 signed char -#define INT16 short -#define INT32 int -#ifdef STA_20_ONLY -#define MAX_FFT_SIZE_256 -#define MAX_FFT_SIZE 256 -#else -#define MAX_FFT_SIZE_2048 -#define MAX_FFT_SIZE 2048 -#endif -// #define TWIDDLE_HALF_SIZE -#define TWIDDLE_BIPT 15 -// #define BIASED_ROUNDING - -// call separate stages -void radix4FftStageOne(INT16 *pSrc, INT16 *pDst, int Nfft); -void radix4FftMainStages(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); -void radix4FftLastStage(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); - -void radix4IfftStageOne(INT16 *pSrc, INT16 *pDst, int Nfft); -void radix4IfftMainStages(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); -void radix4IfftLastStage(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); - -// auxiliary function -unsigned int reverse(register unsigned int x); -#ifndef ARM_DS5 -unsigned int __clz(uint32_t x); -#endif - -void radix4Fft(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); -void radix4Fft4in64(unsigned int *loadPtr, unsigned int *fftOutBfr, const INT16 *pCoeff, int lenCoeff); - -void radix4Ifft(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); -void radix4IfftParallel(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); -void radix4IfftStride(INT16 *pSrc, INT16 *pDst, int Nfft, const INT16 *pCoeff, int lenCoeff); - -void radix2Ifft(INT16 *pSrc, int Nfft, const INT16 *pCoeff, int lenCoeff); -void radix2IfftParallel(INT16 *pSrc, int Nfft, const INT16 *pCoeff, int lenCoeff); -void radix2IfftStride(INT16 *pSrc, int Nfft, const INT16 *pCoeff, int lenCoeff); - -void radix2FftFlt(float *pBfr, int Nfft, const float *pCoeff, int lenCoeff); -void radix2IfftFlt(float *pBfr, int Nfft, const float *pCoeff, int lenCoeff); - -#define MAX_FFT_FLT 64 -extern const float twiddleTableFlt[2 * MAX_FFT_FLT]; - -#ifdef TWIDDLE_HALF_SIZE -extern const INT16 radix4FftTwiddleArr[MAX_FFT_SIZE]; -#else -extern const INT16 radix4FftTwiddleArr[2 * MAX_FFT_SIZE]; -#endif - -#endif - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_structure_defs.h b/mcux/middleware/wifi_nxp/wls/wls_structure_defs.h deleted file mode 100644 index 419c3e76b6..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_structure_defs.h +++ /dev/null @@ -1,566 +0,0 @@ -#if CONFIG_WLS_CSI_PROC -/** @file wls_structure_defs.h - * - * @brief This file contains header file for CSI structure definitions - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * DFW Header file for CSI structure definitions - ************************************************************************/ - -#ifndef WLS_STRUCTURE_DEFS_H -#define WLS_STRUCTURE_DEFS_H - -#include "wls_param_defines.h" - -#ifndef DEF_TYPES -#define DEF_TYPES -#define SINT8 signed char -#define SINT16 signed short -#define SINT32 signed int -#define SINT64 signed long long -#define UINT8 unsigned char -#define UINT16 unsigned short -#define UINT32 unsigned int -#define UINT64 unsigned long long -#endif - -typedef struct csiHeaderInfoStruct -{ - unsigned char csi; - unsigned char LTF; - unsigned short dataLength; - unsigned short FCF; - unsigned short dataParameter; - unsigned int HTCF; - unsigned int TSF; - unsigned char ADDR1[6]; - unsigned char ADDR2[6]; - unsigned char SBFnIndex; - // unsigned short PKTinfo; - unsigned char rxDevBw; - unsigned char nRx; - unsigned char nTx; - unsigned char Ng; - unsigned char sigBw; - unsigned char psb; - unsigned char packetType; - - unsigned char cfoCourseEst; - // unsigned int LSig; - unsigned char LSigRate; - unsigned char LSigRateMbps; - unsigned short LSigLength; - unsigned char LSigParity; - unsigned char LSigTail; - - unsigned char cfoFineEst; - - unsigned char HtSigMcs; - unsigned char HtSigCbw20_40; - unsigned short HtSigLength; - - unsigned char HtSigSmoothing; - unsigned char HtSigNotSounding; - unsigned char HtSigAggregation; - unsigned char HtSigStbc; - unsigned char HtSigFecCoding; - unsigned char HtSigShortGi; - unsigned char HtSigNoExtSpatStreams; - unsigned char HtSigCrc; - unsigned char HtSigTailBits; - - unsigned char VhtSigBw; - unsigned char VhtSigStbc; - unsigned char VhtSigGroupId; - unsigned char VhtSigSuNoSts; - unsigned short VhtSigPartialAid; - unsigned char VhtSigTxOpPsNotAllwd; - - unsigned char VhtSigShortGi; - unsigned char VhtSigShortGiDisAmb; - unsigned char VhtSigSuCoding; - unsigned char VhtSigLdpcExtraSymb; - unsigned char VhtSigMcs; - unsigned char VhtSigBeamformed; - unsigned char VhtSigCrc; - unsigned char VhtSigTail; - - unsigned char rxNoiseFloor, rxNfA, rxNfB, rxNfC, rxNfD; - unsigned char rxRssi, rxRssiA, rxRssiB, rxRssiC, rxRssiD; - - unsigned char LltfDvgaCtrMax; - unsigned char LltfBbCtrMax; - unsigned char LltfIfCtrMax; - unsigned char LltfLnaCtrMax; - unsigned char LltfSlnaMax; - - unsigned char HtDvgaCtrMax; - unsigned char HtBbCtrMax; - unsigned char HtIfCtrMax; - unsigned char HtLnaCtrMax; - unsigned char HtSlnaMax; - - unsigned char isLtf; - unsigned char NgDsfShift; - unsigned char scOffset; - - int totalGainLltf; - int totalGainCsi; - -} CsiHeaderInfo; - -#define SOC_W8X64 -#define SOC_W8864 -#define SOC_W8964 - -typedef struct hal_rxinfo -{ - // DWORD-0 -#ifdef SOC_W8X64 - UINT32 reserved0 : 16; - UINT32 sign : 16; // 0xBEEF: tagged by MAC HW - // DWORD-1 -#ifdef SOC_W8964 - UINT32 cfo : 17; - UINT32 reservedcfo : 7; - UINT32 nf : 8; -#else - UINT8 nf_c; - UINT8 nf_b; - UINT8 nf_a; - UINT8 nf; -#endif - // DWORD-2 -#ifdef SOC_W8964 - UINT16 dta : 12; - UINT16 reserveddta : 4; - UINT8 t2; -#else - UINT8 rssi_c; - UINT8 rssi_b; - UINT8 rssi_a; -#endif - UINT8 rssi; // RSSI - // DWORD-3 - UINT16 Rxlength; - UINT16 rx_svc; - // DWORD-4 - UINT32 rx_cq : 24; -#ifdef SOC_W8964 - UINT32 reservedrxcq : 8; -#else - UINT32 rssi_d : 8; -#endif - // DWORD-5 - UINT32 ht_sig1 : 24; -#ifdef SOC_W8964 - UINT32 reservedhtsig : 8; -#else - UINT32 nf_d : 8; -#endif - // DWORD-6 - UINT32 ht_sig2 : 18; - UINT32 Rx_LSig_Rsvd : 1; - UINT32 Rx_LSig_parity : 1; - UINT32 Rx_LSig_bad_parity : 1; - UINT32 Rx_HTSig_bad_crc : 3; - UINT32 rate : 8; -#ifdef SOC_W8864 - // DWORD-7 - UINT32 Rx_Gain_code_a : 22; - UINT32 BBU_Reserved_9_0 : 10; - // DWORD-8 - UINT32 Rx_Gain_code_b : 22; - UINT32 BBU_Reserved_19_10 : 10; - // DWORD-9 - UINT32 Rx_Gain_code_c : 22; - UINT32 BBU_Reserved_29_20 : 10; - // DWORD-10 - UINT32 Rx_Gain_code_d : 22; -#ifdef SOC_W8964 - UINT32 BBU_Reserved_35_30 : 6; - UINT32 mu_cq_valid : 4; -#else - UINT32 BBU_Reserved_39_30 : 10; -#endif - // DWORD-11 - UINT32 pm_rssi_dbm_b : 12; - UINT32 pm_rssi_dbm_a : 12; -#ifdef SOC_W8964 - UINT32 mu_cq0 : 8; -#else - UINT32 BBU_Reserved_47_40 : 8; -#endif - // DWORD-12 - UINT32 pm_rssi_dbm_d : 12; - UINT32 pm_rssi_dbm_c : 12; -#ifdef SOC_W8964 - UINT32 mu_cq1 : 8; -#else - UINT32 BBU_Reserved_55_48 : 8; -#endif - // DWORD-13 - UINT32 pm_nf_dbm_b : 12; - UINT32 pm_nf_dbm_a : 12; -#ifdef SOC_W8964 - UINT32 mu_cq2 : 8; -#else - UINT32 BBU_Reserved_63_56 : 8; -#endif - // DWORD-14 - UINT32 pm_nf_dbm_d : 12; - UINT32 pm_nf_dbm_c : 12; -#ifdef SOC_W8964 - UINT32 mu_cq3 : 8; -#else - UINT32 BBU_Reserved_71_64 : 8; -#endif - // DWORD-15 - UINT32 rx_pkt_secondary : 1; - UINT32 rx_dyn_bw : 1; - UINT32 rx_ind_bw : 2; - UINT32 rx_dup_likely_bw : 2; - UINT32 rx_pkt_info : 3; -#ifdef SOC_W8964 - UINT32 rx_vhtsigb : 23; - // DWORD-16 - UINT16 lltf_phroll; - UINT16 htlf_phroll; -#else - UINT32 ReservedDword15 : 23; - // DWORD-16 - UINT32 ReservedDword16; -#endif - // DWORD-17 - UINT32 T2; - // DWORD-18 - UINT32 T3; - // DWORD-19 - UINT32 Rx_timestamp; - // DWORD-20 - UINT32 TSF; - // DWORD-21 - UINT32 param : 24; - UINT32 Rx_AMPDU_Num : 8; - // DWORD-22 - UINT16 qc; // - UINT16 sq2; // temp tookout foo fb_mcs_param; - // DWORD - 23 - UINT32 ht_ctrl; // -#else - // DWORD-7 - UINT32 Rx_Gain_code_a : 21; - UINT32 BBU_Reserved_10_0 : 11; - // DWORD-8 - UINT32 Rx_Gain_code_b : 21; - UINT32 BBU_Reserved_22_11 : 11; - // DWORD-9 - UINT32 Rx_Gain_code_c : 21; - UINT32 BBU_Reserved_32_22 : 11; - // DWORD-10 - UINT32 Rx_Gain_code_d : 21; - UINT32 BBU_Reserved_43_33 : 11; - // DWORD-11 - UINT32 Rx_timestamp; - // DWORD-12 - UINT32 TSF; - // DWORD-13 - UINT32 param : 24; - UINT32 Rx_AMPDU_Num : 8; - // DWORD-14 - UINT16 qc; - UINT16 sq2; // temp tookout foo fb_mcs_param; - // DWORD-15 - UINT32 ht_ctrl; -#endif - -#else - UINT32 ht_sig2 : 10; - UINT32 params_hi : 6; - UINT32 sign : 16; /* Signature = 0xBEEF */ - - // 1 - UINT8 param; - UINT8 sq1; - UINT8 rate; - UINT8 rssi; - // 2 - UINT8 ts_low; /* timestamp [7:0] */ - UINT8 nf; /* Noise floor */ - UINT16 ts_hi; /* timestamp [23:8] */ - // 3 - UINT16 qc; /* QoS control */ - UINT16 sq2; - - // 4 - UINT32 ht_sig1; - - // 5 - UINT8 rssi_c; - UINT8 rssi_b; - UINT8 rssi_a; - UINT8 svc_low; - - // 6 - UINT8 nf_c; - UINT8 nf_b; - UINT8 nf_a; - UINT8 svc_hi; - - // 7 - UINT32 ht_ctrl; -#endif -} hal_rxinfo_t; - -typedef struct hal_csirxinfo -{ -#ifdef SMAC_BFINFO - // DWORD-0 - UINT32 header_length : 13; - UINT32 rsvd0 : 3; - UINT32 signature : 16; // 0x0000 - // DWORD-1 - UINT32 pktinfo : 20; - UINT32 rsvd1 : 12; - // DWORD-2 - UINT32 lsig : 24; - UINT32 cfo_coarse : 8; - // DWORD-3 - UINT32 htsig1 : 24; - UINT32 cfo_fine : 8; - // DWORD-4 - UINT32 htsig2 : 24; - UINT32 rsvd2 : 8; - // DWORD-5 - UINT32 rx_rssi : 8; - UINT32 rx_noise_floor : 8; - UINT32 rsvd3 : 16; - // DWORD-6 - UINT32 rx_rssi_a : 8; - UINT32 rx_rssi_b : 8; - UINT32 rx_rssi_c : 8; - UINT32 rx_rssi_d : 8; - // DWORD-7 - UINT32 rx_rssi_e : 8; - UINT32 rx_rssi_f : 8; - UINT32 rx_rssi_g : 8; - UINT32 rx_rssi_h : 8; - // DWORD-8 - UINT32 rx_nf_a : 8; - UINT32 rx_nf_b : 8; - UINT32 rx_nf_c : 8; - UINT32 rx_nf_d : 8; - // DWORD-9 - UINT32 rx_nf_e : 8; - UINT32 rx_nf_f : 8; - UINT32 rx_nf_g : 8; - UINT32 rx_nf_h : 8; - // DWORD-10 - UINT32 gain_code_11ft : 24; - UINT32 rsvd4 : 8; - // DWORD-11 - UINT32 gain_code_ht : 24; - UINT32 rsvd5 : 8; - // DWORD-12 - UINT32 timestamp_0; - // DWORD-13 - UINT32 timestamp_1; - // DWORD-14 - UINT32 timestamp_2; - // DWORD-15 - UINT32 timestamp_3; - // DWORD-16 - UINT32 timestamp_4; - // DWORD-17 - UINT32 timestamp_5; - // DWORD-18 - UINT32 timestamp_6; - // DWORD-19 - UINT32 timestamp_7; - // DWORD-20 - UINT32 timestamp_8; -#else - // DWORD-0 - UINT32 data_length : 13; // Includes BF_INFO, LTF Data, and CSI Data. Length in dwords - UINT32 ltf : 1; - UINT32 csi : 1; - UINT32 tbd : 1; - UINT32 signature : 16; // 0xABCD: tagged by MAC HW - // DWORD-1 - UINT32 dataparam : 16; - UINT32 fcf : 16; - // DWORD-2 - UINT32 htcf; - // DWORD-3 - UINT32 tsf; - // DWORD-4 - UINT32 addr1_lo; - // DWORD-5 - UINT32 addr1_hi : 16; - UINT32 addr2_lo : 16; - // DWORD-6 - UINT32 addr2_hi; - // DWORD-7 - UINT32 sbfnindex : 7; - UINT32 rsvd0 : 17; - UINT32 rx_ppdu_seq : 8; - // DWORD-8 -- start of BBUD portion, same as SC5/SCBT - // DWORD-1 - UINT32 pktinfo : 20; - UINT32 rsvd1 : 12; - // DWORD-2 - UINT32 lsig : 24; - UINT32 cfo_coarse : 8; - // DWORD-3 - UINT32 htsig1 : 24; - UINT32 cfo_fine : 8; - // DWORD-4 - UINT32 htsig2 : 24; - UINT32 rsvd2 : 8; - // DWORD-5 - UINT32 rx_rssi : 8; - UINT32 rx_noise_floor : 8; - UINT32 rsvd3 : 16; - // DWORD-6 - UINT32 rx_rssi_a : 8; - UINT32 rx_rssi_b : 8; - UINT32 rx_rssi_c : 8; - UINT32 rx_rssi_d : 8; - // DWORD-7 - UINT32 rx_rssi_e : 8; - UINT32 rx_rssi_f : 8; - UINT32 rx_rssi_g : 8; - UINT32 rx_rssi_h : 8; - // DWORD-8 - UINT32 rx_nf_a : 8; - UINT32 rx_nf_b : 8; - UINT32 rx_nf_c : 8; - UINT32 rx_nf_d : 8; - // DWORD-9 - UINT32 rx_nf_e : 8; - UINT32 rx_nf_f : 8; - UINT32 rx_nf_g : 8; - UINT32 rx_nf_h : 8; - // DWORD-10 - UINT32 gain_code_11ft : 24; - UINT32 rsvd4 : 8; - // DWORD-11 - UINT32 gain_code_ht : 24; - UINT32 rsvd5 : 8; - // DWORD-12 - UINT32 timestamp_0; - // DWORD-13 - UINT32 timestamp_1; - // DWORD-14 - UINT32 timestamp_2; - // DWORD-15 - UINT32 timestamp_3; - // DWORD-16 - UINT32 timestamp_4; - // DWORD-17 - UINT32 timestamp_5; - // DWORD-18 - UINT32 timestamp_6; - // DWORD-19 - UINT32 timestamp_7; - // DWORD-20 - UINT32 timestamp_8; -#endif -} hal_csirxinfo_t; - -typedef struct hal_pktinfo -{ - UINT32 packetType : 3; - UINT32 psb : 3; - UINT32 sigBw : 2; - UINT32 Ng : 1; - UINT32 nTx : 3; - UINT32 nRx : 3; - UINT32 rxDevBw : 2; - UINT32 MU : 1; - UINT32 HELTF : 2; - UINT32 NgDsfShift : 2; // added to pass parameters - UINT32 fftSize : 4; - UINT32 rsvd1 : 4; - UINT32 scOffset : 12; - UINT32 dcPhase : 2; - UINT32 rsvd2 : 18; -} hal_pktinfo_t; - -#ifndef DFW_CSI_PROC -typedef struct hal_wls_processing_input_params -{ - UINT32 enableCsi : 1; // turn on CSI processing - UINT32 enableAoA : 1; // turn on AoA (req. enableCsi==1) - UINT32 nTx : 3; // limit # tx streams to process - UINT32 nRx : 3; // limit # rx to process - UINT32 selCal : 8; // choose cal values - UINT32 dumpMul : 2; // dump extra peaks in AoA - UINT32 enableAntCycling : 1; // enable antenna cycling - UINT32 dumpRawAngle : 1; // Dump Raw Angle - UINT32 useToaMin : 1; // 1: use min combining, 0: power combining; - UINT32 useSubspace : 1; // 1: use subspace algo; 0: no; - UINT32 useFindAngleDelayPeaks : 1; // use this algorithm for AoA - UINT32 rsvd1 : 9; -} hal_wls_processing_input_params_t; - -typedef struct hal_wls_packet_params -{ - UINT32 chNum : 8; // ch_index 1-4, 36:4:140, 149:4:165 - UINT32 isFtmInit : 1; // indicate if this is FTM exchange initiator or responder - UINT32 ftmSignalBW : 3; // Channel bandwidth 0: 20 MHz, 1: 40 MHz, 2: 80 MHz, 3: 160 Mhz, 4-7 reserved - UINT32 ftmPacketType : 3; // Format of FTM exchange 0: legacy, 1:HT, 3:VHT, 4:HE - UINT32 freqBand : 1; // Declare freq. band, 0: 2.4 GHz, 1: 5 GHz - UINT32 peerMacAddress_lo : 16; // first 16 bit of FTM peer MAC address - UINT32 peerMacAddress_hi; // rest of FTM peer MAC address - UINT32 info_tsf; // TSF counter from tx/rx-info - UINT32 cal_data_low_A : 10; - UINT32 cal_data_low_B : 10; - UINT32 cal_data_low_C : 10; - UINT32 rsvd2 : 2; - UINT32 cal_data_high_A : 10; - UINT32 cal_data_high_B : 10; - UINT32 cal_data_high_C : 10; - UINT32 rsvd3 : 2; - UINT32 cable_len_A : 8; - UINT32 cable_len_B : 8; - UINT32 cable_len_C : 8; - UINT32 antenna_spacing : 8; -} hal_wls_packet_params_t; -#else -#include "dsp_cmd.h" -#endif - -typedef struct hal_cal_struc -{ - short calData[4]; - short centerFreq; -} hal_cal_struc_t; - -typedef struct reg_buf_config -{ - // DWORD-0 - UINT16 buf_size : 13; - UINT16 rsvd1 : 3; - UINT16 buf_num : 12; - UINT16 rsvd0 : 4; -} reg_buf_config_t; - -typedef struct reg_buf_ptr -{ - // DWORD-0 - UINT16 wr_ptr : 13; - UINT16 rsvd1 : 3; - UINT16 rd_ptr : 13; - UINT16 rsvd0 : 3; -} reg_buf_ptr_t; - -#endif - -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_subspace_processing.c b/mcux/middleware/wifi_nxp/wls/wls_subspace_processing.c deleted file mode 100644 index 1df9a15971..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_subspace_processing.c +++ /dev/null @@ -1,883 +0,0 @@ -/** @file wls_subspace_processing.c - * - * @brief This file contains source code to calculate fine timing using sub-space methods (ESPRIT) - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ -* DFW Source code to calculate fine timing using sub-space methods (ESPRIT) -************************************************************************/ - -#include -#if CONFIG_WLS_CSI_PROC - -// Standard includes. -#include -//#include -#include -#include -// 4 sqrtf, 1 atan2f, 1 atanf, 2 floorf -#ifdef ARM_DS5 -#include -#endif - -// Specific includes. -#include "wls_subspace_processing.h" -#include "wls_param_defines.h" -#include "wls_QR_algorithm.h" -#include "wls_radix4Fft.h" - -#define FFT_INPUT_SIZE_MAX 32 -#define FFT_INPUT_SIZE_SHORT_WINDOW 16 -#define FFT_INPUT_SIZE_LONG_WINDOW 32 - -#ifdef STA_20_ONLY -#define BUFFER_OFFSET (12*MAX_FFT_SIZE/2) -#else -#define BUFFER_OFFSET (3*MAX_FFT_SIZE/2) -#endif -#define SUBSP_DIM_L_MAX MAX_MAT_SIZE -//needs to be floor(0.75*SUB_DIM_L) -#define SIG_SUBSP_DIM_MAX 7 // 15 - -#define MUSIC_THRESH_REL_5GHz 0.01f -#define MUSIC_THRESH_REL_2GHz 0.1f -#define MUSIC_THRESH_MIN 0.02f -#define MUSIC_THRESH_MAX 0.10f - -#define SUB_DET_THRESH_5G_80MHZ 0.1f -#define SUB_DET_THRESH_5G_40MHZ 0.08f -#define SUB_DET_THRESH_5G_20MHZ 0.04f // 0.1f -#define SUB_DET_THRESH_20MHZ 0.253f -#define SUB_DET_THRESH_Legacy 0.38f -#define SUB_DET_THRESH_REL_5G_80MHZ 0.01f -#define SUB_DET_THRESH_REL_5G_40MHZ 0.025f -#define SUB_DET_THRESH_REL_5G_20MHZ 0.005f -#define SUB_DET_THRESH_REL_2G 0.015f -#define SUB_DET_THRESH_REL_Legacy 0.25f - -#define SUB_DIM_L_6 6 -#define SUB_DIM_L_10 10 -#define SUB_DIM_L_12 12 -#define SUB_DIM_L_16 16 -#define SUB_DIM_L_20 20 -#define SUB_DIM_L_22 22 - -#define USE_TLS -#define FFT_TD_OSF_SHIFT 1 -#define FFT_FD_EDGE 0 - -void calcCorMatrix(hal_pktinfo_t *pktinfo, unsigned int *fftOutBuffer, unsigned int *totalpower, int firstPathDelay, float *realCorrMatrix, - unsigned int *scratchMemory, int subsp_dim_L, int fft_input_size, int FD_downsample, int* numObsPtr) -{ - - int ii, jj, kk, ll, rowIdx, colIdx, qq; - int loadOffset; - float *fftTempBuffer = (float*)scratchMemory; //[2*(FFT_INPUT_SIZE_MAX<nRx+1; - int nTx = pktinfo->nTx+1; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - - int firstPathIndex = (firstPathDelay+(1<<(TOA_FPATH_BIPT-1)))>>TOA_FPATH_BIPT; - int windowStart = firstPathIndex-(fft_input_size <<(IFFT_OSF_SHIFT-1)); // center window middle on first-path - - int stepsize = 1<<(IFFT_OSF_SHIFT - FFT_TD_OSF_SHIFT); - int numObs = fft_input_size - subsp_dim_L - 2*FFT_FD_EDGE; - - memset((void*)corrMatrix, 0, 2*SUBSP_DIM_L_MAX*SUBSP_DIM_L_MAX*sizeof(float)); - loadOffsetMask = ifftSizeOsf*NUM_PARALLEL - 1; - - for(ii=0;ii> 16) & 0xffff; - - valI = tempI*scale; - *strPtr++ = valI; - valQ = tempQ*scale; - *strPtr++ = valQ; - } - radix2FftFlt(fftTempBuffer, (fft_input_size << FFT_TD_OSF_SHIFT) * FD_downsample, twiddleTableFlt, MAX_FFT_FLT); - if (FD_downsample == 4) { - for (ll = 1;ll < fft_input_size;ll++) { - fftTempBuffer[2 * ll] = fftTempBuffer[2 * FD_downsample * ll]; - fftTempBuffer[2 * ll + 1] = fftTempBuffer[2 * FD_downsample * ll + 1]; - } - } - // diagonal - // rowIdx = 0 - ldPtr = fftTempBuffer + 2*(FFT_FD_EDGE+1); // skip 0 - strPtr = corrMatrix; - sumI = 0; - storeIdx = 0; - for (kk = 0;kk < numObs;kk++) { - valI = *ldPtr++; // fftTempBuffer[2 * kk]; - valQ = *ldPtr++; // fftTempBuffer[2 * kk + 1]; - corrI = valI*valI + valQ*valQ; - sumI += corrI; - tempBuffer[storeIdx++] = corrI; - } - *strPtr += sumI; //corrMatrix[0] - strPtr += 2 * (SUBSP_DIM_L_MAX + 1); - loadIdx = 0; - for (rowIdx = 1;rowIdx < subsp_dim_L;rowIdx++) { - sumI -= tempBuffer[loadIdx++]; - valI = *ldPtr++; // fftTempBuffer[2 * (rowIdx-1 + kk)]; - valQ = *ldPtr++; // fftTempBuffer[2 * (rowIdx-1 + kk) + 1]; - corrI = valI*valI + valQ*valQ; - sumI += corrI; - tempBuffer[storeIdx++] = corrI; - *strPtr += sumI; //corrMatrix[rowIdx * 2 * (SUBSP_DIM_L_MAX+1)] += sumI; - strPtr += 2 * (SUBSP_DIM_L_MAX + 1); - } - // off-diagonals - for (colIdx = 1; colIdx < subsp_dim_L; colIdx++) { - // rowIdx = 0 - ldPtr = fftTempBuffer + 2 * (FFT_FD_EDGE + 1); - ldPtrTr = fftTempBuffer + 2 * (FFT_FD_EDGE + 1) + 2 * colIdx; - strPtr = corrMatrix + 2 * colIdx * SUBSP_DIM_L_MAX; - sumI = 0; - sumQ = 0; - storeIdx = 0; - for (kk = 0;kk < numObs;kk++) { - - valI = *ldPtrTr++; //fftTempBuffer[2 * (colIdx + kk)]; - valQ = *ldPtrTr++; //fftTempBuffer[2 * (colIdx + kk) + 1]; - - valTempI = *ldPtr++; //fftTempBuffer[2 * kk]; - valTempQ = *ldPtr++; //fftTempBuffer[2 * kk + 1]; - - corrI = valI*valTempI + valQ*valTempQ; - corrQ = valQ*valTempI - valI*valTempQ; - - sumI += corrI; - sumQ += corrQ; - - tempBuffer[storeIdx++] = corrI; - tempBuffer[storeIdx++] = corrQ; - } - *strPtr++ += sumI; //corrMatrix[2 * colIdx * SUBSP_DIM_L_MAX] - *strPtr++ -= sumQ; //corrMatrix[2 * colIdx * SUBSP_DIM_L_MAX + 1] - strPtr += 2 * SUBSP_DIM_L_MAX; - loadIdx = 0; - for (rowIdx = 1;rowIdx < subsp_dim_L - colIdx;rowIdx++) { - sumI -= tempBuffer[loadIdx++]; - sumQ -= tempBuffer[loadIdx++]; - - valI = *ldPtrTr++; //fftTempBuffer[2 * (rowIdx - 1 + colIdx + kk)]; - valQ = *ldPtrTr++; //fftTempBuffer[2 * (rowIdx - 1 + colIdx + kk) + 1]; - - valTempI = *ldPtr++; //fftTempBuffer[2 * (rowIdx - 1 + kk)]; - valTempQ = *ldPtr++; //fftTempBuffer[2 * (rowIdx - 1 + kk) + 1]; - - corrI = valI*valTempI + valQ*valTempQ; - corrQ = valQ*valTempI - valI*valTempQ; - - sumI += corrI; - sumQ += corrQ; - - tempBuffer[storeIdx++] = corrI; - tempBuffer[storeIdx++] = corrQ; - - *strPtr++ += sumI; //corrMatrix[(rowIdx + colIdx) * 2 * SUBSP_DIM_L_MAX + 2 * rowIdx] - *strPtr++ -= sumQ; //corrMatrix[(rowIdx + colIdx) * 2 * SUBSP_DIM_L_MAX + 2 * rowIdx + 1] - strPtr += 2 * SUBSP_DIM_L_MAX; - } - } - } - } - } - for (rowIdx = 0;rowIdx < subsp_dim_L;rowIdx++) { - ldPtr = corrMatrix + (rowIdx * 2 * SUBSP_DIM_L_MAX); - strPtrTr = corrMatrix +(2 * rowIdx); - for (colIdx = 0;colIdx < rowIdx;colIdx++) { - - corrI = *ldPtr++; // use symmetry - corrQ = *ldPtr++; - - *strPtrTr++ = corrI; - *strPtrTr = -corrQ; strPtrTr += 2* SUBSP_DIM_L_MAX -1; - } - } - - // create real matrix - for(rowIdx=0;rowIdxmax){ - max = eigVals[jj]; - } - } - - if (sub_det_thresh_rel*max > thresh) - thresh = sub_det_thresh_rel*max; - - for(ii=0;iimax){ - max = eigVals[jj]; - maxIdx = jj; - } - } - if(max>thresh){ - eigVals[maxIdx] = 0; - // copy eigVec - for(jj=0;jj 1){ - tempMatSum = (float*) inputBuff; // tempMatSum[SUBSP_DIM_L*SUBSP_DIM_L]; - //tempMatDiff = (float*) inputBuff+SUBSP_DIM_L*SUBSP_DIM_L; // tempMatDiff[SUBSP_DIM_L*SUBSP_DIM_L] - tempMatDiff = (float*) inputBuff+ SUBSP_DIM_L_MAX*SUBSP_DIM_L_MAX; // tempMatDiff[SUBSP_DIM_L*SUBSP_DIM_L] - for (ii = 0; ii0) ? omega_hat_pos : (omega_hat_pos + 2 * PI); -} - -void calc_QpTimesE(float *eMatrix, float *qMatrix, float *qPeMatrix, int matSizeN, int matSizeM){ - - int ii, jj, kk; - float sum; - - for(ii=0;ii0)?omega_hat[ii]:(omega_hat[ii]+2*PI); - for(jj=ii;jj>0;jj--){ - if(omega_hat_sort[jj]1e-2f){ - jj++; - tempStr = omega_hat_sort[ii]; - omega_hat_sort[jj] = tempStr; - } - } - sigSpaceDim -= (ii-1-jj); - - return sigSpaceDim; -} - -void calcDelayAmplitude(hal_pktinfo_t *pktinfo, - unsigned int *fftOutBuffer, - unsigned int *totalpower, - int firstPathDelay, - int sigSpaceDim, - float *omega_hat_sort, - float *tau_hat, - float *aMatrix, - int fft_input_size) -{ - int ii, jj, kk, qq; - int index; - unsigned int *loadPtr; - unsigned int tempLoad; - short tempI, tempQ; - float tempVal, scale, delta; - float leftI, leftQ, rightI, rightQ; - float leftSqr, rightSqr, valSqr; - //float fracIndex[SUBSP_DIM_L]; - float fracIndex[SUBSP_DIM_L_MAX]; - - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; - int ifftSizeOsf = 1<<(pktinfo->fftSize+6); - int signalBandwidth = 20<<(pktinfo->sigBw); - int stepsize = 1<>TOA_FPATH_BIPT; - int windowStart = firstPathIndex-(fft_input_size <<(IFFT_OSF_SHIFT-1)); // center window middle on first-path - - unsigned int bitMask = NUM_PARALLEL *ifftSizeOsf - 1; - - // map phase to delay - for(kk=0;kk> 16) & 0xffff; - leftI = tempI*scale; - leftQ = tempQ*scale; - leftSqr = leftI*leftI + leftQ*leftQ; - - tempLoad = loadPtr[(NUM_PARALLEL*(index + 1)) &bitMask]; - tempI = tempLoad & 0xffff; - tempQ = (tempLoad >> 16) & 0xffff; - rightI = tempI*scale; - rightQ = tempQ*scale; - rightSqr = rightI*rightI + rightQ*rightQ; - - valSqr = (rightSqr - leftSqr)*delta + leftSqr; - - aMatrix[kk + (jj + qq)*sigSpaceDim + ii*nRx*sigSpaceDim] = valSqr; - } - } - } - } -} - -int determineFirstPath(hal_pktinfo_t *pktinfo, - float *tau_hat, - float *aMatrix, - int sigSpaceDim, - float music_thresh_rel) -{ - int ii, jj, kk; - int nRx = pktinfo->nRx+1; - int nTx = pktinfo->nTx+1; - int firstPathIdx; - int maxIdx = -1; - int firstPathDelay = 0; - - float detThresh; - float maxVal = 0; - - //float sumPower[SUBSP_DIM_L]; - float sumPower[SUBSP_DIM_L_MAX]; - - int signalBandwidth = 20<<(pktinfo->sigBw); - int stepsize = 1<maxVal){ - maxVal = sumPower[kk]; - maxIdx = kk; - } - } - - detThresh = maxVal*music_thresh_rel; - if(detThresh>MUSIC_THRESH_MAX){ - detThresh=MUSIC_THRESH_MAX; - } - else if(detThresh=0;kk--){ - if(sumPower[kk]>detThresh){ - firstPathIdx = kk; - } - } - firstPathDelay = (int) floorf((tau_hat[firstPathIdx]*signalBandwidth*stepsize*(1<nRx + 1; - int nTx = pktinfo->nTx + 1; - int subsp_dim_L = SUB_DIM_L_20; - float sub_det_thresh, sub_det_thresh_rel, music_thresh_rel; - int fft_input_size = FFT_INPUT_SIZE_LONG_WINDOW; - float *realCorrMatrix = (float*)procBuffer; // realCorrMatrix[SUBSP_DIM_L*SUBSP_DIM_L] - float *eigValPlusVec = (float*)procBuffer + BUFFER_OFFSET; // eigValPlusVec[MAX_MAT_SIZE+MAX_MAT_SIZE*MAX_MAT_SIZE] - float *eMatrix = (float*)procBuffer; // eMatrix[2*SUBSP_DIM_L*SUBSP_DIM_L] -#ifdef USE_TLS - float *ePeMatrix = (float*)procBuffer + BUFFER_OFFSET; //ePeMatrix[4*(SIG_SUBSP_DIM_MAX-2)*(SIG_SUBSP_DIM_MAX-2)] - float *eigValPlusVecEE = (float*)procBuffer; // eigValPlusVec[MAX_MAT_SIZE+MAX_MAT_SIZE*MAX_MAT_SIZE] - float *vMatrix = (float*)procBuffer + BUFFER_OFFSET; // vMatrix[4*(SUBSP_DIM_L-2)*(SUBSP_DIM_L-2)] - float *qMatrix = (float*)procBuffer; // qMatrix[MAX_MAT_SIZE*MAX_MAT_SIZE] - float *qPvMatrix = (float*)procBuffer + BUFFER_OFFSET / 2; // qPvMatrix[SUBSP_DIM_L*SUBSP_DIM_L] - float *psiHatMatrix = (float*)procBuffer; // psiHatMatrix[SUBSP_DIM_L*SUBSP_DIM_L] -#else - float *qMatrix = (float*) procBuffer+BUFFER_OFFSET; // qMatrix[MAX_MAT_SIZE*MAX_MAT_SIZE] - float *qPeMatrix = (float*) procBuffer+BUFFER_OFFSET*3/2; // qPeMatrix[SUBSP_DIM_L*SUBSP_DIM_L] - float *psiHatMatrix = (float*) procBuffer+BUFFER_OFFSET; // psiHatMatrix[SUBSP_DIM_L*SUBSP_DIM_L] -#endif - float *eigPsiHat = (float*)procBuffer + BUFFER_OFFSET / 2; // eigPsiHat[SUBSP_DIM_L] - float *omega_hat_sort = (float*)procBuffer; // omega_hat_sort[SUBSP_DIM_L] - float *tau_hat = (float*)procBuffer + BUFFER_OFFSET / 4; // tau_hat[SUBSP_DIM_L] - float *aMatrix = (float*)procBuffer + BUFFER_OFFSET / 2; // aMatrix[MAX_TX*MAX_RX*2*SUBSP_DIM_L] - - int numObs, sigSpaceDim, FD_downsample=1; - - if ((pktinfo->packetType == 0) && (pktinfo->sigBw == 0) && (pktinfo->rxDevBw == 2)) { //process legacy pkt - subsp_dim_L = SUB_DIM_L_6; - sub_det_thresh = SUB_DET_THRESH_Legacy; - sub_det_thresh_rel = SUB_DET_THRESH_REL_Legacy; - } - else if (packetparams->chNum >= 36) { //5G band - if (pktinfo->sigBw == 2) { - sub_det_thresh = SUB_DET_THRESH_5G_80MHZ; - sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_80MHZ; - } - else if (pktinfo->sigBw == 1) { - sub_det_thresh = SUB_DET_THRESH_5G_40MHZ; - sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_40MHZ; - } - else if (pktinfo->sigBw == 0) { -#if 0 //larger auto-correlation matrix size - sub_det_thresh = SUB_DET_THRESH_5G_20MHZ; - sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_20MHZ; -#else - fft_input_size = FFT_INPUT_SIZE_SHORT_WINDOW; - subsp_dim_L = SUB_DIM_L_10; - sub_det_thresh = SUB_DET_THRESH_5G_20MHZ; - sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_20MHZ; -#endif - } - else if (pktinfo->packetType < 2) { - subsp_dim_L = SUB_DIM_L_16; - fft_input_size = FFT_INPUT_SIZE_LONG_WINDOW; - sub_det_thresh = SUB_DET_THRESH_20MHZ; - sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_20MHZ; - } - else { - FD_downsample = 4; - subsp_dim_L = SUB_DIM_L_10; - sub_det_thresh = SUB_DET_THRESH_Legacy; - sub_det_thresh_rel = 0.035f; - } - } - else { //2.4G band - if (pktinfo->sigBw == 1) { - subsp_dim_L = SUB_DIM_L_16; - } - else { - subsp_dim_L = SUB_DIM_L_12; - } - sub_det_thresh = SUB_DET_THRESH_20MHZ; - sub_det_thresh_rel = SUB_DET_THRESH_REL_2G; - fft_input_size = FFT_INPUT_SIZE_LONG_WINDOW; - } - - // determine firstpath power threshold according to frequency, and pass to "determineFirstPath" - music_thresh_rel = (packetparams->chNum < 36) ? MUSIC_THRESH_REL_2GHz : MUSIC_THRESH_REL_5GHz; - - // in: fftOutBuffer[], totalpower[] - // out: realCorrMatrix[SUBSP_DIM_L*SUBSP_DIM_L] {100 DW} - calcCorMatrix(pktinfo, fftOutBuffer, totalpower, firstPathDelay, realCorrMatrix, procBuffer + BUFFER_OFFSET, subsp_dim_L, fft_input_size, FD_downsample, &numObs); - //numObs = (fft_input_size - subsp_dim_L + 1) * 2 * nRx*nTx; - sub_det_thresh *= (numObs * 2 * nRx*nTx); // account for CorMatrix scaling - - // eigen(Shur) decomposition - // in: realCorrMatrix[] - // out: eigValPlusVec[]; [D:MAX_MAT_SIZE (eigen values)| Q:SUBSP_DIM_L*SUBSP_DIM_L (eigen vectors)] {10+100 DW +30DW scratch} - QR_algorithm(realCorrMatrix, eigValPlusVec, subsp_dim_L, 0); - - // find signal space Eig Vals and construct E matrix for ESPRIT - // in: eigValPlusVec[] - used also as scratch memory - // out: eMatrix[]; [E_lower: (SUBSP_DIM_L-1)*SIG_SUBSP_DIM_MAX| E_upper: (SUBSP_DIM_L-1)*SIG_SUBSP_DIM_MAX] {200 DW} - sigSpaceDim = findSigSpace(eigValPlusVec, eMatrix, sub_det_thresh, subsp_dim_L, pktinfo, sub_det_thresh_rel); - - if (sigSpaceDim < 2){ - if (sigSpaceDim == 0){ // error, no signal over threshold - return -1; - } - //else{ // (sigSpaceDim == 1) - // // for single eigen value, use simpler code - // // in: eMatrix[] - // // out: omega_hat_sort[] - // calcDimOnePhase(eMatrix, omega_hat_sort, SUBSP_DIM_L); - - // // determine delays and amplitudes - // // in : fftOutBuffer[], totalpower[], omega_hat_sort[] - // // out: tau_hat[], aMatrix[] {10+80 DW} - // calcDelayAmplitude(pktinfo, fftOutBuffer, totalpower, firstPathDelay, sigSpaceDim, omega_hat_sort, tau_hat, aMatrix); - - // // determine first path - // // in: tau_hat[], aMatrix[] - // // out: fineTimingRes - // *fineTimingRes = determineFirstPath(pktinfo, tau_hat, aMatrix, sigSpaceDim); - - // return 0; - //} - } -#ifdef USE_TLS // QR of E - // calc E'*E - // in: eMatrix[] - // out: ePeMatrix[] {256 DW} - calcEtimesE(eMatrix, ePeMatrix,(subsp_dim_L -1), 2*sigSpaceDim); - // eigen(Shur) decomposition - // in: ePeMatrix[] - // out: eigValPlusVec[]; [D:2*SIG_SUBSP_DIM_MAX (eigen values)| Q:4*SIG_SUBSP_DIM_MAX^2 (eigen vectors)] {12+144 DW +30DW scratch} - QR_algorithm(ePeMatrix, eigValPlusVecEE, 2*sigSpaceDim, 1); - - // in: eigValPlusVec[] - // out: vMatrix[]; [V_upper: SIG_SUBSP_DIM_MAX*SIG_SUBSP_DIM_MAX| V_lower: SIG_SUBSP_DIM_MAX*SIG_SUBSP_DIM_MAX] {256 DW} - constructVmatrix(eigValPlusVecEE, vMatrix, sigSpaceDim, 2*sigSpaceDim); - - // Psi_hat = V_lower\V_upper; - // V_lower = Q*R - // in: vMatrix[] V_lower (updated) - // out: qMatrix[SIG_SUBSP_DIM_MAX*SIG_SUBSP_DIM_MAX] {100 DW } - QR_decomposition(vMatrix+sigSpaceDim*SUBSP_DIM_L_MAX, qMatrix, sigSpaceDim, sigSpaceDim); - // calc Q'*V_upper - // in: vMatrix[] V_upper, qMatrix[] - // out: qPvMatrix[] {100 DW +30 DW scratch} - calc_QpTimesE(vMatrix, qMatrix, qPvMatrix, sigSpaceDim, sigSpaceDim); - // Psi_hat = (R^-1)(Q'*E_upper) solve with back substitution - // in: qPvMatrix[], vMatrix[] V_lower (updated) - // out: Psi_hat[]: (sigSpaceDim*sigSpaceDim) {100 DW} - myBackSub(qPvMatrix, vMatrix+sigSpaceDim*SUBSP_DIM_L_MAX, psiHatMatrix, sigSpaceDim, sigSpaceDim); - -#else - // LS Psi_hat = E_lower\E_upper; - // E_lower = Q*R - // in: eMatrix[] E_lower (updated) - // out: qMatrix[] {100 DW} - QR_decomposition(eMatrix, qMatrix, (subsp_dim_L -1), sigSpaceDim); - // calc Q'*E_upper - // in: eMatrix[] E_upper, qMatrix[] - // out: qPeMatrix[] {100 DW} - calc_QpTimesE(eMatrix+ SUBSP_DIM_L_MAX*sigSpaceDim, qMatrix, qPeMatrix, (subsp_dim_L -1), sigSpaceDim); - // Psi_hat = (R^-1)(Q'*E_upper) solve with back substitution - // in: qPeMatrix[], eMatrix[] E_lower (updated) - // out: Psi_hat[]: (sigSpaceDim*sigSpaceDim) {100 DW} - myBackSub(qPeMatrix, eMatrix, psiHatMatrix, (subsp_dim_L -1), sigSpaceDim); - -#endif - // omega_hat = atan(real(eig(Psi_hat))) - // in: psiHatMatrix[] - // out: eigPsiHat[] {10 DW +30 DW scratch} - unsym_QR_algorithm(psiHatMatrix, eigPsiHat, sigSpaceDim); - - // determine phase - // in: eigPsiHat[], sigSpaceDim - // out: omega_hat_sort[], sigSpaceDim {10 DW} - sigSpaceDim = calcPhase(eigPsiHat, sigSpaceDim, omega_hat_sort); - - // determine delays and amplitudes - // in : fftOutBuffer[], totalpower[], omega_hat_sort[] - // out: tau_hat[], aMatrix[] {10+80 DW} - calcDelayAmplitude(pktinfo, fftOutBuffer, totalpower, firstPathDelay, sigSpaceDim, omega_hat_sort, tau_hat, aMatrix, fft_input_size); - - // determine first path - // in: tau_hat[], aMatrix[] - // out: fineTimingRes - *fineTimingRes = determineFirstPath(pktinfo, tau_hat, aMatrix, sigSpaceDim, music_thresh_rel); - - return 0; -} -#endif /* CONFIG_WLS_CSI_PROC */ diff --git a/mcux/middleware/wifi_nxp/wls/wls_subspace_processing.h b/mcux/middleware/wifi_nxp/wls/wls_subspace_processing.h deleted file mode 100644 index d2ca80b7ab..0000000000 --- a/mcux/middleware/wifi_nxp/wls/wls_subspace_processing.h +++ /dev/null @@ -1,32 +0,0 @@ -#if CONFIG_WLS_CSI_PROC -/** @file wls_subspace_processing.h - * - * @brief This file contains header file for sub-space based fine timing calculation - * - * Copyright 2023-2024 NXP - * - * SPDX-License-Identifier: BSD-3-Clause - * - */ - -/************************************************************************ - * Header file for sub-space based fine timing calculation - ************************************************************************/ - -#ifndef WLS_SUBSPACE_PROCESSING -#define WLS_SUBSPACE_PROCESSING - -#include "wls_structure_defs.h" - -int calcSubspaceFineTiming(hal_pktinfo_t *pktinfo, // structure with CSI buffer parameters - unsigned int *fftOutBuffer, // buffer holding time-domain CSI - unsigned int *totalpower, // array holding power per rx/tx channel - int firstPathDelay, // existing first path estimate - int *fineTimingRes, // result of algorithm - unsigned int *procBuffer, // buffer for processing - needs about 2k bytes - hal_wls_packet_params_t *packetparams // passing packetinfo to determin 2.4/5G -); - -#endif - -#endif /* CONFIG_WLS_CSI_PROC */