1 | library IEEE;
|
2 | use IEEE.STD_LOGIC_1164.ALL;
|
3 | use IEEE.NUMERIC_STD.ALL;
|
4 |
|
5 | --------------------------------------------------------------------
|
6 | entity CALC is
|
7 | --------------------------------------------------------------------
|
8 | Port ( ETrigger_C : in STD_LOGIC;
|
9 | SWDTTrigger_C : in STD_LOGIC;
|
10 | clk_C : in STD_LOGIC;
|
11 | Dir_C : in STD_LOGIC;
|
12 | Resetr_C : in STD_LOGIC;
|
13 | ResetN_C : in STD_LOGIC;
|
14 | ResetMOD_C : in STD_LOGIC;
|
15 | EnableMOD_C : in STD_LOGIC;
|
16 | EnableAdapt_C : in STD_LOGIC;
|
17 | AzimuthType_C : in STD_LOGIC;
|
18 | WE_C : in STD_LOGIC;
|
19 | DIN_C : in STD_LOGIC;
|
20 | ADDRW_C : in STD_LOGIC_VECTOR (12 downto 0);
|
21 | CounterValue_C : in STD_LOGIC_VECTOR (16 downto 0);
|
22 | rmax_C : in UNSIGNED(15 downto 0);
|
23 | MODref_C : in UNSIGNED(31 downto 0);
|
24 | NT_C : in SIGNED(19 downto 0);
|
25 | h_times_g_C : in UNSIGNED(19 downto 0);
|
26 |
|
27 | Reset_C : out STD_LOGIC;
|
28 | Busy_C : out STD_LOGIC;
|
29 | DirOut_C : out STD_LOGIC;
|
30 | N_C : out SIGNED(19 downto 0);
|
31 | dN_C : out UNSIGNED(15 downto 0);
|
32 | ND_C : out UNSIGNED(15 downto 0);
|
33 | r_C : out UNSIGNED(15 downto 0);
|
34 | MOD_C : out UNSIGNED(31 downto 0));
|
35 | end CALC;
|
36 | --------------------------------------------------------------------
|
37 |
|
38 | --------------------------------------------------------------------
|
39 | architecture Behavioral of CALC is
|
40 | --------------------------------------------------------------------
|
41 | component DIVROM is
|
42 | Port ( Clk_R : in STD_LOGIC;
|
43 | count_R : in STD_LOGIC_VECTOR(15 downto 0);
|
44 | dN_R : out STD_LOGIC_VECTOR(15 downto 0));
|
45 | end component;
|
46 |
|
47 | component PSTableRAM is
|
48 | Port ( Clk_PS : in STD_LOGIC;
|
49 | RE_PS : in STD_LOGIC;
|
50 | WE_PS : in STD_LOGIC;
|
51 | ADDRR_PS : in STD_LOGIC_VECTOR(12 downto 0);
|
52 | ADDRW_PS : in STD_LOGIC_VECTOR(12 downto 0);
|
53 | DIN_PS : in STD_LOGIC;
|
54 | DOUT_PS : out STD_LOGIC);
|
55 | end component;
|
56 |
|
57 | component Multiplier32x32 is
|
58 | Port ( In1Mult_M : in STD_LOGIC_VECTOR (31 downto 0);
|
59 | In2Mult_M : in STD_LOGIC_VECTOR (31 downto 0);
|
60 | OutMult_M : out STD_LOGIC_VECTOR (63 downto 0));
|
61 | end component;
|
62 | ---------------------------------------------------------------------
|
63 | type state is (Init, Idle, WaitForPSRAM, prepCalc_N, Calc_N, Calc_dN, Calc_dNsquare, prepCalc_MOD, Calc_MOD, prepCalc_ND, Calc_ND);
|
64 | signal s : state;
|
65 | signal countervalue : STD_LOGIC_VECTOR(15 downto 0);
|
66 | signal Esr : STD_LOGIC_VECTOR(2 downto 0);
|
67 | signal SWDTsr : STD_LOGIC_VECTOR(2 downto 0);
|
68 | signal dN_ROM : STD_LOGIC_VECTOR(15 downto 0);
|
69 | signal RE : STD_LOGIC;
|
70 | signal ADDRR : STD_LOGIC_VECTOR(12 downto 0);
|
71 | signal DOUT : STD_LOGIC;
|
72 | signal In1Mult : STD_LOGIC_VECTOR(31 downto 0);
|
73 | signal In2Mult : STD_LOGIC_VECTOR(31 downto 0);
|
74 | signal OutMult : STD_LOGIC_VECTOR(63 downto 0);
|
75 | signal DOUTreg : STD_LOGIC_VECTOR(2 downto 0);
|
76 | signal rdir : STD_LOGIC_VECTOR(1 downto 0);
|
77 | signal dir : STD_LOGIC_VECTOR(3 downto 0);
|
78 | ---------------------------------------------------------------------
|
79 | begin
|
80 |
|
81 | instDIVROM : DIVROM
|
82 | Port Map ( Clk_R => clk_c,
|
83 | count_R => countervalue,
|
84 | dN_R => dN_ROM);
|
85 |
|
86 | instPSTableRAM : PSTableRAM
|
87 | Port Map ( Clk_PS => clk_C,
|
88 | RE_PS => RE,
|
89 | WE_PS => WE_C,
|
90 | ADDRR_PS => ADDRR,
|
91 | ADDRW_PS => ADDRW_C,
|
92 | DIN_PS => DIN_C,
|
93 | DOUT_PS => DOUT);
|
94 |
|
95 | instMultiplier32x32 : Multiplier32x32
|
96 | Port Map ( In1Mult_M => In1Mult,
|
97 | In2Mult_M => In2Mult,
|
98 | OutMult_M => OutMult);
|
99 |
|
100 | ---------------------------------------------------------------------
|
101 | FSM : process
|
102 | ---------------------------------------------------------------------
|
103 | type NTArray is array (3 downto 0) of SIGNED(19 downto 0);
|
104 | variable NT : NTArray;
|
105 | variable dNsquare : UNSIGNED(31 downto 0);
|
106 | variable ND64bitfp : UNSIGNED(63 downto 0);
|
107 | variable r : UNSIGNED(15 downto 0):=to_unsigned(1,16);
|
108 | variable N : SIGNED(19 downto 0):=to_signed(0,20);
|
109 | variable dN : UNSIGNED(15 downto 0):=to_unsigned(0,16);
|
110 | variable ND : UNSIGNED(15 downto 0):=to_unsigned(0,16);
|
111 | --variable dir : STD_LOGIC_VECTOR(3 downto 0);
|
112 | variable Modifier : UNSIGNED(31 downto 0);
|
113 | variable Modifier64b : UNSIGNED(63 downto 0);
|
114 | variable absDeltaN : UNSIGNED(19 downto 0);
|
115 | variable factor : UNSIGNED(7 downto 0);
|
116 |
|
117 | begin
|
118 |
|
119 | wait until rising_edge(clk_c);
|
120 |
|
121 | If Resetr_C = '1' then r := to_unsigned(1,16); else r := r; end if;
|
122 | If ResetN_C = '1' then N := to_signed(0,20); else N := N; end if;
|
123 | If ResetMOD_C = '1' then Modifier := MODref_C; else Modifier := Modifier; end if;
|
124 |
|
125 | -------------------------------------------------------
|
126 | case s is
|
127 | -------------------------------------------------------
|
128 | ----------------------------------------------------
|
129 | when Init =>
|
130 | ----------------------------------------------------
|
131 | Modifier := MODref_C;
|
132 | s <= Idle;
|
133 |
|
134 | ----------------------------------------------------
|
135 | when Idle =>
|
136 | ----------------------------------------------------
|
137 | if Esr(2 downto 1)="01" then
|
138 | Busy_C <= '1';
|
139 | Reset_C <= '0';
|
140 | If Dir_C='1' then r := r + 1; else r := r - 1; end if;
|
141 | If r = 0 then r := rmax_C; elsif r > rmax_C then r := r - rmax_C; end if;
|
142 | rdir <= rdir(0)&Dir_C;
|
143 | RE <= '1';
|
144 | ADDRR <= std_logic_vector(r(12 downto 0)-1);
|
145 | s <= WaitForPSRAM;
|
146 | elsif SWDTsr(2 downto 1)="01" then
|
147 | Busy_C <= '1';
|
148 | Reset_C <= '1';
|
149 | dN := to_unsigned(0,16);
|
150 | ND := to_unsigned(0,16);
|
151 | RE <= '0';
|
152 | s <= Idle;
|
153 | else
|
154 | Busy_C <= '0';
|
155 | Reset_C <= '0';
|
156 | RE <= '0';
|
157 | s <= Idle;
|
158 | end if;
|
159 |
|
160 | ------------------------------------------------------
|
161 | when WaitForPSRAM =>
|
162 | ------------------------------------------------------
|
163 | s <= prepCalc_N;
|
164 |
|
165 | ------------------------------------------------------
|
166 | when prepCalc_N =>
|
167 | ------------------------------------------------------
|
168 | DOUTreg <= DOUTreg(1 downto 0)&DOUT;
|
169 | s <= Calc_N;
|
170 |
|
171 | ------------------------------------------------------
|
172 | when Calc_N =>
|
173 | ------------------------------------------------------
|
174 | If DOUT='1' OR (DOUTreg="010" AND (rdir(0)/=rdir(1))) then
|
175 | If Dir_C='1' then N := N + 1; else N := N - 1; end if;
|
176 | If AzimuthType_C = '1' then
|
177 | If N < 0 then N := N + signed(h_times_g_C);
|
178 | elsif N > signed(h_times_g_C-1) then N := N - signed(h_times_g_C);
|
179 | end if;
|
180 | end if;
|
181 | countervalue <= CounterValue_C(15 downto 0);
|
182 | dir(3 downto 0) <= dir(2 downto 0)&Dir_C;
|
183 | --dir(0) := Dir_C;
|
184 | NT(3 downto 0):=NT(2 downto 0)&NT_C;
|
185 | --NT(0) := NT_C;
|
186 | Reset_C <= '1';
|
187 | s <= Calc_dN;
|
188 | else
|
189 | s <= Idle;
|
190 | end if;
|
191 | Busy_C <= '1';
|
192 |
|
193 | ------------------------------------------------------
|
194 | when Calc_dN =>
|
195 | ------------------------------------------------------
|
196 | if dir(0)=dir(1) then
|
197 | dN := unsigned(dN_ROM);
|
198 | In1Mult <= std_logic_vector("0000000000000000"&dN);
|
199 | In2Mult <= std_logic_vector("0000000000000000"&dN);
|
200 | s <= Calc_dNsquare;
|
201 | else
|
202 | dN := to_unsigned(0,16);
|
203 | ND := to_unsigned(0,16);
|
204 | s <= Idle;
|
205 | end if;
|
206 | Busy_C <= '1';
|
207 | Reset_C <= '1';
|
208 |
|
209 | -------------------------------------------------------
|
210 | when Calc_dNsquare =>
|
211 | -------------------------------------------------------
|
212 | dNsquare := unsigned(OutMult(31 downto 0));
|
213 | If EnableAdapt_C = '1' and NT(3)=NT(2) and NT(2)=NT(1) and NT(1)=NT(0) then
|
214 | s <= prepCalc_MOD;
|
215 | else
|
216 | s <= prepCalc_ND;
|
217 | end if;
|
218 | Busy_C <= '1';
|
219 | Reset_C <= '1';
|
220 |
|
221 | -------------------------------------------------------
|
222 | when prepCalc_MOD =>
|
223 | -------------------------------------------------------
|
224 | absDeltaN := unsigned(abs(NT_C - N));
|
225 | If ((dir = "1101") or (dir = "0010")) then --(dir(3)=dir(2) and dir(2)/=dir(1) and dir(1)/=dir(0)) then -- reduce Modifier
|
226 | If absDeltaN > to_unsigned(64,20) then
|
227 | factor := to_unsigned(64,8);
|
228 | else
|
229 | factor := 128 - absDeltaN(7 downto 0);
|
230 | end if;
|
231 | elsif (dir(3)=dir(2) and dir(2)/=dir(1) and dir(1)=dir(0)) then -- enlarge Modifier
|
232 | If absDeltaN > to_unsigned(64,20) then
|
233 | factor := to_unsigned(255,8);
|
234 | else
|
235 | factor := 128 + absDeltaN(7 downto 0);
|
236 | end if;
|
237 | else
|
238 | factor := to_unsigned(128,8);
|
239 | end if;
|
240 | In1Mult <= std_logic_vector(Modifier);
|
241 | In2Mult <= std_logic_vector("000000000000000000000000"&factor);
|
242 | Busy_C <= '1';
|
243 | Reset_C <= '1';
|
244 | s <= Calc_MOD;
|
245 |
|
246 | -------------------------------------------------------
|
247 | when Calc_MOD =>
|
248 | -------------------------------------------------------
|
249 | Modifier64b := unsigned(OutMult) / 128;
|
250 | --Modifier64b := (Modifier * ("000000000000000000000000"&factor)) / 128;
|
251 | Modifier := Modifier64b(31 downto 0);
|
252 | Busy_C <= '1';
|
253 | Reset_C <= '1';
|
254 | s <= prepCalc_ND;
|
255 |
|
256 | -------------------------------------------------------
|
257 | when prepCalc_ND =>
|
258 | -------------------------------------------------------
|
259 | If EnableMOD_C = '0' then
|
260 | In1Mult <= std_logic_vector(MODref_C);
|
261 | In2Mult <= std_logic_vector(dNSquare);
|
262 | --ND64bitfp := MODref_C * dNsquare(31 downto 0);
|
263 | else
|
264 | In1Mult <= std_logic_vector(Modifier);
|
265 | In2Mult <= std_logic_vector(dNSquare);
|
266 | --ND64bitfp := Modifier * dNsquare(31 downto 0);
|
267 | end if;
|
268 | Busy_C <= '1';
|
269 | Reset_C <= '1';
|
270 | s <= Calc_ND;
|
271 |
|
272 | -------------------------------------------------------
|
273 | when Calc_ND =>
|
274 | -------------------------------------------------------
|
275 | --dNsquare := dN * dN;
|
276 | ND64bitfp := unsigned(OutMult);
|
277 | ND := ND64bitfp(47 downto 32);
|
278 | Busy_C <= '1';
|
279 | Reset_C <= '1';
|
280 | s <= Idle;
|
281 |
|
282 | ----------------------------------------------------------
|
283 | end case;
|
284 | ----------------------------------------------------------
|
285 |
|
286 | r_C <= r;
|
287 | N_C <= N;
|
288 | dN_C <= dN;
|
289 | ND_C <= ND;
|
290 | DirOut_C <= dir(0);
|
291 | MOD_C <= Modifier;
|
292 |
|
293 | end process;
|
294 |
|
295 | -------------------------------------------------------------------
|
296 | SyncETrigger : process
|
297 | -------------------------------------------------------------------
|
298 | begin
|
299 | wait until rising_edge(clk_C);
|
300 | Esr <= Esr(1 downto 0) & ETrigger_C;
|
301 | end process;
|
302 |
|
303 | -------------------------------------------------------------------
|
304 | SyncSWDTTrigger : process
|
305 | -------------------------------------------------------------------
|
306 | begin
|
307 | wait until rising_edge(clk_C);
|
308 | SWDTsr <= SWDTsr(1 downto 0) & SWDTTrigger_C;
|
309 | end process;
|
310 |
|
311 | end Behavioral;
|