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

module mcu #(
    parameter memfile = "",
    parameter memsize = 8192,
    parameter sim = 1'b0,
    parameter jtag = 1
)(
    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 rf_width = 8;

    wire rst;
    wire rst_wb;
    wire rst_mem_peripherals;
    wire rst_cmd_jtag;
    wire timer_irq;
    assign rst = i_rst | rst_mem_peripherals | rst_cmd_jtag;
    // Keep the Wishbone path alive during JTAG "core reset" so memory can be programmed.
    assign rst_wb = i_rst;

    // Busses
    // CPU<->memory interconnect (CPU is a WB master)
    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_cpu;
    wire wb_mem_ack_cpu;

    // Interconnect->memory (shared WB slave side)
    wire [31:0] wb_mem_adr_s;
    wire [31:0] wb_mem_dat_s;
    wire [3:0] wb_mem_sel_s;
    wire wb_mem_we_s;
    wire wb_mem_stb_s;
    wire [31:0] wb_mem_rdt_s;
    wire wb_mem_ack_s;

    //  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;
    
    // 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;

    cpu #(
        .sim(sim),
        .WITH_CSR(WITH_CSR),
        .rf_width(rf_width)
    ) cpu (
        .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_cpu),
        .i_wb_mem_ack(wb_mem_ack_cpu),

        //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)
    );

    generate
        if (jtag) begin : gen_jtag_wb
            wire [31:0] wb_jtag_adr;
            wire [31:0] wb_jtag_dat;
            wire [3:0] wb_jtag_sel;
            wire wb_jtag_we;
            wire wb_jtag_cyc;
            wire wb_jtag_stb;
            wire [31:0] wb_jtag_rdt;
            wire wb_jtag_ack;

            wire [2*32-1:0] wbm_adr_i;
            wire [2*32-1:0] wbm_dat_i;
            wire [2*4-1:0] wbm_sel_i;
            wire [1:0] wbm_we_i;
            wire [1:0] wbm_cyc_i;
            wire [1:0] wbm_stb_i;
            wire [2*3-1:0] wbm_cti_i;
            wire [2*2-1:0] wbm_bte_i;
            wire [2*32-1:0] wbm_dat_o;
            wire [1:0] wbm_ack_o;
            wire [1:0] wbm_err_o;
            wire [1:0] wbm_rty_o;

            assign wbm_adr_i = {wb_jtag_adr, wb_mem_adr};
            assign wbm_dat_i = {wb_jtag_dat, wb_mem_dat};
            assign wbm_sel_i = {wb_jtag_sel, wb_mem_sel};
            assign wbm_we_i = {wb_jtag_we, wb_mem_we};
            assign wbm_cyc_i = {wb_jtag_cyc, wb_mem_stb};
            assign wbm_stb_i = {wb_jtag_stb, wb_mem_stb};
            assign wbm_cti_i = 6'b0;
            assign wbm_bte_i = 4'b0;

            assign wb_mem_rdt_cpu = wbm_dat_o[31:0];
            assign wb_mem_ack_cpu = wbm_ack_o[0];
            assign wb_jtag_rdt = wbm_dat_o[63:32];
            assign wb_jtag_ack = wbm_ack_o[1];

            wb_arbiter #(
                .dw(32),
                .aw(32),
                .num_masters(2)
            ) wb_mem_arbiter (
                .wb_clk_i(i_clk),
                .wb_rst_i(rst_wb),
                .wbm_adr_i(wbm_adr_i),
                .wbm_dat_i(wbm_dat_i),
                .wbm_sel_i(wbm_sel_i),
                .wbm_we_i(wbm_we_i),
                .wbm_cyc_i(wbm_cyc_i),
                .wbm_stb_i(wbm_stb_i),
                .wbm_cti_i(wbm_cti_i),
                .wbm_bte_i(wbm_bte_i),
                .wbm_dat_o(wbm_dat_o),
                .wbm_ack_o(wbm_ack_o),
                .wbm_err_o(wbm_err_o),
                .wbm_rty_o(wbm_rty_o),
                .wbs_adr_o(wb_mem_adr_s),
                .wbs_dat_o(wb_mem_dat_s),
                .wbs_sel_o(wb_mem_sel_s),
                .wbs_we_o(wb_mem_we_s),
                .wbs_cyc_o(),
                .wbs_stb_o(wb_mem_stb_s),
                .wbs_cti_o(),
                .wbs_bte_o(),
                .wbs_dat_i(wb_mem_rdt_s),
                .wbs_ack_i(wb_mem_ack_s),
                .wbs_err_i(1'b0),
                .wbs_rty_i(1'b0)
            );

            jtag_wb_bridge #(
                .chain(1)
            ) jtag_wb (
                .i_clk(i_clk),
                .i_rst(i_rst),
                .o_wb_adr(wb_jtag_adr),
                .o_wb_dat(wb_jtag_dat),
                .o_wb_sel(wb_jtag_sel),
                .o_wb_we(wb_jtag_we),
                .o_wb_cyc(wb_jtag_cyc),
                .o_wb_stb(wb_jtag_stb),
                .i_wb_rdt(wb_jtag_rdt),
                .i_wb_ack(wb_jtag_ack),
                .o_cmd_reset(rst_cmd_jtag)
            );
        end else begin : gen_no_jtag_wb
            assign wb_mem_adr_s = wb_mem_adr;
            assign wb_mem_dat_s = wb_mem_dat;
            assign wb_mem_sel_s = wb_mem_sel;
            assign wb_mem_we_s = wb_mem_we;
            assign wb_mem_stb_s = wb_mem_stb;
            assign wb_mem_rdt_cpu = wb_mem_rdt_s;
            assign wb_mem_ack_cpu = wb_mem_ack_s;
            assign rst_cmd_jtag = 1'b0;
        end
    endgenerate

    memory #(
        .memfile(memfile),
        .memsize(memsize),
        .sim(sim)
    ) memory (
        .i_clk(i_clk),
        .i_rst(i_rst),
        .i_wb_rst(rst_wb),
        .i_wb_adr(wb_mem_adr_s),
        .i_wb_dat(wb_mem_dat_s),
        .i_wb_sel(wb_mem_sel_s),
        .i_wb_we(wb_mem_we_s),
        .i_wb_stb(wb_mem_stb_s),
        .o_wb_rdt(wb_mem_rdt_s),
        .o_wb_ack(wb_mem_ack_s)
    );

    mcu_peripherals peripherals (
        .i_clk(i_clk),
        .i_rst(rst),
        .i_wb_adr(wb_ext_adr),
        .i_wb_dat(wb_ext_dat),
        .i_wb_sel(wb_ext_sel),
        .i_wb_we(wb_ext_we),
        .i_wb_stb(wb_ext_stb),
        .o_wb_rdt(wb_ext_rdt),
        .o_wb_ack(wb_ext_ack),
        // Peripheral IO
        .i_gpio(GPI),
        .o_gpio(GPO),
        .o_timer_irq(timer_irq),
        .o_core_reset(rst_mem_peripherals)
    );

endmodule

module cpu #(
    parameter sim = 1'b0,
    parameter WITH_CSR = 1,
    parameter rf_width = 8
)(
    input wire i_clk,
    input wire i_rst,
    input wire i_timer_irq,
    // CPU->memory
    output wire [31:0] o_wb_mem_adr,
    output wire [31:0] o_wb_mem_dat,
    output wire [3:0] o_wb_mem_sel,
    output wire o_wb_mem_we,
    output wire o_wb_mem_stb,
    input wire [31:0] i_wb_mem_rdt,
    input wire i_wb_mem_ack,
    // CPU->peripherals
    output wire [31:0] o_wb_ext_adr,
    output wire [31:0] o_wb_ext_dat,
    output wire [3:0] o_wb_ext_sel,
    output wire o_wb_ext_we,
    output wire o_wb_ext_stb,
    input wire [31:0] i_wb_ext_rdt,
    input wire i_wb_ext_ack
);
    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;

    // SERV core with mux splitting dbus into mem and ext and
    // arbiter combining mem and ibus.
    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(i_rst),
        .i_timer_irq(i_timer_irq),

        .o_wb_mem_adr(o_wb_mem_adr),
        .o_wb_mem_dat(o_wb_mem_dat),
        .o_wb_mem_sel(o_wb_mem_sel),
        .o_wb_mem_we(o_wb_mem_we),
        .o_wb_mem_stb(o_wb_mem_stb),
        .i_wb_mem_rdt(i_wb_mem_rdt),
        .i_wb_mem_ack(i_wb_mem_ack),

        .o_wb_ext_adr(o_wb_ext_adr),
        .o_wb_ext_dat(o_wb_ext_dat),
        .o_wb_ext_sel(o_wb_ext_sel),
        .o_wb_ext_we(o_wb_ext_we),
        .o_wb_ext_stb(o_wb_ext_stb),
        .i_wb_ext_rdt(i_wb_ext_rdt),
        .i_wb_ext_ack(i_wb_ext_ack),

        .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)
    );

    serv_rf_ram #(
        .width(rf_width),
        .csr_regs(WITH_CSR*4)
    ) rf_ram (
        .i_clk(i_clk),
        .i_waddr(rf_waddr),
        .i_wdata(rf_wdata),
        .i_wen(rf_wen),
        .i_raddr(rf_raddr),
        .i_ren(rf_ren),
        .o_rdata(rf_rdata)
    );
endmodule

module memory #(
    parameter memfile = "",
    parameter memsize = 8192,
    parameter sim = 1'b0
)(
    input wire i_clk,
    input wire i_rst,
    input wire i_wb_rst,
    input wire [31:0] i_wb_adr,
    input wire [31:0] i_wb_dat,
    input wire [3:0] i_wb_sel,
    input wire i_wb_we,
    input wire i_wb_stb,
    output wire [31:0] o_wb_rdt,
    output wire o_wb_ack
);
    localparam mem_depth = memsize/4;
    localparam mem_aw = `CLOG2(mem_depth);

    reg [31:0] mem [0:mem_depth-1] /* verilator public */;
    reg [31:0] wb_rdt_r;
    reg wb_ack_r;
    wire [mem_aw-1:0] wb_word_adr = i_wb_adr[mem_aw+1:2];

    assign o_wb_rdt = wb_rdt_r;
    assign o_wb_ack = wb_ack_r;

    always @(posedge i_clk) begin
        if (i_rst || i_wb_rst) begin
            wb_ack_r <= 1'b0;
            wb_rdt_r <= 32'b0;
        end else begin
            wb_ack_r <= i_wb_stb & ~wb_ack_r;

            if (i_wb_stb & ~wb_ack_r) begin
                wb_rdt_r <= mem[wb_word_adr];

                if (i_wb_we) begin
                    if (i_wb_sel[0]) mem[wb_word_adr][7:0] <= i_wb_dat[7:0];
                    if (i_wb_sel[1]) mem[wb_word_adr][15:8] <= i_wb_dat[15:8];
                    if (i_wb_sel[2]) mem[wb_word_adr][23:16] <= i_wb_dat[23:16];
                    if (i_wb_sel[3]) mem[wb_word_adr][31:24] <= i_wb_dat[31:24];
                end
            end
        end
    end

    integer i;
    initial begin
        if (sim == 1'b1) begin
            for (i = 0; i < mem_depth; i = i + 1)
                mem[i] = 32'h00000000;
        end
        if (|memfile) begin
            $display("Preloading %m from %s", memfile);
            $readmemh(memfile, mem);
        end
        wb_rdt_r = 32'b0;
        wb_ack_r = 1'b0;
    end

endmodule