/*
* Copyright (c) 2023-2028, SpaceTouch Inc.
*
* SPDX-License-Identifier: Apache-2.0
*
* Change Logs:
* Date          Author      Notes
* 2023-08-11    jinyaoh     the first version
*/

#include "hal_udsp.h"


#define PI_Q29  ((int32_t)0x6487ED51)


void hal_udsp_init()
{
    DEV_RMU->RSTEN0 &= ~DEV_RMU_RSTEN0_UDSP;
    DEV_CMU->CLKEN0 |= DEV_CMU_CLKEN0_UDSP;
    DEV_RMU->RSTEN0 |= DEV_RMU_RSTEN0_UDSP;
    DEV_UDSP->PD = DEV_UDSP_PD_END_MASK
                   | DEV_UDSP_PD_FFT_BIT31_OF
                   | DEV_UDSP_PD_FFT_BIT32_OF;
}


/**
 * This function will load parameters into buffer1.
 * It is used to assist the main operations of UDSP, such as COS DIV Hanning window coefficients.
 *
 * @param channel: The value ranges from 0 to 7
 * @param coef: Coefficient cache address to be loaded
 * @param size: The width of each member, for example sizeof(int)
 * @param nmemb: Number of coefficients
 * @param bur_addr: The coefficient is placed in the address offset of usdp buffer1, and the total buffer size is 4096word
 *
 * @note  Note that the udsp does not have direct access to local memory and cache, but handles it internally
 *
 * @return None
 */
void hal_udsp_load_buffer1(int channel, void *coef, uint32_t size, uint32_t nmemb, uint32_t bur_addr)
{
    uint32_t addr_hsb, input_addr;
    input_addr = (uint32_t)coef;
    addr_hsb = input_addr >> 28;
    input_addr = addr_hsb ? input_addr : (input_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr + size * nmemb;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SMF = size << 16
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | ((size - 1) << 12)
                                 | (nmemb - 1);
    DEV_UDSP->CHN[channel].LMF0 = DEV_UDSP_CHNn_LMF0_SMA_WR_BUF1_SEL_ENABLE
                                  | (bur_addr << 20);
    DEV_UDSP->CHN[channel].CFG0 = DEV_UDSP_CHNn_CFG0_SMR_ENABLE;

    //CCTL
    if (addr_hsb == 0x4 || addr_hsb == 0x5)
    {
        //write_csr(NDS_MCCTLCOMMAND, CCTL_L1D_WB_ALL);
        //将D-cache空间写入memory,但不会将D-cache空间内容无效化
        write_csr(NDS_MCCTLCOMMAND, 7);
    }
}


/**
 * This function will enable or disable the udsp channel
 *
 * @param channel: The value ranges from 0 to 7
 * @param state: ENABLE or DISABLE
 *
 * @return None
 */
void hal_udsp_cmd(uint32_t channelx, uint32_t state)
{
    if (state != DISABLE)
    {
        DEV_UDSP->EN = (1UL << (channelx)) | (1UL << (channelx + 16));
    }
    else
    {
        DEV_UDSP->EN = (1UL << (channelx + 16));
    }
}

/**
 * This function will get channel status
 *
 * @param channelx: The value ranges from 0 to 7
 *
 * @return 1:ENABLE or 0:DISABLE
 */
uint32_t hal_udsp_get_cmd_status(uint32_t channelx)
{
    return (DEV_UDSP->EN & (1UL << (channelx))) >> channelx;
}

/**
 * This function will calculate the rfft
 *
 * @param channel: The value ranges from 0 to 7
 * @param fft_struct: UDSP_FFT_InputStruct structure that contains the fft attributes to be computed
 *
 * @return =0: No abnormality
 *         >0: The configuration is successful, but data overflows. Procedure
 *         <0: Configuration failure
 */
int32_t hal_udsp_rfft(int channel, UDSP_FFT_InputStruct *fft_struct)
{
    int32_t ret = 0;
    uint32_t input_addr_hsb, output_addr_hsb;
    uint32_t low_addr, upper_addr, current_addr;
    input_addr_hsb = (uint32_t)fft_struct->in_data_current;
    input_addr_hsb = input_addr_hsb >> 24;
    low_addr = (uint32_t)fft_struct->in_data_low;
    upper_addr = (uint32_t)fft_struct->in_data_upper;
    current_addr = (uint32_t)fft_struct->in_data_current;

    if (input_addr_hsb == 0)
    {
        low_addr = low_addr | 0x61000000;
        upper_addr = upper_addr | 0x61000000;
        current_addr = current_addr | 0x61000000;
    }

    DEV_UDSP->CHN[channel].SLA = low_addr;
    DEV_UDSP->CHN[channel].SUA = upper_addr;
    DEV_UDSP->CHN[channel].SCA = current_addr;
    output_addr_hsb = (uint32_t)fft_struct->out_data_current;
    output_addr_hsb = output_addr_hsb >> 24;
    low_addr = (uint32_t)fft_struct->out_data_low;
    upper_addr = (uint32_t)fft_struct->out_data_upper;
    current_addr = (uint32_t)fft_struct->out_data_current;

    if (output_addr_hsb == 0)
    {
        low_addr = low_addr | 0x61000000;
        upper_addr = upper_addr | 0x61000000;
        current_addr = current_addr | 0x61000000;
    }

    DEV_UDSP->CHN[channel].DLA = low_addr;
    DEV_UDSP->CHN[channel].DUA = upper_addr;
    DEV_UDSP->CHN[channel].DCA = current_addr;
    DEV_UDSP->CHN[channel].SMF = (fft_struct->input_size << 16) //源偏移地址
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | ((fft_struct->input_size - 1) << 12)
                                 | (fft_struct->nmemb - 1) //总长度
                                 ;
    uint32_t spectrum_enable = fft_struct->spectrum_enable;
    spectrum_enable = spectrum_enable > 1 ? 1 : spectrum_enable;

    if (spectrum_enable == DISABLE)
    {
        DEV_UDSP->CHN[channel].DMF = (fft_struct->output_size << 16) //目的地址偏移
                                     | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_COMPLEXORLONG64
                                     | ((fft_struct->output_size - 1) << 12)
                                     | (fft_struct->nmemb + 1); //因为复数，并只输出前一半加1
    }
    else
    {
        if (fft_struct->output_size == 4)
        {
            DEV_UDSP->CHN[channel].DMF = (4 << 16) //目的地址偏移
                                         | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                         | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                         | (fft_struct->nmemb / 2); //输出fft_struct->nmemb/2+1个32bit数
        }
        else if (fft_struct->output_size == 8)
        {
            DEV_UDSP->CHN[channel].DMF = (4 << 16) //目的地址偏移
                                         | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_COMPLEXORLONG64
                                         | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                         | (fft_struct->nmemb + 1); //输出fft_struct->nmemb/2+1个32bit数
        }
        else
        {
            ret = -2;
        }
    }

    DEV_UDSP->CHN[channel].LMF0 = DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                  | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    uint32_t fft_point_flag = 0;

    switch (fft_struct->nmemb)
    {
    case 32:
        fft_point_flag = 0;
        break;

    case 64:
        fft_point_flag = 1;
        break;

    case 128:
        fft_point_flag = 2;
        break;

    case 256:
        fft_point_flag = 3;
        break;

    case 512:
        fft_point_flag = 4;
        break;

    case 1024:
        fft_point_flag = 5;
        break;

    default:
        ret = -1;
        break;
    }

    uint32_t win_enable = fft_struct->win_enable;
    win_enable = win_enable > 1 ? 1 : win_enable;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   | (win_enable << 2)
                                   | DEV_UDSP_CHNn_CFG0_FFT_ENABLE
                                   | (spectrum_enable << 8)
                                   | DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   | (fft_point_flag << 20)
                                   ;

    //CCTL
    if (input_addr_hsb == 0x40 || input_addr_hsb == 0x50)
    {
        //write_csr(NDS_MCCTLCOMMAND, CCTL_L1D_WB_ALL);
        //将D-cache空间写入memory,但不会将D-cache空间内容无效化
        write_csr(NDS_MCCTLCOMMAND, 7);
    }

    if (output_addr_hsb == 0x40 || output_addr_hsb == 0x50)
    {
        //write_csr(NDS_MCCTLCOMMAND, CCTL_L1D_INVAL_ALL);
        //将D-cache空间无效化
        write_csr(NDS_MCCTLCOMMAND, 23);
    }

    return ret;
}

/**
 * This function will calculate the irfft
 *
 * @param channel: The value ranges from 0 to 7
 * @param irfft_struct: UDSP_IRFFT_InputStruct structure that contains the irfft attributes to be computed
 *
 * @note  The udsp cannot access the local memory and cache directly, but handles them internally.
 *        The input is 32bit but the actual irfft can only handle 24bit data
 *
 * @return =0: No abnormality
 *         >0: The configuration is successful, but data overflows. Procedure
 *         <0: Configuration failure
 */
int hal_udsp_irfft(int channel, UDSP_IRFFT_InputStruct *irfft_struct)
{
    int32_t ret = 0;
    uint32_t input_size = irfft_struct->input_size;
    uint32_t ifft_number = irfft_struct->nmemb;
    int32_t *complex_temp = irfft_struct->complex_temp;

    if (input_size > 4)
    {
        return -1;
    }

    int8_t *complex_temp_i8 = (void *)0;
    int16_t *complex_temp_i16 = (void *)0;
    int32_t *complex_temp_i32 = (void *)0;

    if (input_size == 1)
    {
        complex_temp_i8 = (int8_t *)irfft_struct->in_data_current;
    }
    else if (input_size == 2)
    {
        complex_temp_i16 = (int16_t *)irfft_struct->in_data_current;
    }
    else if (input_size == 4)
    {
        complex_temp_i32 = (int32_t *)irfft_struct->in_data_current;
    }

    for (int i = 0; i < (ifft_number / 2 + 1) * 2; i++)
    {
        if (input_size == 1)
        {
            complex_temp[i] = complex_temp_i8[i];
        }
        else if (input_size == 2)
        {
            complex_temp[i] = complex_temp_i16[i];
        }
        else if (input_size == 4)
        {
            complex_temp[i] = complex_temp_i32[i];
        }
    }

    //将输入共轭对称
    for (int i = 0; i < (ifft_number / 2 - 1); i++)
    {
        complex_temp[(ifft_number * 2 - 2) - 2 * i] = complex_temp[2 + 2 * i];
        complex_temp[(ifft_number * 2 - 1) - 2 * i] = -complex_temp[3 + 2 * i];
    }

    uint32_t input_addr_hsb, output_addr_hsb;
    uint32_t low_addr, upper_addr, current_addr;
    input_addr_hsb = (uint32_t)complex_temp;
    input_addr_hsb = input_addr_hsb >> 28;
    low_addr = (uint32_t)complex_temp;
    upper_addr = (uint32_t)complex_temp + ifft_number * 2 * sizeof(complex_temp[0]);
    current_addr = (uint32_t)complex_temp;

    if (input_addr_hsb == 0)
    {
        low_addr = low_addr | 0x61000000;
        upper_addr = upper_addr | 0x61000000;
        current_addr = current_addr | 0x61000000;
    }

    DEV_UDSP->CHN[channel].SLA = low_addr;
    DEV_UDSP->CHN[channel].SUA = upper_addr;
    DEV_UDSP->CHN[channel].SCA = current_addr;
    output_addr_hsb = (uint32_t)irfft_struct->out_data_current;
    output_addr_hsb = output_addr_hsb >> 28;
    low_addr = (uint32_t)irfft_struct->out_data_low;
    upper_addr = (uint32_t)irfft_struct->out_data_upper;
    current_addr = (uint32_t)irfft_struct->out_data_current;

    if (output_addr_hsb == 0)
    {
        low_addr = low_addr | 0x61000000;
        upper_addr = upper_addr | 0x61000000;
        current_addr = current_addr | 0x61000000;
    }

    DEV_UDSP->CHN[channel].DLA = low_addr;
    DEV_UDSP->CHN[channel].DUA = upper_addr;
    DEV_UDSP->CHN[channel].DCA = current_addr;
    DEV_UDSP->CHN[channel].SMF = (sizeof(complex_temp[0]) << 16) //源偏移地址 与输入无关
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_COMPLEXORLONG64
                                 | ((2) << 12) //fft ifft 不能发是32bit输入
                                 | (ifft_number * 2 - 1) //复数的长度需要乘2
                                 ;
    DEV_UDSP->CHN[channel].DMF = (irfft_struct->output_size << 16) //目的地址偏移
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                 | ((irfft_struct->output_size - 1) << 12)
                                 | (ifft_number - 1);
    DEV_UDSP->CHN[channel].LMF0 = DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                  | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    uint32_t fft_point_flag = 0;

    switch (ifft_number)
    {
    case 32:
        fft_point_flag = 0;
        break;

    case 64:
        fft_point_flag = 1;
        break;

    case 128:
        fft_point_flag = 2;
        break;

    case 256:
        fft_point_flag = 3;
        break;

    case 512:
        fft_point_flag = 4;
        break;

    case 1024:
        fft_point_flag = 5;
        break;

    default:
        ret = -1;
        break;
    }

    DEV_UDSP->CHN[channel].LMF4 = (fft_point_flag + 3) << 12;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_IFFT_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   | (fft_point_flag << 20)
                                   ;

    //CCTL
    if (input_addr_hsb == 0x4 || input_addr_hsb == 0x5)
    {
        //write_csr(NDS_MCCTLCOMMAND, CCTL_L1D_WB_ALL);
        //将D-cache空间写入memory,但不会将D-cache空间内容无效化
        write_csr(NDS_MCCTLCOMMAND, 7);
    }

    if (output_addr_hsb == 0x4 || output_addr_hsb == 0x5)
    {
        //write_csr(NDS_MCCTLCOMMAND, CCTL_L1D_INVAL_ALL);
        //将D-cache空间无效化
        write_csr(NDS_MCCTLCOMMAND, 23);
    }

    return ret;
}

/**
 * This function will calculate 1024 fir coefficients and 1536 data, and the data format is fixed point number Q31
 *
 * @param channel: The value ranges from 0 to 7
 * @param x: The entered value has a length of 1536
 * @param y: The output value is 1536 in length
 * @param coeff: Input fir filter coefficient length 1024
 *
 * @return =0: No abnormality
 */
int32_t hal_udsp_fir_c1024_d1536_q31(int32_t channel
                                     , int32_t *x
                                     , int32_t *y
                                     , int32_t *coeff
                                    )
{
    uint32_t addr_hsb, x_addr, y_addr, coef_addr;
    x_addr = (uint32_t)x;
    addr_hsb = x_addr >> 28;
    x_addr = addr_hsb ? x_addr : (x_addr | 0x61000000);
    y_addr = (uint32_t)y;
    addr_hsb = y_addr >> 28;
    y_addr = addr_hsb ? y_addr : (y_addr | 0x61000000);
    coef_addr = (uint32_t)coeff;
    addr_hsb = coef_addr >> 28;
    coef_addr = addr_hsb ? coef_addr : (coef_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)coef_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)coef_addr + 4 * 1024;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)coef_addr;
    DEV_UDSP->CHN[channel].SMF = 4 << 16
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                 | (1024 - 1);
    DEV_UDSP->CHN[channel].CFG0 = DEV_UDSP_CHNn_CFG0_SMR_ENABLE;
    //系数填入buffer 0
    DEV_UDSP->CHN[channel].LMF0 = DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE;
    DEV_UDSP->CHN[channel + 1].SLA = (uint32_t)x_addr;
    DEV_UDSP->CHN[channel + 1].SUA = (uint32_t)x_addr + 4 * (1024 + 1536 - 1);
    DEV_UDSP->CHN[channel + 1].SCA = (uint32_t)x_addr;
    DEV_UDSP->CHN[channel + 1].SMF = (4 << 16) //源偏移地址
                                     | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                     | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                     | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                     | (1024 + 1536 - 2)
                                     ;
    DEV_UDSP->CHN[channel + 1].DLA = (uint32_t)y_addr;
    DEV_UDSP->CHN[channel + 1].DUA = (uint32_t)y_addr + 4 * (1536);
    DEV_UDSP->CHN[channel + 1].DCA = (uint32_t)y_addr;
    DEV_UDSP->CHN[channel + 1].DMF = (4 << 16) //目的地址偏移
                                     | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                     | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                     | (1536 - 1);
    DEV_UDSP->CHN[channel + 1].CFG0 =  DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                       | DEV_UDSP_CHNn_CFG0_FIR_ENABLE
                                       | DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                       | DEV_UDSP_CHNn_CFG0_BUF_SWAP_MASK
                                       ;
    //数据放在buffer 1
    DEV_UDSP->CHN[channel + 1].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF1_SEL_ENABLE
                                       | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF1_SEL_ENABLE
                                       | 10 << 24 //读取buffer1数据地址
                                       | 0 << 20 //写入buff11数据地址
                                       | (10ul) << 28; //计算结果放在buffer1的位置256的倍数，在cgf1 buf swap 置位有效
    DEV_UDSP->CHN[channel + 1].LMF4 = 0xb << 24; //Q31时右移程度
    DEV_UDSP->CHN[channel + 1].LMF1 = 0 << 30 | 0 << 28; //计算结果放在buffer1的位置256的倍数
    DEV_UDSP->CHN[channel + 1].LMF3 = 0;
    DEV_UDSP->CHN[channel + 1].CFG1 = 1024 - 1;
    return 0;
}

/**
 * This function will calculate actan
 *
 * @param channel: The value ranges from 0 to 7
 * @param complex_data: Complex address
 * @param radian: The calculated amplitude results in a fixed address number Q2.13
 * @param nmemb: Number of complex numbers
 *
 * @return =0: No abnormality
 */
int hal_udsp_actan2(uint32_t channel, complex32_number_type *complex_data, int16_t *radian, uint32_t nmemb)
{
    int32_t ret = 0;
    uint32_t addr_hsb, input_addr, output_addr;
    input_addr = (uint32_t)complex_data;
    addr_hsb = input_addr >> 28;
    input_addr = addr_hsb ? input_addr : (input_addr | 0x61000000);
    output_addr = (uint32_t)radian;
    addr_hsb = output_addr >> 28;
    output_addr = addr_hsb ? output_addr : (output_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr + 8 * nmemb;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SMF = (4 << 16) //源偏移地址
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_COMPLEXORLONG64
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_24BIT
                                 | (2 * nmemb - 1)
                                 ;
    DEV_UDSP->CHN[channel].DLA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DUA = (uint32_t)output_addr + 2 * nmemb;
    DEV_UDSP->CHN[channel].DCA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DMF = (2 << 16) //目的地址偏移
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_16BIT
                                 | (nmemb - 1);
    DEV_UDSP->CHN[channel].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                   | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_ATAN_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   ;
    return ret;
}

/**
 * This function will calculate ln
 *
 * @param channel: The value ranges from 0 to 7
 * @param in_data: Enter 64bit integer
 * @param out_data: The calculated amplitude address fixed point number Q10.5
 * @param nmemb: Number of input data
 *
 * @return =0: No abnormality
 */
int32_t hal_udsp_ln(int32_t channel, uint64_t *in_data, int16_t *out_data, uint32_t nmemb)
{
    int32_t ret = 0;
    uint32_t addr_hsb, input_addr, output_addr;
    input_addr = (uint32_t)in_data;
    addr_hsb = input_addr >> 28;
    input_addr = addr_hsb ? input_addr : (input_addr | 0x61000000);
    output_addr = (uint32_t)out_data;
    addr_hsb = output_addr >> 28;
    output_addr = addr_hsb ? output_addr : (output_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr + 8 * nmemb;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SMF = (4 << 16) //ַ
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_COMPLEXORLONG64
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                 | (2 * nmemb - 1)
                                 ;
    DEV_UDSP->CHN[channel].DLA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DUA = (uint32_t)output_addr + 2 * nmemb;
    DEV_UDSP->CHN[channel].DCA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DMF = (2 << 16) //
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_16BIT
                                 | (nmemb - 1);
    DEV_UDSP->CHN[channel].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                   | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_LOG_CAL_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_LOG_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   ;
    return ret;
}

/**
 * This function will calculate sin cos
 *
 * @param channel: The value ranges from 0 to 7
 * @param radian_q29: q2.29 Calculation range (-π, π)
 * @param cosine_q31: The values of con and sin output q31
 * @param nmemb: Number of input data
 *
 * @return =0: No abnormality
 */
int32_t hal_udsp_cosine(uint32_t channel, int32_t *radian_q29, udsp_cosine_number_type *cosine_q31, uint32_t nmemb)
{
    int32_t ret = 0;
    uint32_t addr_hsb, input_addr, output_addr;
    input_addr = (uint32_t)radian_q29;
    addr_hsb = input_addr >> 28;
    input_addr = addr_hsb ? input_addr : (input_addr | 0x61000000);
    output_addr = (uint32_t)cosine_q31;
    addr_hsb = output_addr >> 28;
    output_addr = addr_hsb ? output_addr : (output_addr | 0x61000000);

    for (int i = 0; i < nmemb; i++)
    {
        if (radian_q29[i] > PI_Q29)
        {
            radian_q29[i] = radian_q29[i] - PI_Q29;
            radian_q29[i] = radian_q29[i] - PI_Q29;
        }
        else if (radian_q29[i] < -PI_Q29)
        {
            radian_q29[i] = radian_q29[i] + PI_Q29;
            radian_q29[i] = radian_q29[i] + PI_Q29;
        }
    }

    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr + 4 * nmemb;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SMF = (4 << 16) //ַ
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                 | (nmemb - 1)
                                 ;
    DEV_UDSP->CHN[channel].DLA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DUA = (uint32_t)output_addr + 8 * nmemb;
    DEV_UDSP->CHN[channel].DCA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DMF = (4 << 16) //
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_COMPLEXORLONG64
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                 | (2 * nmemb - 1);
    DEV_UDSP->CHN[channel].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                   | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_COS_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   ;
    return ret;
}

/**
 * This function will calculate sqrt
 *
 * @param channel: The value ranges from 0 to 7
 * @param in_data: Input data
 * @param out_data: Output data q33.30
 * @param nmemb: Number of input data
 *
 * @return =0: No abnormality
 */
int32_t hal_udsp_sqrt(int32_t channel, int64_t *in_data, int64_t *out_data, uint32_t nmemb)
{
    int32_t ret = 0;
    uint32_t addr_hsb, input_addr, output_addr;
    input_addr = (uint32_t)in_data;
    addr_hsb = input_addr >> 28;
    input_addr = addr_hsb ? input_addr : (input_addr | 0x61000000);
    output_addr = (uint32_t)out_data;
    addr_hsb = output_addr >> 28;
    output_addr = addr_hsb ? output_addr : (output_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr + 8 * nmemb;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SMF = (4 << 16) //ַ
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_COMPLEXORLONG64
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                 | (2 * nmemb - 1)
                                 ;
    DEV_UDSP->CHN[channel].DLA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DUA = (uint32_t)output_addr + 8 * nmemb;
    DEV_UDSP->CHN[channel].DCA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DMF = (4 << 16) //
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_COMPLEXORLONG64
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                 | (2 * nmemb - 1);
    DEV_UDSP->CHN[channel].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                   | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SQRT_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   ;
    DEV_UDSP->CHN[channel].CFG1 = nmemb - 1;
    return ret;
}

/**
 * This function will calculate div
 *
 * @param channel: The value ranges from 0 to 7
 * @param in_data: Input range [0.5-1) q31 [0x40000000,0x7fffffff)
 * @param out_data: Output range (1,2), corresponding to (0x80000000,0xffffffff)
 * @param nmemb: Number of input data
 *
 * @return =0: No abnormality
 */
int32_t hal_udsp_div(int32_t channel, int32_t *in_data, uint32_t *out_data, uint32_t nmemb)
{
    int32_t ret = 0;
    uint32_t addr_hsb, input_addr, output_addr;
    input_addr = (uint32_t)in_data;
    addr_hsb = input_addr >> 28;
    input_addr = addr_hsb ? input_addr : (input_addr | 0x61000000);
    output_addr = (uint32_t)out_data;
    addr_hsb = output_addr >> 28;
    output_addr = addr_hsb ? output_addr : (output_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr + 4 * nmemb;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SMF = (4 << 16) //源偏移地址
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                 | (nmemb - 1)
                                 ;
    DEV_UDSP->CHN[channel].DLA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DUA = (uint32_t)output_addr + 4 * nmemb;
    DEV_UDSP->CHN[channel].DCA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DMF = (4 << 16) //目的地址偏移
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                 | (nmemb - 1);
    DEV_UDSP->CHN[channel].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                   | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_DIV_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   ;
    DEV_UDSP->CHN[channel].LMF3 = 0;
//  DEV_UDSP->CHN[channel].LMF4 = 0xb<<28;//[63:31]
    return ret;
}

/**
 * This function will calculate vector multiplication
 *
 * @param channel: The value ranges from 0 to 7
 * @param in_data0: Input vector0
 * @param in_data1: Input vector1
 * @param out_data: Output result address
 * @param nmemb: Number of input data
 *
 * @return =0: No abnormality
 */
int32_t hal_udsp_vmul_32x32(int32_t channel, int32_t *in_data0, int32_t *in_data1, int32_t *out_data, uint32_t nmemb)
{
    uint32_t addr_hsb, input_addr0, input_addr1, output_addr;
    input_addr0 = (uint32_t)in_data0;
    addr_hsb = input_addr0 >> 28;
    input_addr0 = addr_hsb ? input_addr0 : (input_addr0 | 0x61000000);
    input_addr1 = (uint32_t)in_data1;
    addr_hsb = input_addr1 >> 28;
    input_addr1 = addr_hsb ? input_addr1 : (input_addr1 | 0x61000000);
    output_addr = (uint32_t)out_data;
    addr_hsb = output_addr >> 28;
    output_addr = addr_hsb ? output_addr : (output_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr0;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr0 + 4 * nmemb;
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr0;
    DEV_UDSP->CHN[channel].SMF = 4 << 16
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                 | (nmemb - 1);
    DEV_UDSP->CHN[channel].LMF0 = DEV_UDSP_CHNn_LMF0_SMA_WR_BUF1_SEL_ENABLE;
    DEV_UDSP->CHN[channel].CFG0 = DEV_UDSP_CHNn_CFG0_SMR_ENABLE;
    DEV_UDSP->CHN[channel + 1].SLA = (uint32_t)input_addr1;
    DEV_UDSP->CHN[channel + 1].SUA = (uint32_t)input_addr1 + 4 * nmemb;
    DEV_UDSP->CHN[channel + 1].SCA = (uint32_t)input_addr1;
    DEV_UDSP->CHN[channel + 1].SMF = (4 << 16) //源偏移地址
                                     | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                     | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                     | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                     | (nmemb - 1)
                                     ;
    DEV_UDSP->CHN[channel + 1].DLA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel + 1].DUA = (uint32_t)output_addr + 4 * nmemb;
    DEV_UDSP->CHN[channel + 1].DCA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel + 1].DMF = (4 << 16) //目的地址偏移
                                     | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                     | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                     | (nmemb - 1);
    DEV_UDSP->CHN[channel + 1].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                       | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE;
    DEV_UDSP->CHN[channel + 1].CFG0 =  DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                       | DEV_UDSP_CHNn_CFG0_MUL_ENABLE
                                       | DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                       ;
    DEV_UDSP->CHN[channel + 1].LMF4 = 8 << 8; //取[62:31]
    return 0;
}

/**
 * This function will calculate vector times matrix
 *
 * @param channel: The value ranges from 0 to 7
 * @param in_data: The input vector can only be a 1xN vector
 * @param out_data: The output vector is a 1xM vector
 * @param nrow: The number of rows of the matrix
 * @param mcol: The number of columns of the matrix
 *
 * @note  Note that the matrix is loaded by hal_udsp_load_buffer1,
 *        and the loaded matrix must be the transposed matrix
 *
 * @return =0: No abnormality
 */
int32_t hal_udsp_matrix_mul(int32_t channel, int32_t *in_data, int32_t *out_data, uint16_t nrow, uint16_t mcol)
{
    uint32_t addr_hsb, input_addr, output_addr;
    input_addr = (uint32_t)in_data;
    addr_hsb = input_addr >> 28;
    input_addr = addr_hsb ? input_addr : (input_addr | 0x61000000);
    output_addr = (uint32_t)out_data;
    addr_hsb = output_addr >> 28;
    output_addr = addr_hsb ? output_addr : (output_addr | 0x61000000);
    DEV_UDSP->CHN[channel].SLA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SUA = (uint32_t)input_addr + 4 * (nrow);
    DEV_UDSP->CHN[channel].SCA = (uint32_t)input_addr;
    DEV_UDSP->CHN[channel].SMF = (4 << 16) //源偏移地址
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_FORMAT_LOW_24BIT
                                 | DEV_UDSP_CHNn_SMF_SRC_DATA_WIDTH_32BIT
                                 | (nrow - 1)
                                 ;
    DEV_UDSP->CHN[channel].DLA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DUA = (uint32_t)output_addr + 4 * mcol;
    DEV_UDSP->CHN[channel].DCA = (uint32_t)output_addr;
    DEV_UDSP->CHN[channel].DMF = (4 << 16) //目的地址偏移
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_TYPE_REALINT32
                                 | DEV_UDSP_CHNn_DMF_DST_DATA_WIDTH_32BIT
                                 | (mcol - 1);
    uint32_t read_offset = ((1024 - mcol) / 64);
    DEV_UDSP->CHN[channel].LMF0 =  DEV_UDSP_CHNn_LMF0_SMA_WR_BUF0_SEL_ENABLE
                                   | DEV_UDSP_CHNn_LMF0_SMA_RD_BUF0_SEL_ENABLE
                                   | read_offset << 8;
    DEV_UDSP->CHN[channel].LMF4 = 0;//0xc<<20;//取[94:31]
    DEV_UDSP->CHN[channel].LMF1 = 0 << 24 | read_offset << 20;
    DEV_UDSP->CHN[channel].LMF3 = 0;
    DEV_UDSP->CHN[channel].CFG0 =  DEV_UDSP_CHNn_CFG0_SMW_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_MAC_ENABLE
                                   | DEV_UDSP_CHNn_CFG0_SMR_ENABLE
                                   ;
    DEV_UDSP->CHN[channel].CFG1 = (nrow - 1) | (((uint32_t)(mcol - 1)) << 16);
    return 0;
}
