diff --git a/Makefile b/Makefile
index 899f3e6..87e719f 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 EXE = fastsa
-SOURCES = main.cpp
+SOURCES = fft.cpp main.cpp
 SOURCES += $(IMGUI_DIR)/imgui.cpp $(IMGUI_DIR)/imgui_demo.cpp $(IMGUI_DIR)/imgui_draw.cpp $(IMGUI_DIR)/imgui_tables.cpp $(IMGUI_DIR)/imgui_widgets.cpp
 SOURCES += $(IMGUI_DIR)/backends/imgui_impl_glfw.cpp $(IMGUI_DIR)/backends/imgui_impl_opengl3.cpp
 OBJS = $(addsuffix .o, $(basename $(notdir $(SOURCES))))
diff --git a/fft.cpp b/fft.cpp
new file mode 100644
index 0000000..e4ca68e
--- /dev/null
+++ b/fft.cpp
@@ -0,0 +1,23 @@
+#include <vector>
+#include <complex>
+
+#define POCKETFFT_CACHE_SIZE (16*1024*1024)
+#define POCKETFFT_NO_MULTITHREADING  // MT doesn't seem to help at small sizes
+#include <pocketfft_hdronly.h>
+
+void fft_mag(std::complex<float>* in, float* out, size_t len)
+{
+    pocketfft::shape_t shape{len};
+    pocketfft::stride_t stride(1);
+    stride[0] = sizeof(std::complex<float>);
+    pocketfft::shape_t axes;
+    axes.push_back(0);
+    std::vector<std::complex<float>> out_rev(len);
+
+    pocketfft::c2c(shape, stride, stride, axes, pocketfft::FORWARD, in, out_rev.data(), 1.0f);
+    // negative frequencies first
+    for(size_t i=0;i<len/2;i++)
+        out[i] = abs(out_rev[i + len/2]);
+    for(size_t i=0;i<len/2;i++)
+        out[i + len/2] = abs(out_rev[i]);
+}
diff --git a/main.cpp b/main.cpp
index 24bceea..c9a372e 100644
--- a/main.cpp
+++ b/main.cpp
@@ -18,12 +18,12 @@
 
 #include <libbladeRF.h>
 
-#define POCKETFFT_CACHE_SIZE (16*1024*1024)
-#define POCKETFFT_NO_MULTITHREADING  // MT doesn't seem to help at small sizes
-#include <pocketfft_hdronly.h>
-
 namespace net = std::experimental::net;
 
+
+void fft_mag(std::complex<float>* in, float* out, size_t len);
+
+
 static std::atomic<bool> terminate_dsp;
 
 struct bladerf* bladerf_dev;
@@ -51,12 +51,6 @@ static void dsp_thread() {
     size_t len = 16384;
     std::vector<std::complex<int16_t>> frames(len);
     std::vector<std::complex<float>> frames_f(len);
-    pocketfft::shape_t shape{len};
-    pocketfft::stride_t stride(1);
-    stride[0] = sizeof(std::complex<float>);
-    pocketfft::shape_t axes;
-    axes.push_back(0);
-    std::vector<std::complex<float>> frames_ft_rev(len);
     std::vector<float> frames_mag(len);
 
     int iterations = 0;
@@ -69,15 +63,7 @@ static void dsp_thread() {
             break;
         }
 
-        // FFT
-        for(size_t i=0;i<len;i++)
-            frames_f[i] = frames[i];
-        pocketfft::c2c(shape, stride, stride, axes, pocketfft::FORWARD, frames_f.data(), frames_ft_rev.data(), 1.0f);
-        // negative frequencies first
-        for(size_t i=0;i<len/2;i++)
-            frames_mag[i] = abs(frames_ft_rev[i + len/2]);
-        for(size_t i=0;i<len/2;i++)
-            frames_mag[i + len/2] = abs(frames_ft_rev[i]);
+        fft_mag(frames_f.data(), frames_mag.data(), len);
 
         // stabilize laser
         bool tick = false;