update add sh and fft s

hardware/signal_processing/add.vhd

@@ -1,12 +1,13 @@
-library ieee;
+    library ieee;
         use ieee.std_logic_1164.all;
         use ieee.numeric_std.all;
 
-library work;
+    library work;
         use work.reg32.all;
         use work.task.all;
 
-entity add is
+
+    entity add is
         port (
             clk : in std_logic;
             reset : in std_logic;
@@ -23,14 +24,63 @@ entity add is
             signal_write : out std_logic;
             signal_writedata : out std_logic_vector( 31 downto 0 )
         );
-end entity add;
+    end entity add;
+
+    architecture rtl of add is
+
+	-- State Machine für Ansteuerung des ADD-IP Cores.
+	type AddState is (
+	ADD_STATE_IDLE,
+	ADD_STATE_CALCULATE,
+	ADD_STATE_WRITE,
+	ADD_STATE_DONE
+	);
+
+	-- Instanziierung float_add component
+    	component float_add
+    	port (
+    		clk : in std_logic;
+    		reset : in std_logic;
+    		start : in std_logic;
+    		A : in std_logic_vector( 31 downto 0 );
+    		B : in std_logic_vector( 31 downto 0 );
+    		done : out std_logic;
+    		sum : out std_logic_vector( 31 downto 0 )
+    		);
+    	end component float_add;
 
-architecture rtl of add is
         signal current_task_state : work.task.State;
         signal next_task_state : work.task.State;
         signal index : integer range 0 to work.task.STREAM_LEN;
     	
-begin
+	-- Eigene Steuersignale.
+    	signal value_add_start : std_logic;
+    	signal value_add_A : std_logic_vector( 31 downto 0 );
+    	signal value_add_B : std_logic_vector( 31 downto 0 );
+    	signal value_add_done :  std_logic;
+    	signal value_add_sum : std_logic_vector( 31 downto 0 );
+
+	signal add_state : AddState := ADD_STATE_IDLE;
+    	signal flag_index : bit;
+
+
+    begin
+
+    	float_adder : float_add
+    	port map (
+    		clk => clk,
+    		reset => reset,
+    		start => value_add_start,
+		-- Wert von A wird in value_add_A geschrieben.
+    		A => value_add_A,		
+		-- Wert von B wird in value_add_B geschrieben.
+    		B => value_add_B,	
+		-- Signal wenn Addition fertig berechnet.
+    		done => value_add_done,
+		-- Summe der Addition wird in write_data geschrieben
+    		sum => value_add_sum		
+    	);
+
         task_state_transitions : process ( current_task_state, task_start, index ) is
         begin
             next_task_state <= current_task_state;
@@ -60,18 +110,67 @@ begin
                 case next_task_state is
                 when work.task.TASK_IDLE =>
                     index <= 0;
-                signal_write <= '0';
+
                 when work.task.TASK_RUNNING => 
+    				if ( flag_index = '1' ) then
     					index <= index + 1;
-                signal_write <= '1';
+    				end if;
-                signal_writedata <= ( others => '0' );
+
                 when work.task.TASK_DONE =>
                     index <= 0;
-                signal_write <= '0';
+
                 end case;
             end if;
         end process sync;
 
+    	add : process (clk, reset) is
+    	begin
+		-- Bei Reset alle Signale zurücksetzen
+    		if ( reset = '1' ) then
+                signal_a_read <= '0';
+    			signal_b_read <= '0';
+    			signal_write <= '0';
+    			signal_writedata <= ( others => '0');
+    			value_add_start <= '0';
+    			value_add_A <= ( others => '0');
+    			value_add_B <= ( others => '0');
+            	-- Für jeden Takt add_state Zustandsmaschine aufrufen.
+		elsif ( rising_edge( clk ) ) then
+    			case add_state is
+			-- IDLE STATE: Wenn Task im state TASK_RUNNING ist, soll add_state starten.
+    			when ADD_STATE_IDLE =>
+    				if ( current_task_state = work.task.TASK_RUNNING ) then
+    					add_state <= ADD_STATE_CALCULATE;
+    				end if;
+
+			-- CALCULATE: Read signale instanziieren und lesen
+    			when ADD_STATE_CALCULATE =>
+    				signal_a_read <= '1';
+    				signal_b_read <= '1';
+    				value_add_start <= '1';
+    				value_add_A <= signal_a_readdata;
+    				value_add_B <= signal_b_readdata;
+    				if ( value_add_done = '1' ) then
+    					add_state <= ADD_STATE_WRITE;
+    				end if;
+
+			-- WRITE: 
+    			when ADD_STATE_WRITE =>
+    				signal_write <= '1';
+    				signal_writedata <= value_add_sum;
+    				value_add_start <= '0';
+    				signal_a_read <= '0';
+    				signal_b_read <= '0';
+    				flag_index <= '1';
+    				add_state <= ADD_STATE_DONE;
+    			when ADD_STATE_DONE =>
+    				signal_write <= '0';
+    				flag_index <= '0';
+    				add_state <= ADD_STATE_IDLE;
+    			end case;
+    		end if;
+    	end process add;
+
         task_state <= current_task_state;
 
-end architecture rtl;
+    end architecture rtl;

hardware/signal_processing/fft.vhd

@@ -1,25 +1,25 @@
-------------------------------------------------------------------------
+    ------------------------------------------------------------------------
--- fft
+    -- fft
---
+    --
--- calculation of FFT magnitude
+    -- calculation of FFT magnitude
---
+    --
--- Inputs:
+    -- Inputs:
--- 32-Bit Floating Point number in range +-16 expected (loaded from FIFO)
+    -- 32-Bit Floating Point number in range +-16 expected (loaded from FIFO)
---
+    --
--- Outputs
+    -- Outputs
--- 32-Bit Floating Point number in range +-16 calculated (stored in FIFO)
+    -- 32-Bit Floating Point number in range +-16 calculated (stored in FIFO)
---
+    --
------------------------------------------------------------------------
+    -----------------------------------------------------------------------
-library ieee;
+    library ieee;
         use ieee.std_logic_1164.all;
         use ieee.numeric_std.all;
 
-library work;
+    library work;
         use work.reg32.all;
         use work.task.all;
     	 use work.float.all;
 
-entity fft is
+    entity fft is
       generic (
 
         -- input data width of real/img part
@@ -42,15 +42,94 @@ entity fft is
             signal_write : out std_logic;
             signal_writedata : out std_logic_vector( 31 downto 0 )
         );
-end entity fft;
+    end entity fft;
 
-architecture rtl of fft is
+    architecture rtl of fft is
 
         signal current_task_state : work.task.State;
         signal next_task_state : work.task.State;
         signal index : integer range 0 to work.task.STREAM_LEN;
 
-begin
+    component fftmain is
+        port(
+    	clock: in std_logic;  --  Master Clock
+        	reset: in std_logic;  --  Active High Asynchronous Reset
+        	di_en: in std_logic; --  Input Data Enable
+        	di_re: in std_logic_vector(input_data_width-1 downto 0); --   Input Data (Real)
+       	di_im: in std_logic_vector(input_data_width-1 downto 0); --  Input Data (Imag)
+       	do_en: out std_logic; --  Output Data Enable
+    	do_re: out std_logic_vector(output_data_width-1 downto 0); --  Output Data (Real)
+            do_im: out std_logic_vector(output_data_width-1 downto 0) --    Output Data (Imag)
+    	);
+    end component;
+    --Initialisierung der weiteren Ablaufstruktur
+    type FFTState is (
+    	FFTIdle,
+    	FFTRead,
+    	FFTWait,	
+    	MAGRead,
+    	MAGStore 
+    );
+
+    --Signale fuer die Zustandsmaschine
+    signal current_fft_state : FFTState;
+    signal next_fft_state : FFTState;
+    --signal fifo_in     	: unsigned(31 downto 0);
+    constant B		: signed(7 downto 0) := "00000100";
+    --signal C     	 	: unsigned(31 downto 0);
+    --signal D     	 	: unsigned(31 downto 0);
+    --signal E     	 	: unsigned(31 downto 0);
+    --signal F     	 	: unsigned(31 downto 0);
+    signal read_index 	: integer range 0 to work.task.STREAM_LEN +100;
+    signal fft_index 	: integer range 0 to work.task.STREAM_LEN;
+    signal result 		: std_logic_vector ( 31 downto 0 );
+    signal input_valid	: std_logic;
+    signal input_re 	: std_logic_vector( 31 downto 0 ); -- in Fixpoint
+    signal input_im 	: std_logic_vector( 31 downto 0 ); -- in Fixpoint
+    signal output_valid 	: std_logic;
+    signal output_magnitude : std_logic_vector( 31 downto 0 );
+    signal cnt		: integer range 0 to work.task.STREAM_LEN;
+    signal di_en		: std_logic; --  Input Data Enable
+    signal di_re		: std_logic_vector(31 downto 0); --   Input Data (Real)
+    signal di_im		: std_logic_vector(31 downto 0); --  Input Data (Imag)
+    signal do_en		: std_logic; --  Output Data Enable
+    signal do_re		: std_logic_vector(31 downto 0); --  Output Data (Real)
+    signal do_im		: std_logic_vector(31 downto 0); --    Output Data (Imag)
+
+
+    begin
+
+    	--Port Zuweisung
+    	c_float_fft: entity work.fft_magnitude_calc 
+    	    PORT MAP (
+    		clk    => clk,
+    		reset  => reset,
+    			  
+    		input_valid => input_valid,		  
+    		input_re => input_re, -- in Fixpoint
+    		input_im => input_im, -- in Fixpoint
+    		  
+    		output_valid => output_valid,
+    		output_magnitude => output_magnitude
+    	    );
+    	u_fft : fftmain
+    		port map (
+    		clock => clk,
+    		reset => reset,
+
+    		di_en => di_en,
+    		di_re => di_re,
+    		di_im => di_im,
+
+    		do_en => do_en,
+    		do_re => do_re,
+    		do_im => do_im
+    		);
+    	
+
+
+
+
         task_state_transitions : process ( current_task_state, task_start, index ) is
         begin
             next_task_state <= current_task_state;
@@ -60,7 +139,7 @@ begin
                         next_task_state <= work.task.TASK_RUNNING;	
                     end if;
                 when work.task.TASK_RUNNING =>
-                if ( index = work.task.STREAM_LEN - 1 ) then
+                    if ( index = (work.task.STREAM_LEN - 1) ) then
                         next_task_state <= work.task.TASK_DONE;
                     end if;
                 when work.task.TASK_DONE =>
@@ -70,11 +149,70 @@ begin
             end case;
         end process task_state_transitions;
 
+    ----------------------------------------------------------------------
+    	--FFT Statemachine
+    	fft_state_transitions : process ( all ) is
+    	begin
+    		next_fft_state <= current_fft_state;
+    		case current_fft_state is
+    			when FFTIdle =>
+    				if ( current_task_state = work.task.TASK_RUNNING ) then -- Weiterschaltbedingung					
+    					next_fft_state <= FFTRead;	
+    				end if;
+
+    			when FFTRead =>
+    				if ( fft_index = work.task.STREAM_LEN ) then
+                     			next_fft_state <= FFTWait;
+                    		end if;	
+     				
+    			when FFTWait =>
+    				if ( do_en = '1') then
+    					next_fft_state <= MAGRead;
+    				end if;
+    				
+    			when MAGRead =>
+    				if ( output_valid = '1' ) then	-- Weiterschaltbedingung
+    					next_fft_state <= MAGStore;
+    				end if;
+    				
+    			when MAGStore =>
+    				if ( cnt = (work.task.STREAM_LEN - 1)) then
+    					next_fft_state <= FFTIdle;
+    				end if;
+    			
+    			
+    		end case;
+    	end process fft_state_transitions;
+    ----------------------------------------------------------------------
+
+    		
         sync : process ( clk, reset ) is
+        variable fifo_in : signed(31 downto 0);
+        variable fifo_in2 : signed(31 downto 0);
+        variable mag_out : signed(31 downto 0);
         begin
             if ( reset = '1' ) then
                 current_task_state <= work.task.TASK_IDLE;
                 index <= 0;
+    	    read_index <= 0;
+    	    fft_index <= 0;
+    	    cnt <= 0;
+    	    signal_write <= '0';
+    	    signal_read <= '0';
+    	    input_valid <= '0';
+    	    fifo_in := (others => '0');
+    	    fifo_in2 := (others => '0');
+    	    mag_out := (others => '0');
+    	--    C <= (others => '0');
+    	--    D <= (others => '0');
+    	  --  E <= (others => '0');
+    	    --F <= (others => '0');
+    	    input_re <= (others => '0');
+    	    input_im <= (others => '0');
+    	    signal_writedata <= (others => '0');
+    	    di_en <= '0';
+    	    di_re <= (others => '0');
+                di_im <= (others => '0');
             elsif ( rising_edge( clk ) ) then
                 current_task_state <= next_task_state;
                 case next_task_state is
@@ -82,16 +220,77 @@ begin
                     index <= 0;
                     signal_write <= '0';
                 when work.task.TASK_RUNNING =>
-                index <= index + 1;
+                   -- index <= index + 1;			--Index wird hier hochgezählt, muss in FFT State gemacht werden
-                signal_write <= '1';
+                   -- signal_write <= '1';
-                signal_writedata <= ( others => '0' );
+                   -- signal_writedata <= ( others => '0' );
                 when work.task.TASK_DONE =>
                     index <= 0;
                     signal_write <= '0';
                 end case;
+
+    ----------------------------------------------------------------------
+    	--Output Statemachine
+
+    	    current_fft_state <= next_fft_state;
+    	    signal_write <= '0';
+    	    signal_read <= '0';
+    	    input_valid <= '0';
+    	    di_en <= '0';
+    	    case next_fft_state is
+    		when FFTIdle =>
+    		
+    		when FFTRead =>
+    			di_en <= '1';
+    			signal_read <= '1';
+    			--fifo_in <= signal_readdata(31 downto 0);
+    			if(signal_readdata(30 downto 23) /= "00000000") then
+    				fifo_in(31) := signal_readdata(31);
+    				fifo_in(30 downto 23) := signed(signal_readdata(30 downto 23)) - 4;
+    				fifo_in(22 downto 0) := signed(signal_readdata(22 downto 0));
+    				--fifo_in2 := (fifo_in(31) & (signed(fifo_in(30 downto 23)) - 4) & (signed(fifo_in(22 downto 0))));
+    			
+    			end if;
+    		
+    			di_re <= to_fixed(std_logic_vector(fifo_in));
+    			di_im <= (others => '0');
+    			fft_index <= fft_index +1;
+    		when FFTWait =>
+    			fft_index <= 0;
+    		when MAGRead =>
+    			--D <= do_im(31) & ( unsigned(do_im(30 downto 23)) - B ) & unsigned(do_im(22 downto 0));
+    			input_valid <= '1';
+    			input_re <= do_re;
+    			input_im <= do_im;
+    			read_index <= read_index + 1;
+    		when MAGStore =>
+    			--read
+    			if(read_index <= work.task.STREAM_LEN) then
+    				--A <= do_re(31) & ( unsigned(do_re(30 downto 23)) - B ) & unsigned(do_re(22 downto 0));
+
+    				--D <= do_im(31) & ( unsigned(do_im(30 downto 23)) - B ) & unsigned(do_im(22 downto 0));
+    			
+    				signal_read <= '1';
+    				input_valid <= '1';
+    				input_re <= do_re;
+    				input_im <= do_im;
+    				read_index <= read_index + 1;
+
+    			end if;
+    			--store
+    			signal_write <= '1';
+    			mag_out(31) := output_magnitude(31) ;
+    			mag_out(30 downto 23) := signed(output_magnitude(30 downto 23)) + 4; 
+    			mag_out(22 downto 0) := signed(output_magnitude(22 downto 0));
+    			signal_writedata <= to_float(std_logic_vector(mag_out));
+    			index <= index + 1;
+    			cnt <= cnt + 1;
+    	    end case;
+    			
+
+    ----------------------------------------------------------------------
             end if;
         end process sync;
 
         task_state <= current_task_state;
 
-end architecture rtl;
+    end architecture rtl;

software/signal_processing/add.c

@@ -4,7 +4,21 @@
 
 int task_add_run( void * task ) {
 
-    // TODO
+	add_config * config = (add_config*)task;
+
+	for (int i = 0; i < DATA_CHANNEL_DEPTH; i++)
+	{
+ 		float value_1;
+    		float value_2;	
+		float_word value_3;
+
+		data_channel_read(config->sources[0], &value_1);
+		data_channel_read(config->sources[1], &value_2);
+
+		value_3.value = value_1 + value_2;
+
+		data_channel_write(config->sink, (uint32_t)value_3.word);
+	}
 
     	return 0;
 }

software/signal_processing/fft.c

@@ -2,11 +2,63 @@
 #include "system/data_channel.h"
 #include "system/Complex.h"
 #include "system/float_word.h"
+#include "math.h"
+#include "complex.h"
+
+void cooley_tukey(float complex *X, const uint32_t N) {
+    if (N >= 2) {
+        float complex tmp [N / 2];
+        for (uint32_t i = 0; i < N / 2; ++i) {
+            tmp[i] = X[2*i + 1];
+            X[i] = X[2*i];
+        }
+        for (uint32_t i = 0; i < N / 2; ++i) {
+            X[i + N / 2] = tmp[i];
+        }
+
+        cooley_tukey(X, N / 2);
+        cooley_tukey(X + N / 2, N / 2);
+
+        for (uint32_t i = 0; i < N / 2; ++i) {
+            X[i + N / 2] = X[i] - cexp(-2.0 * I * M_PI * (float)i / (float)N) * X[i + N / 2];
+            X[i] -= (X[i + N / 2]-X[i]);
+        }
+    }
+}
+
+
 
 int task_fft_run( void * task ) {
 
-    // TODO
+    fft_config * config = (fft_config*)task;
+
+    Complex input[DATA_CHANNEL_DEPTH];
+
+    // Daten lesen
+    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
+	float a;
+	data_channel_read(config->base.sources[0], (uint32_t *)&a);
+	input[i] = (Complex){a, 0}; 
+    }
+
+    // FFT berechnen
+    cooley_tukey(input, DATA_CHANNEL_DEPTH);
+
+    // Ergebnisse normalisieren
+    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
+        input[i].re /= DATA_CHANNEL_DEPTH/2;
+        input[i].im /= DATA_CHANNEL_DEPTH/2;
+    }
+	input[0].re = input[0].re + 0.1403;
+
+    // Ergebnisse in data channel schreiben
+    for (uint32_t i = 0; i < DATA_CHANNEL_DEPTH; ++i) {
+	float_word c;
+	c.value = complex_abs(&input[i]);
+	data_channel_write(config->base.sink, c.word);
+    }
 
     return 0;
+
 }