litedram: Add an L2 cache with store queue

This adds a cache between the wishbone and litedram with the following
features (at this point, it's still evolving)

  - 128 bytes line width in order to have a reasonable amount of
litedram pipelining on the 128-bit wide data port.

  - Configurable geometry otherwise

  - Stores are acked immediately on wishbone whether hit or miss
(minus a 2 cycles delay if there's a previous load response in the
way) and sent to LiteDRAM via 8 entries (configurable) store queue

Signed-off-by: Benjamin Herrenschmidt <benh@kernel.crashing.org>

Makefile

@@ -50,7 +50,7 @@ core_files = decode_types.vhdl common.vhdl wishbone_types.vhdl fetch1.vhdl \
 	loadstore1.vhdl mmu.vhdl dcache.vhdl writeback.vhdl core_debug.vhdl \
 	core.vhdl
 
-soc_files = wishbone_arbiter.vhdl wishbone_bram_wrapper.vhdl \
+soc_files = wishbone_arbiter.vhdl wishbone_bram_wrapper.vhdl sync_fifo.vhdl \
 	wishbone_debug_master.vhdl xics.vhdl syscon.vhdl soc.vhdl
 
 soc_sim_files = sim_console.vhdl sim_uart.vhdl sim_bram_helpers.vhdl \

litedram/extras/wave.opt

@@ -0,0 +1,84 @@
+$ version 1.1
+
+# Signals in entities :
+/core_dram_tb/dram/rst
+/core_dram_tb/dram/system_clk
+/core_dram_tb/dram/system_reset
+/core_dram_tb/dram/wb_in
+/core_dram_tb/dram/wb_out
+/core_dram_tb/dram/user_port0_cmd_valid
+/core_dram_tb/dram/user_port0_cmd_ready
+/core_dram_tb/dram/user_port0_cmd_we
+/core_dram_tb/dram/user_port0_cmd_addr
+/core_dram_tb/dram/user_port0_wdata_valid
+/core_dram_tb/dram/user_port0_wdata_ready
+/core_dram_tb/dram/user_port0_wdata_we
+/core_dram_tb/dram/user_port0_wdata_data
+/core_dram_tb/dram/user_port0_rdata_valid
+/core_dram_tb/dram/user_port0_rdata_ready
+/core_dram_tb/dram/user_port0_rdata_data
+/core_dram_tb/dram/cache_tags
+/core_dram_tb/dram/cache_valids
+/core_dram_tb/dram/storeq_rd_ready
+/core_dram_tb/dram/storeq_rd_valid
+/core_dram_tb/dram/storeq_rd_data
+/core_dram_tb/dram/storeq_wr_ready
+/core_dram_tb/dram/storeq_wr_valid
+/core_dram_tb/dram/storeq_wr_data
+/core_dram_tb/dram/accept_store
+/core_dram_tb/dram/state
+/core_dram_tb/dram/wb_req
+/core_dram_tb/dram/store_queued
+/core_dram_tb/dram/read_ack_0
+/core_dram_tb/dram/read_ack_1
+/core_dram_tb/dram/read_ad3_0
+/core_dram_tb/dram/read_ad3_1
+/core_dram_tb/dram/read_way_0
+/core_dram_tb/dram/read_way_1
+/core_dram_tb/dram/req_index
+/core_dram_tb/dram/req_row
+/core_dram_tb/dram/req_hit_way
+/core_dram_tb/dram/req_tag
+/core_dram_tb/dram/req_op
+/core_dram_tb/dram/req_laddr
+/core_dram_tb/dram/req_ad3
+/core_dram_tb/dram/req_we
+/core_dram_tb/dram/req_wdata
+/core_dram_tb/dram/store_way
+/core_dram_tb/dram/store_index
+/core_dram_tb/dram/store_row
+/core_dram_tb/dram/cache_out
+/core_dram_tb/dram/plru_victim
+/core_dram_tb/dram/replace_way
+/core_dram_tb/dram/rams/do_read
+/core_dram_tb/dram/rams/do_write
+/core_dram_tb/dram/rams/rd_addr
+/core_dram_tb/dram/rams/wr_addr
+/core_dram_tb/dram/rams/wr_data
+/core_dram_tb/dram/rams/wr_sel
+/core_dram_tb/dram/rams/wr_sel_m
+/core_dram_tb/dram/rams/dout
+/core_dram_tb/dram/rams/way/clk
+/core_dram_tb/dram/rams/way/rd_en
+/core_dram_tb/dram/rams/way/rd_addr
+/core_dram_tb/dram/rams/way/rd_data
+/core_dram_tb/dram/rams/way/wr_sel
+/core_dram_tb/dram/rams/way/wr_addr
+/core_dram_tb/dram/rams/way/wr_data
+/core_dram_tb/dram/rams/way/rd_data0
+/core_dram_tb/dram/store_queue/wr_ready
+/core_dram_tb/dram/store_queue/wr_valid
+/core_dram_tb/dram/store_queue/wr_data
+/core_dram_tb/dram/store_queue/rd_ready
+/core_dram_tb/dram/store_queue/rd_valid
+/core_dram_tb/dram/store_queue/rd_data
+/core_dram_tb/dram/store_queue/rd_idx
+/core_dram_tb/dram/store_queue/rd_next
+/core_dram_tb/dram/store_queue/wr_idx
+/core_dram_tb/dram/store_queue/wr_next
+/core_dram_tb/dram/store_queue/op_prev
+/core_dram_tb/dram/store_queue/op_next
+/core_dram_tb/dram/store_queue/full
+/core_dram_tb/dram/store_queue/empty
+/core_dram_tb/dram/store_queue/push
+/core_dram_tb/dram/store_queue/pop

litedram/gen-src/sdram_init/include/system.h

@@ -9,9 +9,9 @@
 #define CONFIG_CPU_NOP		"nop"
 
 #ifdef __SIM__
-#define MEMTEST_BUS_SIZE	16
-#define MEMTEST_DATA_SIZE	16
-#define MEMTEST_ADDR_SIZE	16
+#define MEMTEST_BUS_SIZE	512//16
+#define MEMTEST_DATA_SIZE	1024//16
+#define MEMTEST_ADDR_SIZE	128//16
 #define CONFIG_SIM_DISABLE_DELAYS
 #endif
 

litedram/generated/arty/litedram_core.v

@@ -1,5 +1,5 @@
 //--------------------------------------------------------------------------------
-// Auto-generated by Migen (0d16e03) & LiteX (564d731a) on 2020-05-26 20:37:38
+// Auto-generated by Migen (0d16e03) & LiteX (564d731a) on 2020-05-30 20:25:53
 //--------------------------------------------------------------------------------
 module litedram_core(
 	input wire clk,

litedram/generated/nexys-video/litedram_core.v

@@ -1,5 +1,5 @@
 //--------------------------------------------------------------------------------
-// Auto-generated by Migen (0d16e03) & LiteX (564d731a) on 2020-05-26 20:37:40
+// Auto-generated by Migen (0d16e03) & LiteX (564d731a) on 2020-05-30 20:25:55
 //--------------------------------------------------------------------------------
 module litedram_core(
 	input wire clk,

litedram/generated/sim/litedram_core.init

@@ -510,7 +510,7 @@ a64b5a7d14004a39
 0000000000000000
 0000000000000000
 0000000000000000
-384296003c4c0001
+384297003c4c0001
 fbc1fff07c0802a6
 f8010010fbe1fff8
 3be10020f821fe91
@@ -519,11 +519,11 @@ f8c101a838800140
 38c101987c651b78
 7fe3fb78f8e101b0
 f92101c0f90101b8
-48000da5f94101c8
+48000d65f94101c8
 7c7e1b7860000000
-480008bd7fe3fb78
+4800087d7fe3fb78
 3821017060000000
-480013647fc3f378
+480013247fc3f378
 0100000000000000
 4e80002000000280
 0000000000000000
@@ -531,67 +531,67 @@ f92101c0f90101b8
 4e8000204c00012c
 0000000000000000
 3c4c000100000000
-7c0802a63842955c
+7c0802a63842965c
 7d800026fbe1fff8
 91810008f8010010
-480007b1f821ff91
+48000771f821ff91
 3c62ffff60000000
-4bffff3538637de8
+4bffff3538637ca8
 548400023880ffff
 7c8026ea7c0004ac
 3fe0c0003c62ffff
-63ff000838637e08
+63ff000838637cc8
 3c62ffff4bffff11
-38637e287bff0020
+38637ce87bff0020
 7c0004ac4bffff01
 73e900017fe0feea
 3c62ffff41820010
-4bfffee538637e40
+4bfffee538637d00
 4d80000073e90002
 3c62ffff41820010
-4bfffecd38637e48
+4bfffecd38637d08
 4e00000073e90004
 3c62ffff41820010
-4bfffeb538637e50
-3be2804860000000
+4bfffeb538637d10
+3bff7fa03fe2ffff
 4bfffea57fe3fb78
 3c80c00041920028
 7884002060840010
 7c8026ea7c0004ac
 7884b2823c62ffff
-4bfffe7d38637e58
+4bfffe7d38637d18
 3c80c000418e004c
 7884002060840018
 7c8026ea7c0004ac
 788465023c62ffff
-4bfffe5538637e78
+4bfffe5538637d38
 608400303c80c000
 7c0004ac78840020
 3c62ffff7c8026ea
-38637e987884b282
+38637d587884b282
 3d20c0004bfffe31
 7929002061290020
 7d204eea7c0004ac
 3c62ffff3c80000f
-38637eb860844240
+38637d7860844240
 4bfffe057c892392
 4bfffdfd7fe3fb78
 3ca2ffff418e0028
 3c62ffff3c82ffff
-38847ee838a57ed8
-4bfffddd38637ef0
-60000000480004c1
+38847da838a57d98
+4bfffddd38637db0
+6000000048000481
 3c62ffff41920020
-4bfffdc538637f20
+4bfffdc538637de0
 8181000838210070
-480011807d818120
-38637f383c62ffff
+480011407d818120
+38637df83c62ffff
 3c80f0004bfffda9
 6084400038a0ffff
 7884002054a50422
-480008553c604000
+480008153c604000
 3c62ffff60000000
-4bfffd7d38637f58
+4bfffd7d38637e18
 e801001038210070
 ebe1fff881810008
 7d8181207c0803a6
@@ -605,138 +605,130 @@ ebe1fff881810008
 4e8000207d20572a
 0000000000000000
 3c4c000100000000
-7c0802a63842930c
+7c0802a63842940c
 614a08003d40c010
 794a002039200001
 f821ffa1f8010010
 7d20572a7c0004ac
-3862802860000000
+38637f803c62ffff
 600000004bfffce1
 e801001038210060
 4e8000207c0803a6
 0100000000000000
 3c4c000100000080
-7c0802a6384292b4
-6129000c3d204000
-3fc0aaaa48000ffd
-f821ff713f804000
-63deaaaa3fa04000
-639c00043fe04000
-93df000063bd0008
-93dd000093dc0000
-4bfffce993c90000
-813f000060000000
-7d29f278815c0000
-7d2900347f8af000
-692900015529d97e
-7fff07b43be90001
-7d3f07b4409e0008
-7f89f000813d0000
-3bff0001419e000c
-3d2040007fff07b4
-812900006129000c
-2f8aaaaa6d2a5555
-3bff0001419e000c
-3fc055557fff07b4
-3d2040003fa04000
-3f80400063de5555
-63bd000461290008
-93dd000093dc0000
-3d20400093c90000
-93c900006129000c
-600000004bfffc4d
-7f89f000813c0000
-3bff0001419e000c
-813d00007fff07b4
-2f8a55556d2a5555
-3bff0001419e000c
-3d2040007fff07b4
-8129000061290008
-2f8a55556d2a5555
-3bff0001419e000c
-3d2040007fff07b4
-812900006129000c
-2f8a55556d2a5555
-3bff0001419e0028
-3c62ffff7fff07b4
-7fe4fb7838a00100
-4bfffb5538637f70
-4800000c60000000
-409effe02fbf0000
-3ce0802039000004
-60e700037d0903a6
-392000013d404000
-7928f84278e70020
-7d2900d0792907e0
-7d293838394a0004
-912afffc7d294278
-4bfffb794200ffe4
-3900000460000000
-7d0903a63ce08020
-3d40400060e70003
-392000013bc00000
-7928f84278e70020
-7d2900d0792907e0
-7d2942787d293838
-7f884840810a0000
-3bde0001419e000c
-394a00047fde07b4
-2fbe00004200ffd4
-3c62ffff419e001c
-7fc4f37838a00004
-4bfffa9538637f98
-3d20400060000000
-6129000839400000
-914900003ba00000
-394000013d204000
-914900006129000c
-394000023d204000
-9149000061290010
-394000033d204000
-9149000061290014
-600000004bfffabd
-3940000039200004
-3d2a10007d2903a6
-8129000879291764
-7f8950005529043e
-3bbd0001419e000c
-394a00017fbd07b4
-2fbd00004200ffdc
-3c62ffff419e001c
-7fa4eb7838a00004
-4bfff9f538637fc0
-7ffefa1460000000
-7fffea143bc00000
-409e00a42f9f0000
-38637fe83c62ffff
-600000004bfff9d1
-3f8040007f5602a6
-639c00043f604000
-3fa0400039200001
-3fc0400093db0000
-63bd0008913c0000
-63de000c39200002
-39200003913d0000
-7ff602a6913e0000
-600000004bfff9fd
-815b00007d3602a6
-815d0000815c0000
-7cb602a6815e0000
-7ca5485038803200
-7ca42b967d3fd050
-3c62ffff7c844b96
-788404a078a504a0
-3bc0000138637ff8
-600000004bfff941
-7fc3f37838210090
-0000000048000cd4
-0000068001000000
-38428f183c4c0001
-600000007c0802a6
-48000c6138628050
+7c0802a6384293b4
+38637e303c62ffff
+f821ff7148000fc1
+600000004bfffca1
+3d40aaaa39000080
+3d2040007d0903a6
+91490000614aaaaa
+4200fff839290004
+4bfffce93f60aaaa
+3fa0aaaa60000000
+637baaaa3f82ffff
+3be000003bc00000
+3b9c7e4063bdaaaa
+3d3e10007b7b0020
+792917647fc407b4
+7f85e80080a90000
+3bff0001419e001c
+7f83e3787f66db78
+4bfffc257fff07b4
+3bde000160000000
+409effc82bbe0080
+3d40555539000080
+3d2040007d0903a6
+91490000614a5555
+4200fff839290004
+600000004bfffc65
+3f82ffff3fa05555
+3bc000003f605555
+3b9c7e6063bd5555
+3d3e1000637b5555
+792917647fc407b4
+7f85e80080a90000
+3bff0001419e001c
+7f83e3787f66db78
+4bfffba57fff07b4
+3bde000160000000
+409effc82bbe0080
+419e001c2fbf0000
+38a001003c62ffff
+38637e807fe4fb78
+600000004bfffb79
+3fc2ffff3c62ffff
+3bde7ec038637ea8
+600000004bfffb61
+3d20400039400100
+390000017d4903a6
+3929000439480001
+9149fffc79480020
+4bfffba94200fff0
+3940010060000000
+7d4903a639200000
+3d09100038c00001
+7908176439460001
+794600207d2407b4
+7f8a284080a80000
+7fc3f378419e0014
+600000004bfffaf9
+392900014bffff98
+3c62ffff4200ffcc
+4bfffadd38637ee0
+3920002060000000
+7d2903a639400000
+794800203d2a1000
+394a000139290002
+9109000079291764
+4bfffb214200ffe8
+3f82ffff60000000
+3bc000003ba00000
+3d3d10003b9c7ef8
+792917647fa607b4
+5529043e81290008
+7d2507b47f893000
+3bde0001419e001c
+7f83e3787cc43378
+4bfffa657fde07b4
+3bbd000160000000
+409effc02bbd0020
+419e001c2fbe0000
+38a000203c62ffff
+38637f187fc4f378
+600000004bfffa39
+386000007ffff214
+409e00b02f9f0000
+38637f403c62ffff
+600000004bfffa19
+394001007c9602a6
+7d4903a678840020
+3d49100039200000
+794a176479280020
+910a000039290001
+7ff602a64200ffec
+3fe0000c7c9f2050
+7fff239663ff8000
+600000004bfffa45
+7d3602a67bff0020
+7929002039000100
+3d4040007d0903a6
+394a0004810a0000
+7cb602a64200fff8
+3ca0000c7d254850
+3c62ffff60a58000
+7fe4fb787ca54b96
+78a5032038637f50
+600000004bfff981
+3821009038600001
+0000000048000cd8
+0000058001000000
+384290583c4c0001
+3c62ffff7c0802a6
+48000c6138637fa8
 3f60c010f821ff71
 637b10003be00000
-4bfff8f57b7b0020
+4bfff9357b7b0020
 7c0004ac60000000
 3f40c0107fe0df2a
 7b5a0020635a1008
@@ -756,22 +748,22 @@ f821ff713f804000
 7d20ef2a7c0004ac
 7c0004ac39200002
 3860000f7d20f72a
-7c0004ac4bfffb09
+7c0004ac4bfffb49
 392000037fe0ef2a
 7d20f72a7c0004ac
-4bfffaed3860000f
+4bfffb2d3860000f
 7c0004ac39200006
 3b8000017d20ef2a
 7f80f72a7c0004ac
-4bfffacd3860000f
+4bfffb0d3860000f
 7c0004ac39200920
 7c0004ac7d20ef2a
 3860000f7fe0f72a
-392004004bfffab1
+392004004bfffaf1
 7d20ef2a7c0004ac
 7fe0f72a7c0004ac
-4bfffa9538600003
-4bfffb294bfffad5
+4bfffad538600003
+4bfffb694bfffb15
 4082001c2c230000
 7f80df2a7c0004ac
 7f80d72a7c0004ac
@@ -780,27 +772,27 @@ f821ff713f804000
 4bffffec38600001
 0100000000000000
 3c4c000100000680
-3d20c00038428d94
+3d20c00038428ed4
 6129200060000000
-f92280b879290020
+f922801079290020
 612900203d20c000
 7c0004ac79290020
 3d40001c7d204eea
 7d295392614a2000
-394a0018e94280b8
+394a0018e9428010
 7c0004ac3929ffff
 4e8000207d2057ea
 0000000000000000
 3c4c000100000000
-6000000038428d34
-39290010e92280b8
+6000000038428e74
+39290010e9228010
 7d204eea7c0004ac
 4082ffe871290008
-e94280b85469063e
+e94280105469063e
 7d2057ea7c0004ac
 000000004e800020
 0000000000000000
-38428cf03c4c0001
+38428e303c4c0001
 fbc1fff07c0802a6
 3bc3fffffbe1fff8
 f821ffd1f8010010
@@ -874,7 +866,7 @@ f924000039290002
 7c6307b43863ffe0
 000000004e800020
 0000000000000000
-38428aa03c4c0001
+38428be03c4c0001
 3d2037367c0802a6
 612935347d908026
 65293332792907c6
@@ -908,7 +900,7 @@ fbfd00007fe9fa14
 4bfffff07d29f392
 0300000000000000
 3c4c000100000580
-7c0802a638428994
+7c0802a638428ad4
 f821ffb1480006e9
 7c7f1b78eb630000
 7cbd2b787c9c2378
@@ -924,7 +916,7 @@ f821ffb1480006e9
 4bffffb8f93f0000
 0100000000000000
 3c4c000100000580
-7c0802a638428914
+7c0802a638428a54
 f821ffa148000661
 7c9b23787c7d1b78
 388000007ca32b78
@@ -955,16 +947,16 @@ e95d00009b270000
 f95d0000394a0001
 000000004bffffa8
 0000078001000000
-384288183c4c0001
+384289583c4c0001
 480005397c0802a6
 7c741b79f821fed1
 38600000f8610060
 2fa4000041820068
 39210040419e0060
-3ac4ffff60000000
+3ac4ffff3e42ffff
 f92100703b410020
 3ae0000060000000
-3a428068392280b0
+3a527fc039228008
 f92100783ba10060
 ebc1006089250000
 419e00102fa90000
@@ -1196,16 +1188,35 @@ e8010010ebc1fff0
 20676e69746f6f42
 415244206d6f7266
 0000000a2e2e2e4d
+20747365746d656d
+000a2e2e2e737562
+7830203a7375625b
+7830203a5d783025
+2073762078383025
+000a783830257830
+257830207375625b
+257830203a5d7830
+3020737620783830
+00000a7838302578
 20747365746d654d
 6c69616620737562
 252f6425203a6465
 73726f7272652064
 000000000000000a
-20747365746d654d
-6961662061746164
-2f6425203a64656c
-726f727265206425
-0000000000000a73
+20747365746d656d
+0a2e2e2e61746164
+0000000000000000
+783020617461645b
+7830203a5d783025
+2073762078383025
+000a783830257830
+20747365746d656d
+0a2e2e2e72646461
+0000000000000000
+783020726464615b
+7830203a5d783025
+2073762078383025
+000a783830257830
 20747365746d654d
 6961662072646461
 2f6425203a64656c

litedram/generated/sim/litedram_core.v

@@ -1,5 +1,5 @@
 //--------------------------------------------------------------------------------
-// Auto-generated by Migen (0d16e03) & LiteX (564d731a) on 2020-05-26 20:37:42
+// Auto-generated by Migen (0d16e03) & LiteX (564d731a) on 2020-05-30 20:25:57
 //--------------------------------------------------------------------------------
 module litedram_core(
 	input wire clk,

microwatt.core

@@ -48,6 +48,7 @@ filesets:
       - soc.vhdl
       - xics.vhdl
       - syscon.vhdl
+      - sync_fifo.vhdl
     file_type : vhdlSource-2008
 
   fpga:

sync_fifo.vhdl

@@ -0,0 +1,163 @@
+-- Synchronous FIFO with a protocol similar to AXI
+--
+-- The outputs are generated combinationally from the inputs
+-- in order to allow for back-to-back transfers with the type
+-- of flow control used by busses lite AXI, pipelined WB or
+-- LiteDRAM native port when the FIFO is full.
+--
+-- That means that care needs to be taken by the user not to
+-- generate the inputs combinationally from the outputs otherwise
+-- it would create a logic loop.
+--
+-- If breaking that loop is required, a stash buffer could be
+-- added to break the flow control "loop" between the read and
+-- the write port.
+--
+library ieee;
+use ieee.std_logic_1164.all;
+
+library work;
+use work.utils.all;
+
+entity sync_fifo is
+    generic(
+        -- Fifo depth in entries
+        DEPTH     : natural := 64;
+
+        -- Fifo width in bits
+        WIDTH     : natural := 32;
+
+        -- When INIT_ZERO is set, the memory is pre-initialized to 0's
+        INIT_ZERO : boolean := false
+        );
+    port(
+        -- Control lines:
+        clk      : in std_ulogic;
+        reset    : in std_ulogic;
+
+        -- Write port
+        wr_ready : out std_ulogic;
+        wr_valid : in std_ulogic;
+        wr_data  : in std_ulogic_vector(WIDTH - 1 downto 0);
+
+        -- Read port
+        rd_ready : in std_ulogic;
+        rd_valid : out std_ulogic;
+        rd_data  : out std_ulogic_vector(WIDTH - 1 downto 0)
+        );
+end entity sync_fifo;
+
+architecture behaviour of sync_fifo is
+
+    subtype data_t is std_ulogic_vector(WIDTH - 1 downto 0);    
+    type memory_t is array(0 to DEPTH - 1) of data_t;
+
+    function init_mem return memory_t is
+        variable m : memory_t;
+    begin
+        if INIT_ZERO then
+            for i in 0 to DEPTH - 1 loop
+                m(i) := (others => '0');
+            end loop;
+        end if;
+        return m;
+    end function;
+
+    signal memory : memory_t := init_mem;
+
+    subtype index_t is integer range 0 to DEPTH - 1;
+    signal rd_idx  : index_t;
+    signal rd_next : index_t;
+    signal wr_idx  : index_t;
+    signal wr_next : index_t;
+
+    function next_index(idx : index_t) return index_t is
+        variable r : index_t;
+    begin
+        if ispow2(DEPTH) then
+            r := (idx + 1) mod DEPTH;
+        else
+            r := idx + 1;
+            if r = DEPTH then
+                r := 0;
+            end if;
+        end if;
+        return r;
+    end function;
+    
+    type op_t is (OP_POP, OP_PUSH);
+    signal op_prev : op_t := OP_POP;
+    signal op_next : op_t;
+
+    signal full, empty : std_ulogic;
+    signal push, pop   : std_ulogic;
+begin
+
+    -- Current state at last clock edge
+    empty <= '1' when rd_idx = wr_idx and op_prev = OP_POP  else '0';
+    full  <= '1' when rd_idx = wr_idx and op_prev = OP_PUSH else '0';
+
+    -- We can accept new data if we aren't full or we are but
+    -- the read port is going to accept data this cycle    
+    wr_ready <= rd_ready or not full;
+
+    -- We can provide data if we aren't empty or we are but
+    -- the write port is going to provide data this cycle
+    rd_valid <= wr_valid or not empty;
+
+    -- Internal control signals
+    push <= wr_ready and wr_valid;
+    pop  <= rd_ready and rd_valid;
+
+    -- Next state
+    rd_next <= next_index(rd_idx) when pop  = '1' else rd_idx;
+    wr_next <= next_index(wr_idx) when push = '1' else wr_idx;
+    with push & pop select op_next <=
+        OP_PUSH when "10",
+        OP_POP  when "01",
+        op_prev when others;
+
+    -- Read port output
+    rd_data <= memory(rd_idx) when empty = '0' else wr_data;
+
+    -- Read counter
+    reader: process(clk)
+    begin
+        if rising_edge(clk) then
+            if reset = '1' then
+                rd_idx <= 0;
+            else
+                rd_idx <= rd_next;
+            end if;
+        end if;
+    end process;
+
+    -- Write counter and memory write
+    producer: process(clk)
+    begin
+        if rising_edge(clk) then
+            if reset = '1' then
+                wr_idx <= 0;
+            else
+                wr_idx <= wr_next;
+
+                if push = '1' then
+                    memory(wr_idx) <= wr_data;
+                end if;
+            end if;
+        end if;
+    end process;
+
+    -- Previous op latch used for generating empty/full
+    op: process(clk)
+    begin
+        if rising_edge(clk) then
+            if reset = '1' then
+                op_prev <= OP_POP;
+            else
+                op_prev <= op_next;
+            end if;
+        end if;
+    end process;
+
+end architecture behaviour;