`timescale 1ns/1ps
`include "../util/clog2.vh"

module mcu #(
    parameter memfile = "",
    parameter memsize = 8192,
    parameter sim = 1'b0
)(
    input wire i_clk,
    input wire i_rst,

    input wire [31:0] i_GPI_A,
    input wire [31:0] i_GPI_B,
    input wire [31:0] i_GPI_C,
    input wire [31:0] i_GPI_D,
    output wire [31:0] o_GPO_A,
    output wire [31:0] o_GPO_B,
    output wire [31:0] o_GPO_C,
    output wire [31:0] o_GPO_D
);
    localparam WITH_CSR = 1;
    localparam regs = 32+WITH_CSR*4;
    localparam rf_width = 8;

    wire rst;
    wire rst_mem_reason;
    wire timer_irq;
    assign rst = i_rst | rst_mem_reason;
    assign timer_irq = 1'b0;

    // Busses
    //  CPU->memory
    wire [31:0] wb_mem_adr;
    wire [31:0] wb_mem_dat;
    wire [3:0] wb_mem_sel;
    wire wb_mem_we;
    wire wb_mem_stb;
    wire [31:0] wb_mem_rdt;
    wire wb_mem_ack;
    //  CPU->peripherals
    wire [31:0] wb_ext_adr;
    wire [31:0] wb_ext_dat;
    wire [3:0] wb_ext_sel;
    wire wb_ext_we;
    wire wb_ext_stb;
    wire [31:0] wb_ext_rdt;
    wire wb_ext_ack;
    //  CPU->RF
    wire [6+WITH_CSR:0] rf_waddr;
    wire [rf_width-1:0] rf_wdata;
    wire rf_wen;
    wire [6+WITH_CSR:0] rf_raddr;
    wire [rf_width-1:0] rf_rdata;
    wire rf_ren;
    //  combined RF and mem bus to actual RAM
    wire [`CLOG2(memsize)-1:0] sram_waddr;
    wire [rf_width-1:0] sram_wdata;
    wire sram_wen;
    wire [`CLOG2(memsize)-1:0] sram_raddr;
    wire [rf_width-1:0] sram_rdata;
    wire sram_ren;

    // GPIO
    wire [4*32-1:0] GPO;
    wire [4*32-1:0] GPI;
    assign o_GPO_A = GPO[32*1-1:32*0];
    assign o_GPO_B = GPO[32*2-1:32*1];
    assign o_GPO_C = GPO[32*3-1:32*2];
    assign o_GPO_D = GPO[32*4-1:32*3];
    assign GPI[32*1-1:32*0] = i_GPI_A;
    assign GPI[32*2-1:32*1] = i_GPI_B;
    assign GPI[32*3-1:32*2] = i_GPI_C;
    assign GPI[32*4-1:32*3] = i_GPI_D;

    // SERV core with mux splitting dbus into mem and ext and
    //  arbiter combining mem and ibus
    //  separate rst line to let other hardware keep core under reset
    servile #(
        .reset_pc(32'h0000_0000),
        .reset_strategy("MINI"),
        .rf_width(rf_width),
        .sim(sim),
        .with_csr(WITH_CSR),
        .with_c(0),
        .with_mdu(0)
    ) servile (
        .i_clk(i_clk),
        .i_rst(rst),
        .i_timer_irq(timer_irq),

        //Memory interface
        .o_wb_mem_adr(wb_mem_adr),
        .o_wb_mem_dat(wb_mem_dat),
        .o_wb_mem_sel(wb_mem_sel),
        .o_wb_mem_we(wb_mem_we),
        .o_wb_mem_stb(wb_mem_stb),
        .i_wb_mem_rdt(wb_mem_rdt),
        .i_wb_mem_ack(wb_mem_ack),

        //Extension interface
        .o_wb_ext_adr(wb_ext_adr),
        .o_wb_ext_dat(wb_ext_dat),
        .o_wb_ext_sel(wb_ext_sel),
        .o_wb_ext_we(wb_ext_we),
        .o_wb_ext_stb(wb_ext_stb),
        .i_wb_ext_rdt(wb_ext_rdt),
        .i_wb_ext_ack(wb_ext_ack),

        //RF IF
        .o_rf_waddr(rf_waddr),
        .o_rf_wdata(rf_wdata),
        .o_rf_wen(rf_wen),
        .o_rf_raddr(rf_raddr),
        .o_rf_ren(rf_ren),
        .i_rf_rdata(rf_rdata)
    );

    // WB arbiter combining RF and mem interfaces into 1
    //  Last 128 bytes are used for registers  
    servile_rf_mem_if #(
        .depth(memsize),
        .rf_regs(regs)
    ) rf_mem_if (
        .i_clk (i_clk),
        .i_rst (i_rst),
        
        .i_waddr(rf_waddr),
        .i_wdata(rf_wdata),
        .i_wen(rf_wen),
        .i_raddr(rf_raddr),
        .o_rdata(rf_rdata),
        .i_ren(rf_ren),

        .o_sram_waddr(sram_waddr),
        .o_sram_wdata(sram_wdata),
        .o_sram_wen(sram_wen),
        .o_sram_raddr(sram_raddr),
        .i_sram_rdata(sram_rdata),
        // .o_sram_ren(sram_ren),

        .i_wb_adr(wb_mem_adr[`CLOG2(memsize)-1:2]),
        .i_wb_stb(wb_mem_stb),
        .i_wb_we(wb_mem_we) ,
        .i_wb_sel(wb_mem_sel),
        .i_wb_dat(wb_mem_dat),
        .o_wb_rdt(wb_mem_rdt),
        .o_wb_ack(wb_mem_ack)
    );

    memory #(
        .memfile(memfile),
        .depth(memsize),
        .sim(sim)
    ) mem (
        .i_clk(i_clk),
        .i_waddr(sram_waddr),
        .i_wdata(sram_wdata),
        .i_wen(sram_wen),
        .i_raddr(sram_raddr),
        .o_rdata(sram_rdata),
        .o_core_reset(rst_mem_reason)
    );

    wb_gpio_banks #(
      .BASE_ADDR(32'h40000000),
      .NUM_BANKS(4)
    ) gpio (
        .i_wb_clk(i_clk),
        .i_wb_rst(rst),
        .i_wb_dat(wb_ext_dat),
        .i_wb_adr(wb_ext_adr),
        .i_wb_we(wb_ext_we),
        .i_wb_stb(wb_ext_stb),
        .i_wb_sel(wb_ext_sel),
        .o_wb_rdt(wb_ext_rdt),
        .o_wb_ack(wb_ext_ack),
        .i_gpio(GPI),
        .o_gpio(GPO)
    );

endmodule

module jtag_mem_protocol #(
    parameter aw = 8,
    parameter chain = 1
)(
    input  wire          i_clk,
    input  wire          i_rst,
    input  wire [7:0]    i_mem_rdata,
    output reg           o_mem_wen,
    output reg [7:0]     o_mem_wdata,
    output reg [aw-1:0]  o_mem_waddr,
    output reg [aw-1:0]  o_mem_raddr,
    output reg           o_core_reset
);
    localparam FRAME_BITS = 42;

    // Frame format (LSB-first on JTAG shift stream):
    // [0]     : core reset
    // [1]     : write (1=write, 0=read)
    // [33:2]  : address
    // [41:34] : data
    wire [31:0] cmd_addr_full;
    wire        cmd_reset;
    wire        cmd_write;
    wire [7:0]  cmd_data;

    // JTAG interface wires
    wire jtag_tck;
    wire jtag_tdi;
    wire jtag_drck;
    wire jtag_capture;
    wire jtag_shift;
    wire jtag_update;
    wire jtag_runtest;
    wire jtag_reset;
    wire jtag_sel;

    // JTAG clock-domain regs
    reg [FRAME_BITS-1:0] shift_q;
    reg [FRAME_BITS-1:0] cmd_jtag_q;
    reg update_toggle_jtag_q;
    reg [FRAME_BITS-1:0] resp_meta_q;
    reg [FRAME_BITS-1:0] resp_sync_q;

    // Core clock-domain CDC regs
    reg update_toggle_meta_q;
    reg update_toggle_sync_q;
    reg update_toggle_sync_d_q;
    reg [FRAME_BITS-1:0] cmd_meta_q;
    reg [FRAME_BITS-1:0] cmd_sync_q;

    // Core clock-domain protocol state
    reg [FRAME_BITS-1:0] resp_core_q;
    reg read_pending_q;
    reg [31:0] read_addr_full_q;

    assign cmd_reset = cmd_sync_q[0];
    assign cmd_write = cmd_sync_q[1];
    assign cmd_addr_full = cmd_sync_q[33:2];
    assign cmd_data = cmd_sync_q[41:34];

    jtag_if #(
        .chain(chain)
    ) jtag (
        .i_tdo(shift_q[0]),
        .o_tck(jtag_tck),
        .o_tdi(jtag_tdi),
        .o_drck(jtag_drck),
        .o_capture(jtag_capture),
        .o_shift(jtag_shift),
        .o_update(jtag_update),
        .o_runtest(jtag_runtest),
        .o_reset(jtag_reset),
        .o_sel(jtag_sel)
    );

    // JTAG domain: shift register and update event capture.
    always @(posedge jtag_drck or posedge jtag_reset or posedge i_rst) begin
        if (jtag_reset || i_rst) begin
            shift_q <= {FRAME_BITS{1'b0}};
            resp_meta_q <= {FRAME_BITS{1'b0}};
            resp_sync_q <= {FRAME_BITS{1'b0}};
        end else begin
            resp_meta_q <= resp_core_q;
            resp_sync_q <= resp_meta_q;

            if (jtag_sel && jtag_capture) begin
                shift_q <= resp_sync_q;
            end else if (jtag_sel && jtag_shift) begin
                shift_q <= {jtag_tdi, shift_q[FRAME_BITS-1:1]};
            end
        end
    end

    always @(posedge jtag_update or posedge jtag_reset or posedge i_rst) begin
        if (jtag_reset || i_rst) begin
            cmd_jtag_q <= {FRAME_BITS{1'b0}};
            update_toggle_jtag_q <= 1'b0;
        end else if (jtag_sel) begin
            cmd_jtag_q <= shift_q;
            update_toggle_jtag_q <= ~update_toggle_jtag_q;
        end
    end

    // Core domain: receive command and execute against RAM port-B.
    always @(posedge i_clk or posedge i_rst) begin
        if (i_rst) begin
            update_toggle_meta_q <= 1'b0;
            update_toggle_sync_q <= 1'b0;
            update_toggle_sync_d_q <= 1'b0;
            cmd_meta_q <= {FRAME_BITS{1'b0}};
            cmd_sync_q <= {FRAME_BITS{1'b0}};
            o_mem_wen <= 1'b0;
            o_mem_wdata <= 8'h00;
            o_mem_waddr <= {aw{1'b0}};
            o_mem_raddr <= {aw{1'b0}};
            o_core_reset <= 1'b0;
            resp_core_q <= {FRAME_BITS{1'b0}};
            read_pending_q <= 1'b0;
            read_addr_full_q <= 32'h0000_0000;
        end else begin
            // Defaults are one-cycle strobes for write/reset.
            o_mem_wen <= 1'b0;
            o_core_reset <= 1'b0;

            update_toggle_meta_q <= update_toggle_jtag_q;
            update_toggle_sync_q <= update_toggle_meta_q;
            update_toggle_sync_d_q <= update_toggle_sync_q;
            cmd_meta_q <= cmd_jtag_q;
            cmd_sync_q <= cmd_meta_q;

            // Complete pending read (port-B read is synchronous).
            if (read_pending_q) begin
                read_pending_q <= 1'b0;
                resp_core_q[0] <= 1'b0;
                resp_core_q[1] <= 1'b0;
                resp_core_q[33:2] <= read_addr_full_q;
                resp_core_q[41:34] <= i_mem_rdata;
            end

            // New JTAG command arrived.
            if (update_toggle_sync_q ^ update_toggle_sync_d_q) begin
                if (cmd_reset) begin
                    o_core_reset <= 1'b1;
                end

                if (cmd_write) begin
                    o_mem_waddr <= cmd_addr_full[aw-1:0];
                    o_mem_wdata <= cmd_data;
                    o_mem_wen <= 1'b1;
                    resp_core_q[0] <= cmd_reset;
                    resp_core_q[1] <= 1'b1;
                    resp_core_q[33:2] <= cmd_addr_full;
                    resp_core_q[41:34] <= cmd_data;
                end else begin
                    o_mem_raddr <= cmd_addr_full[aw-1:0];
                    read_pending_q <= 1'b1;
                    read_addr_full_q <= cmd_addr_full;
                end
            end
        end
    end

    // Keep lint quiet for currently-unused JTAG outputs.
    wire _unused_tck;
    wire _unused_runtest;
    assign _unused_tck = jtag_tck;
    assign _unused_runtest = jtag_runtest;
endmodule

module memory #(
	parameter memfile = "",
    parameter depth = 256,
	parameter sim = 1'b0,
	localparam aw = `CLOG2(depth)
)(
    input wire i_clk,
	input wire [aw-1:0]	i_waddr,
	input wire [7:0] i_wdata,
	input wire i_wen,
	input wire [aw-1:0]	i_raddr,
	output reg [7:0] o_rdata,
    output wire o_core_reset
);
    // The actual memory
   	reg [7:0]mem [0:depth-1];

    // Second interface
    wire wen_b;
    wire [7:0] wdata_b;
    wire [aw-1:0] waddr_b;
    reg [7:0] rdata_b;
    wire [aw-1:0] raddr_b;
    jtag_mem_protocol #(
        .aw(aw),
        .chain(1)
    ) jtag_mem (
        .i_clk(i_clk),
        .i_rst(1'b0),
        .i_mem_rdata(rdata_b),
        .o_mem_wen(wen_b),
        .o_mem_wdata(wdata_b),
        .o_mem_waddr(waddr_b),
        .o_mem_raddr(raddr_b),
        .o_core_reset(o_core_reset)
    );

    // Read/Write
	always @(posedge i_clk) begin
        // main interface
		if (i_wen) 
            mem[i_waddr] <= i_wdata;
		o_rdata <= mem[i_raddr];
        // second interface
        if (wen_b)
            mem[waddr_b] <= wdata_b;
        rdata_b <= mem[raddr_b];
	end

    // Preload memory
	integer i;
   	initial begin
		if(sim==1'b1) begin
			for (i = 0; i < depth; i = i + 1)
				mem[i] = 8'h00;
		end
		if(|memfile) begin
			$display("Preloading %m from %s", memfile);
			$readmemh(memfile, mem);
		end
   end

endmodule