2014-03-03 11:09:26 -08:00
|
|
|
/**
|
2014-01-28 03:41:17 -08:00
|
|
|
*
|
2014-03-03 11:09:26 -08:00
|
|
|
* \section COPYRIGHT
|
2014-01-28 03:41:17 -08:00
|
|
|
*
|
2015-05-08 08:05:40 -07:00
|
|
|
* Copyright 2013-2015 The srsLTE Developers. See the
|
2014-03-03 11:09:26 -08:00
|
|
|
* COPYRIGHT file at the top-level directory of this distribution.
|
|
|
|
*
|
|
|
|
* \section LICENSE
|
|
|
|
*
|
2015-03-20 05:01:08 -07:00
|
|
|
* This file is part of the srsLTE library.
|
2014-03-03 11:09:26 -08:00
|
|
|
*
|
2015-03-20 05:01:08 -07:00
|
|
|
* srsLTE is free software: you can redistribute it and/or modify
|
2015-05-08 08:05:40 -07:00
|
|
|
* it under the terms of the GNU Affero General Public License as
|
2014-03-03 11:09:26 -08:00
|
|
|
* published by the Free Software Foundation, either version 3 of
|
|
|
|
* the License, or (at your option) any later version.
|
|
|
|
*
|
2015-03-20 05:01:08 -07:00
|
|
|
* srsLTE is distributed in the hope that it will be useful,
|
2014-01-28 03:41:17 -08:00
|
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
2015-05-08 08:05:40 -07:00
|
|
|
* GNU Affero General Public License for more details.
|
2014-01-28 03:41:17 -08:00
|
|
|
*
|
2015-05-08 08:05:40 -07:00
|
|
|
* A copy of the GNU Affero General Public License can be found in
|
2014-03-03 11:09:26 -08:00
|
|
|
* the LICENSE file in the top-level directory of this distribution
|
|
|
|
* and at http://www.gnu.org/licenses/.
|
|
|
|
*
|
2014-01-28 03:41:17 -08:00
|
|
|
*/
|
|
|
|
|
2014-03-03 11:09:26 -08:00
|
|
|
|
2014-01-28 03:41:17 -08:00
|
|
|
#include <math.h>
|
|
|
|
#include <complex.h>
|
|
|
|
#include <fftw3.h>
|
|
|
|
#include <string.h>
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
#include "srslte/dft/dft.h"
|
2015-03-18 05:31:13 -07:00
|
|
|
#include "srslte/utils/vector.h"
|
2014-01-28 03:41:17 -08:00
|
|
|
|
2014-06-20 07:35:37 -07:00
|
|
|
#define dft_ceil(a,b) ((a-1)/b+1)
|
|
|
|
#define dft_floor(a,b) (a/b)
|
2014-01-28 03:41:17 -08:00
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
int srslte_dft_plan(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir,
|
|
|
|
srslte_dft_mode_t mode) {
|
|
|
|
if(mode == SRSLTE_DFT_COMPLEX){
|
|
|
|
return srslte_dft_plan_c(plan,dft_points,dir);
|
2014-06-20 07:35:37 -07:00
|
|
|
} else {
|
2015-03-18 11:14:24 -07:00
|
|
|
return srslte_dft_plan_r(plan,dft_points,dir);
|
2014-06-17 02:11:41 -07:00
|
|
|
}
|
|
|
|
return 0;
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 05:41:50 -07:00
|
|
|
static void allocate(srslte_dft_plan_t *plan, int size_in, int size_out, int len) {
|
2014-06-17 02:11:41 -07:00
|
|
|
plan->in = fftwf_malloc(size_in*len);
|
|
|
|
plan->out = fftwf_malloc(size_out*len);
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
int srslte_dft_plan_c(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
|
2014-06-17 02:11:41 -07:00
|
|
|
allocate(plan,sizeof(fftwf_complex),sizeof(fftwf_complex), dft_points);
|
2015-03-18 11:14:24 -07:00
|
|
|
int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_FORWARD : FFTW_BACKWARD;
|
2014-06-17 02:11:41 -07:00
|
|
|
plan->p = fftwf_plan_dft_1d(dft_points, plan->in, plan->out, sign, 0U);
|
|
|
|
if (!plan->p) {
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
plan->size = dft_points;
|
2015-03-18 11:14:24 -07:00
|
|
|
plan->mode = SRSLTE_DFT_COMPLEX;
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->dir = dir;
|
2015-03-18 11:14:24 -07:00
|
|
|
plan->forward = (dir==SRSLTE_DFT_FORWARD)?true:false;
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->mirror = false;
|
|
|
|
plan->db = false;
|
|
|
|
plan->norm = false;
|
2014-06-20 10:55:47 -07:00
|
|
|
plan->dc = false;
|
2014-01-28 03:41:17 -08:00
|
|
|
|
2014-06-17 02:11:41 -07:00
|
|
|
return 0;
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
int srslte_dft_plan_r(srslte_dft_plan_t *plan, const int dft_points, srslte_dft_dir_t dir) {
|
2014-06-17 02:11:41 -07:00
|
|
|
allocate(plan,sizeof(float),sizeof(float), dft_points);
|
2015-03-18 11:14:24 -07:00
|
|
|
int sign = (dir == SRSLTE_DFT_FORWARD) ? FFTW_R2HC : FFTW_HC2R;
|
2014-06-17 02:11:41 -07:00
|
|
|
plan->p = fftwf_plan_r2r_1d(dft_points, plan->in, plan->out, sign, 0U);
|
|
|
|
if (!plan->p) {
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
plan->size = dft_points;
|
2015-03-18 11:14:24 -07:00
|
|
|
plan->mode = SRSLTE_REAL;
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->dir = dir;
|
2015-03-18 11:14:24 -07:00
|
|
|
plan->forward = (dir==SRSLTE_DFT_FORWARD)?true:false;
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->mirror = false;
|
|
|
|
plan->db = false;
|
|
|
|
plan->norm = false;
|
2014-06-20 10:55:47 -07:00
|
|
|
plan->dc = false;
|
2014-01-28 03:41:17 -08:00
|
|
|
|
2014-06-17 02:11:41 -07:00
|
|
|
return 0;
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_plan_set_mirror(srslte_dft_plan_t *plan, bool val){
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->mirror = val;
|
|
|
|
}
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_plan_set_db(srslte_dft_plan_t *plan, bool val){
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->db = val;
|
|
|
|
}
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_plan_set_norm(srslte_dft_plan_t *plan, bool val){
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->norm = val;
|
|
|
|
}
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_plan_set_dc(srslte_dft_plan_t *plan, bool val){
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->dc = val;
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2014-10-17 11:44:01 -07:00
|
|
|
static void copy_pre(uint8_t *dst, uint8_t *src, int size_d, int len,
|
2014-06-20 07:35:37 -07:00
|
|
|
bool forward, bool mirror, bool dc) {
|
|
|
|
int offset = dc?1:0;
|
|
|
|
if(mirror && !forward){
|
|
|
|
int hlen = dft_floor(len,2);
|
2014-06-17 02:11:41 -07:00
|
|
|
memset(dst,0,size_d*offset);
|
2014-06-20 07:35:37 -07:00
|
|
|
memcpy(&dst[size_d*offset], &src[size_d*hlen], size_d*(len-hlen-offset));
|
|
|
|
memcpy(&dst[(len-hlen)*size_d], src, size_d*hlen);
|
|
|
|
} else {
|
|
|
|
memcpy(dst,src,size_d*len);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-10-17 11:44:01 -07:00
|
|
|
static void copy_post(uint8_t *dst, uint8_t *src, int size_d, int len,
|
2014-06-20 07:35:37 -07:00
|
|
|
bool forward, bool mirror, bool dc) {
|
|
|
|
int offset = dc?1:0;
|
|
|
|
if(mirror && forward){
|
|
|
|
int hlen = dft_ceil(len,2);
|
|
|
|
memcpy(dst, &src[size_d*hlen], size_d*(len-hlen));
|
|
|
|
memcpy(&dst[(len-hlen)*size_d], &src[size_d*offset], size_d*(hlen-offset));
|
2014-06-17 02:11:41 -07:00
|
|
|
} else {
|
|
|
|
memcpy(dst,src,size_d*len);
|
|
|
|
}
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_run(srslte_dft_plan_t *plan, void *in, void *out) {
|
|
|
|
if(plan->mode == SRSLTE_DFT_COMPLEX) {
|
|
|
|
srslte_dft_run_c(plan,in,out);
|
2014-06-20 07:35:37 -07:00
|
|
|
} else {
|
2015-03-18 11:14:24 -07:00
|
|
|
srslte_dft_run_r(plan,in,out);
|
2014-06-17 02:11:41 -07:00
|
|
|
}
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_run_c(srslte_dft_plan_t *plan, cf_t *in, cf_t *out) {
|
2014-06-17 02:11:41 -07:00
|
|
|
float norm;
|
|
|
|
int i;
|
|
|
|
fftwf_complex *f_out = plan->out;
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
copy_pre((uint8_t*)plan->in, (uint8_t*)in, sizeof(cf_t), plan->size,
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->forward, plan->mirror, plan->dc);
|
2014-06-17 02:11:41 -07:00
|
|
|
fftwf_execute(plan->p);
|
2014-06-20 07:35:37 -07:00
|
|
|
if (plan->norm) {
|
2014-07-30 03:05:17 -07:00
|
|
|
norm = 1.0/sqrtf(plan->size);
|
2015-03-18 11:14:24 -07:00
|
|
|
srslte_vec_sc_prod_cfc(f_out, norm, f_out, plan->size);
|
2014-06-17 02:11:41 -07:00
|
|
|
}
|
2014-06-20 07:35:37 -07:00
|
|
|
if (plan->db) {
|
2014-06-17 02:11:41 -07:00
|
|
|
for (i=0;i<plan->size;i++) {
|
|
|
|
f_out[i] = 10*log10(f_out[i]);
|
|
|
|
}
|
|
|
|
}
|
2015-03-18 11:14:24 -07:00
|
|
|
copy_post((uint8_t*)out, (uint8_t*)plan->out, sizeof(cf_t), plan->size,
|
2014-06-20 07:35:37 -07:00
|
|
|
plan->forward, plan->mirror, plan->dc);
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_run_r(srslte_dft_plan_t *plan, float *in, float *out) {
|
2014-06-17 02:11:41 -07:00
|
|
|
float norm;
|
|
|
|
int i;
|
|
|
|
int len = plan->size;
|
|
|
|
float *f_out = plan->out;
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
memcpy(plan->in,in,sizeof(float)*plan->size);
|
2014-06-17 02:11:41 -07:00
|
|
|
fftwf_execute(plan->p);
|
2014-06-20 07:35:37 -07:00
|
|
|
if (plan->norm) {
|
2014-07-30 03:05:17 -07:00
|
|
|
norm = 1.0/plan->size;
|
2015-03-18 11:14:24 -07:00
|
|
|
srslte_vec_sc_prod_fff(f_out, norm, f_out, plan->size);
|
2014-06-17 02:11:41 -07:00
|
|
|
}
|
2014-06-20 07:35:37 -07:00
|
|
|
if (plan->db) {
|
2014-06-17 02:11:41 -07:00
|
|
|
for (i=0;i<len;i++) {
|
2014-06-20 07:35:37 -07:00
|
|
|
f_out[i] = 10*log10(f_out[i]);
|
2014-06-17 02:11:41 -07:00
|
|
|
}
|
|
|
|
}
|
2015-03-18 11:14:24 -07:00
|
|
|
memcpy(out,plan->out,sizeof(float)*plan->size);
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
2015-03-18 11:14:24 -07:00
|
|
|
void srslte_dft_plan_free(srslte_dft_plan_t *plan) {
|
2014-06-17 02:11:41 -07:00
|
|
|
if (!plan) return;
|
|
|
|
if (!plan->size) return;
|
|
|
|
if (plan->in) fftwf_free(plan->in);
|
|
|
|
if (plan->out) fftwf_free(plan->out);
|
|
|
|
if (plan->p) fftwf_destroy_plan(plan->p);
|
2015-03-18 05:41:50 -07:00
|
|
|
bzero(plan, sizeof(srslte_dft_plan_t));
|
2014-01-28 03:41:17 -08:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|