// retroDE_ps2 — tb_gs_image_xfer_swizzle_psmt4 (Ch139)
//
// Write-side wiring of the Ch137 PSMT4 swizzle math primitive
// into gif_image_xfer_stub. PSMT4 TRXDIR uploads now route their
// per-pixel VRAM byte address through gs_swizzle_psmt4_stub when
// the new `PSMT4_SWIZZLE=1` parameter is set, AND the per-pixel
// nibble-mask selector keys on the swizzle's `nibble_hi` output
// (instead of the linear `x_eff[0]`) so the upload's 4-bit
// pixel hits the right nibble of the swizzled byte. PSMCT32 is
// gated by PSMCT32_SWIZZLE (Ch121), PSMCT16 by PSMCT16_SWIZZLE
// (Ch127), PSMT8 by PSMT8_SWIZZLE (Ch133). Default 0 keeps the
// legacy linear path for every existing PSMT4 image-xfer TB
// (Ch118 etc.). Mirrors the Ch127 / Ch133 wiring shape but for
// the fourth (and last) PSM, and uses the Ch138 swizzled-pcrtc
// as the read-side cross-check.
//
// Topology:
//
//   TB.gif_reg_wr_*  →  gs_stub (TRX latches only)
//                     → gif_image_xfer_stub#(PSMT4_SWIZZLE=1)
//                     → vram_stub
//                     → gs_pcrtc_stub#(PSMT4_SWIZZLE=1) → r/g/b
//
// Three-phase verification (mirrors Ch127 / Ch133 audit-closed
// shape).
//
// PHASE 1 (write-side lock): TRXDIR upload of a 16×4 PSMT4 image
// at DBP=0, DBW=2, DSAX=DSAY=0. Each pixel has a unique-per-
// (xx, yy) 4-bit nibble value `nibble_at(x, y) = ((y << 1) ^ x)
// & 4'h7` ∈ [0..7]. After upload, TB reads VRAM via the second
// read port at the SWIZZLED byte address (computed by an in-TB
// ref_addr_psmt4() that mirrors gs_swizzle_psmt4_stub — block
// table + column table both transcribed verbatim from the
// Ch137-locked PCSX2 source) and asserts each pixel nibble
// landed at the correct (byte, nibble_hi) slot.
//
// Strict linear-vs-swizzled separator: at FBP=0/DBW=2, the 16×4
// PSMT4 image lives in block (0,0) of page (0,0). columnTable4
// values for yb=0..3, xb=0..15 cover nibble_idx values up to
// 127 → byte_in_block ∈ [0..63]. Under LINEAR the row stride =
// DBW*32 = 64 bytes, so y=1 row starts at byte 64, strictly
// outside the swizzled write set [0..63]. Assert byte 64 stays
// 0 (pre-coloring would conflict with the test-region setup).
//
// PHASE 2 (end-to-end agreement): without changing VRAM, enable
// the Ch138 swizzled-pcrtc path on the same VRAM and capture
// the frame. Both modules instantiate the same
// gs_swizzle_psmt4_stub (and use its nibble_hi output for the
// nibble selector), so a successful frame proves byte-level
// agreement under DMAC-style emit + scanout-style read. CLUT
// disabled → PCRTC's PSMT4 grayscale fallback gives R=G=B=
// {nibble, nibble} at scanout.
//
// PHASE 3 (non-origin transfer): re-arm with DBP=8, DBW=2,
// DSAX=28, DSAY=12, RRW=8, RRH=8. Effective coords (28..35,
// 12..19) cross block_x=0→1 at effective_x=32 AND block_y=0→1
// at effective_y=16, so all 4 corner blocks of page (0,0) at
// DBP=8 are visited (blockTable4[0][0]=0, [0][1]=2, [1][0]=1,
// [1][1]=3 → block bases 2048/2560/2304/2816). Pins three
// contracts the origin transfer can't:
//   (a) `dest_base_q (= DBP*256)` is correctly ADDED ON TOP of
//       the swizzle output (DBP=0 in P1 would mask a missing-
//       add).
//   (b) The swizzle is fed the FULL effective coords (DSAX/DSAY
//       both nonzero, with DSAX=28 specifically chosen so that
//       low bits engage non-trivial xb values).
//   (c) BOTH block_x and block_y propagate through blockTable4 —
//       block_x=0 throughout Phase 1 would mask a tied-block_x
//       regression.
//
// Phase 3 strict separator: linear formula puts effective coord
// (28, 12) at byte:
//   DBP*256 + (12 * DBW*32) + (28/2) = 2048 + 768 + 14 = 2830
// Under linear, the pixel's nibble_hi=x_eff[0]=0 → low nibble of
// byte 2830 holds nibble_at(0,0)=0 (which is 0 → byte stays at
// its nibble-RMW-merged result). The neighboring linear pixel
// (29, 12) writes high nibble (x_eff[0]=1) → high nibble = 1.
// So under LINEAR fall-through, byte 2830 ends up at 0x10.
// Under SWIZZLED, the Phase-3 write set fills specific nibbles
// inside the 4 block-bases [2048/2304/2560/2816+/256-byte
// extents]. Byte 2830 sits inside block (1,1)'s extent
// [2816..3071], but no Phase-3 swizzled write hits byte 2830 in
// particular (cross-checked by computing `nibble_idx >> 1` for
// every pixel — none produces (block_idx*256+14)). Assert byte
// 2830 stays 0 — proves a fall-through to linear would have
// stomped it with 0x10.

`timescale 1ns/1ps

module tb_gs_image_xfer_swizzle_psmt4;

    localparam int PCRTC_H_ACTIVE = 16;
    localparam int PCRTC_V_ACTIVE = 4;

    logic clk;
    logic rst_n;
    initial clk = 1'b0;
    always #5 clk = ~clk;

    // ------------------------------------------------------------
    // gs_stub — only used here for its TRX-register latches +
    // trxdir_wr_q pulse. Everything else is unused.
    // ------------------------------------------------------------
    logic        gif_reg_wr_en;
    logic [7:0]  gif_reg_num;
    logic [63:0] gif_reg_data;

    logic [63:0] tb_pmode_q;
    logic [63:0] tb_dispfb1_q;
    logic [63:0] tb_display1_q;

    logic [7:0]          bg_r, bg_g, bg_b;
    logic [63:0]         pmode_q, dispfb1_q, display1_q;
    logic [63:0]         prim_q, rgbaq_q, xyz2_q, xyzf2_q, frame_1_q, zbuf_1_q;
    logic [63:0]         tex0_1_q;
    logic [13:0]         tex0_1_cbp_q;
    logic [3:0]          tex0_1_cpsm_q;
    logic                tex0_1_csm_q;
    logic [4:0]          tex0_1_csa_q;
    logic [2:0]          tex0_1_cld_q;
    logic                tex0_1_wr_q;
    logic [63:0]         bitbltbuf_q, trxpos_q, trxreg_q, trxdir_q;
    logic                trxdir_wr_q;
    logic                prim_complete;
    logic [31:0]         prim_complete_count;
    logic [63:0]         prim_v0_q, prim_v1_q, prim_v2_q;
    logic [63:0]         prim_color_q;
    logic [63:0]         prim_color_v0_q, prim_color_v1_q, prim_color_v2_q;
    trace_pkg::vertex_t  prim_v0_decoded_q, prim_v1_decoded_q, prim_v2_decoded_q;
    trace_pkg::color_t   prim_v0_color_decoded_q, prim_v1_color_decoded_q, prim_v2_color_decoded_q;
    logic                pixel_emit;
    logic [31:0]         pixel_emit_count;
    logic [11:0]         pixel_x_q, pixel_y_q;
    logic [63:0]         pixel_color_q;
    logic [8:0]          pixel_fbp_q;
    logic [5:0]          pixel_fbw_q, pixel_psm_q;
    logic [31:0]         pixel_fb_addr_q;
    logic                raster_pixel_emit;
    logic [31:0]         raster_pixel_emit_count;
    logic [11:0]         raster_pixel_x_q, raster_pixel_y_q;
    logic [63:0]         raster_pixel_color_q;
    logic [31:0]         raster_pixel_fb_addr_q;
    logic [3:0]          raster_pixel_be_q;
    logic [31:0]         raster_pixel_mask_q;
    logic [5:0]          raster_pixel_psm_q;
    logic                raster_active;
    logic                raster_overflow;
    logic                raster_degenerate;
    logic                gs_ev_valid;
    trace_pkg::subsys_e  gs_ev_subsys;
    trace_pkg::event_e   gs_ev_event;
    logic [63:0]         gs_ev_arg0, gs_ev_arg1, gs_ev_arg2, gs_ev_arg3;
    logic [31:0]         gs_ev_flags;

    gs_stub u_gs (
        .clk(clk), .rst_n(rst_n),
        .reg_wr_en(1'b0), .reg_wr_addr(16'd0), .reg_wr_data(64'd0),
        .gif_reg_wr_en(gif_reg_wr_en),
        .gif_reg_num(gif_reg_num),
        .gif_reg_data(gif_reg_data),
        .bg_r(bg_r), .bg_g(bg_g), .bg_b(bg_b),
        .pmode_q(pmode_q), .dispfb1_q(dispfb1_q), .display1_q(display1_q),
        .prim_q(prim_q), .rgbaq_q(rgbaq_q),
        .xyz2_q(xyz2_q), .xyzf2_q(xyzf2_q),
        .frame_1_q(frame_1_q), .zbuf_1_q(zbuf_1_q),
        .tex0_1_q(tex0_1_q),
        .tex0_1_cbp_q(tex0_1_cbp_q),
        .tex0_1_cpsm_q(tex0_1_cpsm_q),
        .tex0_1_csm_q(tex0_1_csm_q),
        .tex0_1_csa_q(tex0_1_csa_q),
        .tex0_1_cld_q(tex0_1_cld_q),
        .tex0_1_wr_q(tex0_1_wr_q),
        .bitbltbuf_q(bitbltbuf_q),
        .trxpos_q(trxpos_q),
        .trxreg_q(trxreg_q),
        .trxdir_q(trxdir_q),
        .trxdir_wr_q(trxdir_wr_q),
        .prim_complete(prim_complete),
        .prim_complete_count(prim_complete_count),
        .prim_v0_q(prim_v0_q), .prim_v1_q(prim_v1_q), .prim_v2_q(prim_v2_q),
        .prim_color_q(prim_color_q),
        .prim_color_v0_q(prim_color_v0_q),
        .prim_color_v1_q(prim_color_v1_q),
        .prim_color_v2_q(prim_color_v2_q),
        .prim_v0_decoded_q(prim_v0_decoded_q),
        .prim_v1_decoded_q(prim_v1_decoded_q),
        .prim_v2_decoded_q(prim_v2_decoded_q),
        .prim_v0_color_decoded_q(prim_v0_color_decoded_q),
        .prim_v1_color_decoded_q(prim_v1_color_decoded_q),
        .prim_v2_color_decoded_q(prim_v2_color_decoded_q),
        .pixel_emit(pixel_emit),
        .pixel_emit_count(pixel_emit_count),
        .pixel_x_q(pixel_x_q), .pixel_y_q(pixel_y_q),
        .pixel_color_q(pixel_color_q),
        .pixel_fbp_q(pixel_fbp_q),
        .pixel_fbw_q(pixel_fbw_q),
        .pixel_psm_q(pixel_psm_q),
        .pixel_fb_addr_q(pixel_fb_addr_q),
        .raster_pixel_emit(raster_pixel_emit),
        .raster_pixel_emit_count(raster_pixel_emit_count),
        .raster_pixel_x_q(raster_pixel_x_q),
        .raster_pixel_y_q(raster_pixel_y_q),
        .raster_pixel_color_q(raster_pixel_color_q),
        .raster_pixel_fb_addr_q(raster_pixel_fb_addr_q),
        .raster_pixel_be_q(raster_pixel_be_q),
        .raster_pixel_mask_q(raster_pixel_mask_q),
        .raster_pixel_psm_q(raster_pixel_psm_q),
        .raster_active(raster_active),
        .raster_overflow(raster_overflow),
        .raster_degenerate(raster_degenerate),
        .ev_valid(gs_ev_valid),
        .ev_subsys(gs_ev_subsys),
        .ev_event(gs_ev_event),
        .ev_arg0(gs_ev_arg0), .ev_arg1(gs_ev_arg1),
        .ev_arg2(gs_ev_arg2), .ev_arg3(gs_ev_arg3),
        .ev_flags(gs_ev_flags)
    );

    // ------------------------------------------------------------
    // gif_image_xfer_stub with the new Ch139 PSMT4_SWIZZLE gate ON
    // (and the Ch121/Ch127/Ch133 gates explicitly OFF — this TB
    // exercises only the PSMT4 path).
    // ------------------------------------------------------------
    logic         xfer_data_valid;
    logic [127:0] xfer_data_qword;
    logic         xfer_data_last;
    logic         xfer_data_ready;
    logic         xfer_we;
    logic [31:0]  xfer_waddr;
    logic [31:0]  xfer_wdata;
    logic [3:0]   xfer_wbe;
    logic [31:0]  xfer_wmask;
    logic         xfer_busy;

    gif_image_xfer_stub #(
        .PSMCT32_SWIZZLE(1'b0),
        .PSMCT16_SWIZZLE(1'b0),
        .PSMT8_SWIZZLE  (1'b0),
        .PSMT4_SWIZZLE  (1'b1)
    ) u_xfer (
        .clk(clk), .rst_n(rst_n),
        .trxdir_wr_pulse(trxdir_wr_q),
        .trxdir(trxdir_q),
        .bitbltbuf(bitbltbuf_q),
        .trxpos(trxpos_q),
        .trxreg(trxreg_q),
        .data_valid(xfer_data_valid),
        .data_qword(xfer_data_qword),
        .data_last (xfer_data_last),
        .data_ready(xfer_data_ready),
        .vram_we   (xfer_we),
        .vram_waddr(xfer_waddr),
        .vram_wdata(xfer_wdata),
        .vram_wbe  (xfer_wbe),
        .vram_wmask(xfer_wmask),
        .busy      (xfer_busy)
    );

    // ------------------------------------------------------------
    // vram_stub. Two readers: pcrtc (Phase 2) + TB (Phase 1/3).
    // ------------------------------------------------------------
    logic [31:0] vram_raddr;
    logic [31:0] vram_rdata;
    logic [31:0] vram_raddr2;
    logic [31:0] vram_rdata2;

    vram_stub #(.BYTES(16384)) u_vram (
        .clk(clk), .rst_n(rst_n),
        .write_en  (xfer_we),
        .write_addr(xfer_waddr),
        .write_data(xfer_wdata),
        .write_be  (xfer_wbe),
        .write_mask(xfer_wmask),
        .read_addr (vram_raddr),
        .read_data (vram_rdata),
        .read2_addr(vram_raddr2),
        .read2_data(vram_rdata2)
    );

    // ------------------------------------------------------------
    // gs_pcrtc_stub with the Ch138 PSMT4_SWIZZLE gate ON (and the
    // Ch120/Ch126/Ch132 gates explicitly OFF — this TB exercises
    // only the PSMT4 scanout path). CLUT disabled.
    // ------------------------------------------------------------
    logic       hsync_o, vsync_o, de_o;
    logic [7:0] r_o, g_o, b_o;
    logic                pcrtc_ev_valid;
    trace_pkg::subsys_e  pcrtc_ev_subsys;
    trace_pkg::event_e   pcrtc_ev_event;
    logic [63:0]         pcrtc_ev_arg0, pcrtc_ev_arg1;
    logic [63:0]         pcrtc_ev_arg2, pcrtc_ev_arg3;
    logic [31:0]         pcrtc_ev_flags;

    gs_pcrtc_stub #(
        .H_ACTIVE(PCRTC_H_ACTIVE), .H_FRONT(1), .H_SYNC(1), .H_BACK(1),
        .V_ACTIVE(PCRTC_V_ACTIVE), .V_FRONT(1), .V_SYNC(1), .V_BACK(1),
        .PSMCT32_SWIZZLE(1'b0),
        .PSMCT16_SWIZZLE(1'b0),
        .PSMT8_SWIZZLE  (1'b0),
        .PSMT4_SWIZZLE  (1'b1)
    ) u_pcrtc (
        .clk(clk), .rst_n(rst_n),
        .pmode_q       (tb_pmode_q),
        .dispfb1_q     (tb_dispfb1_q),
        .display1_q    (tb_display1_q),
        .vram_read_addr(vram_raddr),
        .vram_read_data(vram_rdata),
        .clut_enable   (1'b0),
        .clut_csa      (5'd0),
        .clut_read_idx (),
        .clut_read_data(32'd0),
        .hsync(hsync_o), .vsync(vsync_o), .de(de_o),
        .r(r_o), .g(g_o), .b(b_o),
        .ev_valid(pcrtc_ev_valid),
        .ev_subsys(pcrtc_ev_subsys),
        .ev_event(pcrtc_ev_event),
        .ev_arg0(pcrtc_ev_arg0), .ev_arg1(pcrtc_ev_arg1),
        .ev_arg2(pcrtc_ev_arg2), .ev_arg3(pcrtc_ev_arg3),
        .ev_flags(pcrtc_ev_flags)
    );

    // ------------------------------------------------------------
    // Counters + invariants observers.
    // ------------------------------------------------------------
    int  errors;
    int  xfer_writes_seen;
    int  trxdir_arms_seen;

    initial begin
        errors           = 0;
        xfer_writes_seen = 0;
        trxdir_arms_seen = 0;
    end

    always_ff @(posedge clk) begin
        if (rst_n && trxdir_wr_q)
            trxdir_arms_seen <= trxdir_arms_seen + 1;
        if (rst_n && xfer_we) begin
            xfer_writes_seen <= xfer_writes_seen + 1;
            // Ch139 — every emit is PSMT4: be=4'b0001 (single-byte
            // commit), mask=0x0F or 0xF0 (nibble RMW).
            if (xfer_wbe !== 4'b0001) begin
                $error("xfer write %0d: be=%b (expected 4'b0001 for PSMT4)",
                       xfer_writes_seen, xfer_wbe);
                errors <= errors + 1;
            end
            if (xfer_wmask !== 32'h0000_000F
                && xfer_wmask !== 32'h0000_00F0) begin
                $error("xfer write %0d: mask=0x%08x (expected 0x0F or 0xF0 for PSMT4)",
                       xfer_writes_seen, xfer_wmask);
                errors <= errors + 1;
            end
        end
    end

    // ------------------------------------------------------------
    // Frame capture (Phase 2).
    // ------------------------------------------------------------
    logic [7:0] cap_r [0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];
    logic [7:0] cap_g [0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];
    logic [7:0] cap_b [0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];
    logic       cap_de[0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];
    bit         capture_armed;

    initial begin
        for (int y = 0; y < PCRTC_V_ACTIVE; y++)
            for (int x = 0; x < PCRTC_H_ACTIVE; x++) begin
                cap_r[y][x]  = 8'd0;
                cap_g[y][x]  = 8'd0;
                cap_b[y][x]  = 8'd0;
                cap_de[y][x] = 1'b0;
            end
        capture_armed = 1'b0;
    end

    always_ff @(posedge clk) begin
        if (rst_n && capture_armed && de_o
            && (u_pcrtc.vcnt < PCRTC_V_ACTIVE)
            && (u_pcrtc.hcnt < PCRTC_H_ACTIVE)) begin
            cap_r [u_pcrtc.vcnt][u_pcrtc.hcnt] <= r_o;
            cap_g [u_pcrtc.vcnt][u_pcrtc.hcnt] <= g_o;
            cap_b [u_pcrtc.vcnt][u_pcrtc.hcnt] <= b_o;
            cap_de[u_pcrtc.vcnt][u_pcrtc.hcnt] <= 1'b1;
        end
    end

    // ------------------------------------------------------------
    // GIF / TRX register helpers.
    // ------------------------------------------------------------
    task automatic step_drive(input logic        wr_en,
                              input logic [7:0]  num,
                              input logic [63:0] data);
        @(negedge clk);
        gif_reg_wr_en = wr_en;
        gif_reg_num   = num;
        gif_reg_data  = data;
        @(posedge clk);
    endtask

    task automatic drive_reg(input logic [7:0] num, input logic [63:0] data);
        step_drive(1'b1, num, data);
    endtask

    task automatic drive_idle();
        step_drive(1'b0, 8'd0, 64'd0);
    endtask

    function automatic logic [63:0] bitbltbuf_pack(
        input logic [13:0] dbp_v,
        input logic [5:0]  dbw_v,
        input logic [5:0]  dpsm_v);
        logic [63:0] v;
        v = 64'd0;
        v[45:32] = dbp_v;
        v[53:48] = dbw_v;
        v[61:56] = dpsm_v;
        return v;
    endfunction
    function automatic logic [63:0] trxpos_pack(
        input logic [10:0] dsax_v,
        input logic [10:0] dsay_v);
        logic [63:0] v;
        v = 64'd0;
        v[42:32] = dsax_v;
        v[58:48] = dsay_v;
        return v;
    endfunction
    function automatic logic [63:0] trxreg_pack(
        input logic [11:0] rrw_v,
        input logic [11:0] rrh_v);
        logic [63:0] v;
        v = 64'd0;
        v[11:0]  = rrw_v;
        v[43:32] = rrh_v;
        return v;
    endfunction
    function automatic logic [63:0] trxdir_pack(input logic [1:0] xdir_v);
        return {62'd0, xdir_v};
    endfunction

    task automatic push_image_qword(input logic [127:0] qword,
                                    input logic         last);
        @(negedge clk);
        wait (xfer_data_ready == 1'b1);
        @(negedge clk);
        xfer_data_valid = 1'b1;
        xfer_data_qword = qword;
        xfer_data_last  = last;
        @(posedge clk);
        @(negedge clk);
        xfer_data_valid = 1'b0;
        xfer_data_qword = 128'd0;
        xfer_data_last  = 1'b0;
    endtask

    // ------------------------------------------------------------
    // PSMT4 swizzle reference — block + column tables verbatim
    // from PCSX2 (Ch137 source-locked).
    // ------------------------------------------------------------
    function automatic int ref_block_idx4(input int by, input int bx);
        case ({by[2:0], bx[1:0]})
            5'd0:  return  0;  5'd1:  return  2;  5'd2:  return  8;  5'd3:  return 10;
            5'd4:  return  1;  5'd5:  return  3;  5'd6:  return  9;  5'd7:  return 11;
            5'd8:  return  4;  5'd9:  return  6;  5'd10: return 12;  5'd11: return 14;
            5'd12: return  5;  5'd13: return  7;  5'd14: return 13;  5'd15: return 15;
            5'd16: return 16;  5'd17: return 18;  5'd18: return 24;  5'd19: return 26;
            5'd20: return 17;  5'd21: return 19;  5'd22: return 25;  5'd23: return 27;
            5'd24: return 20;  5'd25: return 22;  5'd26: return 28;  5'd27: return 30;
            5'd28: return 21;  5'd29: return 23;  5'd30: return 29;  default: return 31;
        endcase
    endfunction

    function automatic int ref_col_idx4(input int yb, input int xb);
        // 512-entry column table — only the rows actually touched
        // by Phase 1 (yb=0..3) and Phase 3 (yb=0..3 again) are
        // exercised. Phase 1 region uses yb=0..3, xb=0..15.
        // Phase 3 effective yb = ey%16 where ey∈[12..19] →
        // yb∈{12..15, 0..3}. We need yb=0..3 AND yb=12..15.
        case ({yb[3:0], xb[4:0]})
            // yb=0
            9'd0:   return   0; 9'd1:   return   8; 9'd2:   return  32; 9'd3:   return  40;
            9'd4:   return  64; 9'd5:   return  72; 9'd6:   return  96; 9'd7:   return 104;
            9'd8:   return   2; 9'd9:   return  10; 9'd10:  return  34; 9'd11:  return  42;
            9'd12:  return  66; 9'd13:  return  74; 9'd14:  return  98; 9'd15:  return 106;
            9'd16:  return   4; 9'd17:  return  12; 9'd18:  return  36; 9'd19:  return  44;
            9'd20:  return  68; 9'd21:  return  76; 9'd22:  return 100; 9'd23:  return 108;
            9'd24:  return   6; 9'd25:  return  14; 9'd26:  return  38; 9'd27:  return  46;
            9'd28:  return  70; 9'd29:  return  78; 9'd30:  return 102; 9'd31:  return 110;
            // yb=1
            9'd32:  return  16; 9'd33:  return  24; 9'd34:  return  48; 9'd35:  return  56;
            9'd36:  return  80; 9'd37:  return  88; 9'd38:  return 112; 9'd39:  return 120;
            9'd40:  return  18; 9'd41:  return  26; 9'd42:  return  50; 9'd43:  return  58;
            9'd44:  return  82; 9'd45:  return  90; 9'd46:  return 114; 9'd47:  return 122;
            9'd48:  return  20; 9'd49:  return  28; 9'd50:  return  52; 9'd51:  return  60;
            9'd52:  return  84; 9'd53:  return  92; 9'd54:  return 116; 9'd55:  return 124;
            9'd56:  return  22; 9'd57:  return  30; 9'd58:  return  54; 9'd59:  return  62;
            9'd60:  return  86; 9'd61:  return  94; 9'd62:  return 118; 9'd63:  return 126;
            // yb=2
            9'd64:  return  65; 9'd65:  return  73; 9'd66:  return  97; 9'd67:  return 105;
            9'd68:  return   1; 9'd69:  return   9; 9'd70:  return  33; 9'd71:  return  41;
            9'd72:  return  67; 9'd73:  return  75; 9'd74:  return  99; 9'd75:  return 107;
            9'd76:  return   3; 9'd77:  return  11; 9'd78:  return  35; 9'd79:  return  43;
            9'd80:  return  69; 9'd81:  return  77; 9'd82:  return 101; 9'd83:  return 109;
            9'd84:  return   5; 9'd85:  return  13; 9'd86:  return  37; 9'd87:  return  45;
            9'd88:  return  71; 9'd89:  return  79; 9'd90:  return 103; 9'd91:  return 111;
            9'd92:  return   7; 9'd93:  return  15; 9'd94:  return  39; 9'd95:  return  47;
            // yb=3
            9'd96:  return  81; 9'd97:  return  89; 9'd98:  return 113; 9'd99:  return 121;
            9'd100: return  17; 9'd101: return  25; 9'd102: return  49; 9'd103: return  57;
            9'd104: return  83; 9'd105: return  91; 9'd106: return 115; 9'd107: return 123;
            9'd108: return  19; 9'd109: return  27; 9'd110: return  51; 9'd111: return  59;
            9'd112: return  85; 9'd113: return  93; 9'd114: return 117; 9'd115: return 125;
            9'd116: return  21; 9'd117: return  29; 9'd118: return  53; 9'd119: return  61;
            9'd120: return  87; 9'd121: return  95; 9'd122: return 119; 9'd123: return 127;
            9'd124: return  23; 9'd125: return  31; 9'd126: return  55; 9'd127: return  63;
            // yb=12
            9'd384: return 448; 9'd385: return 456; 9'd386: return 480; 9'd387: return 488;
            9'd388: return 384; 9'd389: return 392; 9'd390: return 416; 9'd391: return 424;
            9'd392: return 450; 9'd393: return 458; 9'd394: return 482; 9'd395: return 490;
            9'd396: return 386; 9'd397: return 394; 9'd398: return 418; 9'd399: return 426;
            9'd400: return 452; 9'd401: return 460; 9'd402: return 484; 9'd403: return 492;
            9'd404: return 388; 9'd405: return 396; 9'd406: return 420; 9'd407: return 428;
            9'd408: return 454; 9'd409: return 462; 9'd410: return 486; 9'd411: return 494;
            9'd412: return 390; 9'd413: return 398; 9'd414: return 422; 9'd415: return 430;
            // yb=13
            9'd416: return 464; 9'd417: return 472; 9'd418: return 496; 9'd419: return 504;
            9'd420: return 400; 9'd421: return 408; 9'd422: return 432; 9'd423: return 440;
            9'd424: return 466; 9'd425: return 474; 9'd426: return 498; 9'd427: return 506;
            9'd428: return 402; 9'd429: return 410; 9'd430: return 434; 9'd431: return 442;
            9'd432: return 468; 9'd433: return 476; 9'd434: return 500; 9'd435: return 508;
            9'd436: return 404; 9'd437: return 412; 9'd438: return 436; 9'd439: return 444;
            9'd440: return 470; 9'd441: return 478; 9'd442: return 502; 9'd443: return 510;
            9'd444: return 406; 9'd445: return 414; 9'd446: return 438; 9'd447: return 446;
            // yb=14
            9'd448: return 385; 9'd449: return 393; 9'd450: return 417; 9'd451: return 425;
            9'd452: return 449; 9'd453: return 457; 9'd454: return 481; 9'd455: return 489;
            9'd456: return 387; 9'd457: return 395; 9'd458: return 419; 9'd459: return 427;
            9'd460: return 451; 9'd461: return 459; 9'd462: return 483; 9'd463: return 491;
            9'd464: return 389; 9'd465: return 397; 9'd466: return 421; 9'd467: return 429;
            9'd468: return 453; 9'd469: return 461; 9'd470: return 485; 9'd471: return 493;
            9'd472: return 391; 9'd473: return 399; 9'd474: return 423; 9'd475: return 431;
            9'd476: return 455; 9'd477: return 463; 9'd478: return 487; 9'd479: return 495;
            // yb=15
            9'd480: return 401; 9'd481: return 409; 9'd482: return 433; 9'd483: return 441;
            9'd484: return 465; 9'd485: return 473; 9'd486: return 497; 9'd487: return 505;
            9'd488: return 403; 9'd489: return 411; 9'd490: return 435; 9'd491: return 443;
            9'd492: return 467; 9'd493: return 475; 9'd494: return 499; 9'd495: return 507;
            9'd496: return 405; 9'd497: return 413; 9'd498: return 437; 9'd499: return 445;
            9'd500: return 469; 9'd501: return 477; 9'd502: return 501; 9'd503: return 509;
            9'd504: return 407; 9'd505: return 415; 9'd506: return 439; 9'd507: return 447;
            9'd508: return 471; 9'd509: return 479; 9'd510: return 503; default: return 511;
        endcase
    endfunction

    typedef struct packed {
        logic [31:0] addr;
        logic        nibble_hi;
    } psmt4_pos_t;

    function automatic psmt4_pos_t ref_pos_psmt4(
        input int fbp_v, input int fbw_v, input int x_v, input int y_v);
        psmt4_pos_t r;
        int page_x, page_y, bw_pg_v, page_idx, page_base;
        int by, bx, blk_idx, xb, yb, nibble_idx;
        int addr_v;
        page_x   = x_v / 128;
        page_y   = y_v / 128;
        bw_pg_v  = fbw_v / 2;
        page_idx = page_y * bw_pg_v + page_x;
        page_base = fbp_v * 2048 + page_idx * 8192;
        by  = (y_v % 128) / 16;
        bx  = (x_v % 128) / 32;
        blk_idx = ref_block_idx4(by, bx);
        xb = x_v % 32;
        yb = y_v % 16;
        nibble_idx = ref_col_idx4(yb, xb);
        addr_v = page_base + blk_idx * 256 + (nibble_idx >> 1);
        r.addr      = addr_v[31:0];
        r.nibble_hi = 1'(nibble_idx & 1);
        return r;
    endfunction

    // -----------------------------------------------------------
    // Per-pixel nibble pattern (matches Ch138 PSMT4 scanout TB).
    //   nibble_at(x, y) = ((y << 1) ^ x) & 4'h7  ∈ [0..7]
    // PCRTC PSMT4 grayscale fallback gives r=g=b={n, n} at scanout.
    // -----------------------------------------------------------
    function automatic logic [3:0] nibble_at(input int x, input int y);
        return 4'(((y << 1) ^ x) & 4'h7);
    endfunction

    function automatic logic [7:0] gray_at(input int x, input int y);
        logic [3:0] n;
        n = nibble_at(x, y);
        return {n, n};
    endfunction

    // ------------------------------------------------------------
    // TB-side nibble readback at a swizzled (addr, nibble_hi) slot.
    // ------------------------------------------------------------
    task automatic check_pixel_at_swizzled(input int x, input int y,
                                           input int fbp_v, input int fbw_v);
        psmt4_pos_t  pos;
        logic [3:0]  expected;
        logic [3:0]  got;
        pos      = ref_pos_psmt4(fbp_v, fbw_v, x, y);
        expected = nibble_at(x % 16, y % 16);
        // We re-derive nibble_at using the TB-LOCAL pattern below
        // (defined per phase). Override: the call sites pass the
        // pixel pattern in via wrapper tasks that compute the
        // expected nibble from the test-local (xx, yy) coords.
        // (This task is unused; per-phase verifies inline.)
        vram_raddr2 = pos.addr & ~32'd3;
        #1;
        case (pos.addr[1:0])
            2'b00: got = pos.nibble_hi ? vram_rdata2[7:4]   : vram_rdata2[3:0];
            2'b01: got = pos.nibble_hi ? vram_rdata2[15:12] : vram_rdata2[11:8];
            2'b10: got = pos.nibble_hi ? vram_rdata2[23:20] : vram_rdata2[19:16];
            default: got = pos.nibble_hi ? vram_rdata2[31:28] : vram_rdata2[27:24];
        endcase
        if (got !== expected) begin
            $error("[swizzled-nibble] (%0d,%0d) @addr 0x%08x hi=%0b got 0x%01x expected 0x%01x",
                   x, y, pos.addr, pos.nibble_hi, got, expected);
            errors = errors + 1;
        end
    endtask

    task automatic check_byte_zero(input logic [31:0] byte_addr,
                                   input string       tag);
        // Read the 4-byte word containing byte_addr and check ONLY
        // the targeted byte (selected by addr[1:0]). Earlier
        // versions checked the full word, which produced false
        // failures when a neighbor byte in the same word was
        // independently touched.
        logic [7:0]  got_byte;
        vram_raddr2 = byte_addr & ~32'd3;
        #1;
        case (byte_addr[1:0])
            2'b00:   got_byte = vram_rdata2[7:0];
            2'b01:   got_byte = vram_rdata2[15:8];
            2'b10:   got_byte = vram_rdata2[23:16];
            default: got_byte = vram_rdata2[31:24];
        endcase
        if (got_byte !== 8'd0) begin
            $error("[%s] @byte 0x%08x got 0x%02x expected 0x00",
                   tag, byte_addr, got_byte);
            errors = errors + 1;
        end
    endtask

    // ------------------------------------------------------------
    // Constants.
    // ------------------------------------------------------------
    localparam logic [7:0] R_BITBLTBUF = 8'h50;
    localparam logic [7:0] R_TRXPOS    = 8'h51;
    localparam logic [7:0] R_TRXREG    = 8'h52;
    localparam logic [7:0] R_TRXDIR    = 8'h53;

    // Phase 1/2 transfer parameters (origin: DBP=DSAX=DSAY=0).
    // PSMT4: 32 pixels per IMAGE qword. DBW must be even; minimum
    // is 2.
    localparam int IMG_DBP = 0;
    localparam int DBW     = 2;
    localparam int RRW     = 16;
    localparam int RRH     = 4;
    localparam int NUM_PIXELS = RRW * RRH;       // 64
    // Each PSMT4 IMAGE qword holds 32 pixels; with RRW=16, 2 rows
    // per qword → 64 pixels = 2 qwords.

    // Phase 3 transfer parameters (non-origin: nonzero DBP +
    // nonzero DSAX + nonzero DSAY). PSMT4 blocks are 32×16 px,
    // so DSAX=28 (effective_x crosses 32 mid-row → block_x=0→1)
    // and DSAY=12 (effective_y crosses 16 mid-column → block_y=
    // 0→1) cover all 4 corner blocks of page (0,0) at DBP=8.
    localparam int P3_DBP    = 8;
    localparam int P3_DSAX   = 28;
    localparam int P3_DSAY   = 12;
    localparam int P3_RRW    = 8;
    localparam int P3_RRH    = 8;
    localparam int P3_PIXELS = P3_RRW * P3_RRH;  // 64

    initial begin
        rst_n            = 1'b0;
        gif_reg_wr_en    = 1'b0;
        gif_reg_num      = 8'd0;
        gif_reg_data     = 64'd0;
        xfer_data_valid  = 1'b0;
        xfer_data_qword  = 128'd0;
        xfer_data_last   = 1'b0;
        vram_raddr2      = 32'd0;
        tb_pmode_q       = 64'd0;
        tb_dispfb1_q     = 64'd0;
        tb_display1_q    = 64'd0;

        repeat (4) @(posedge clk);
        rst_n = 1'b1;
        repeat (2) @(posedge clk);

        // ============================================================
        // Phase 1 — origin TRXDIR upload.
        // ============================================================
        drive_reg(R_BITBLTBUF, bitbltbuf_pack(14'(IMG_DBP), 6'(DBW), 6'h14)); // PSMT4
        drive_reg(R_TRXPOS,    trxpos_pack(11'd0, 11'd0));
        drive_reg(R_TRXREG,    trxreg_pack(12'(RRW), 12'(RRH)));
        drive_reg(R_TRXDIR,    trxdir_pack(2'd0));
        drive_idle();

        @(posedge clk);
        @(posedge clk);

        // Push 2 IMAGE qwords (32 PSMT4 pixels each = 2 rows of
        // 16 pixels at RRW=16). Lane 0..15 carry row 2*qi, lane
        // 16..31 carry row 2*qi+1.
        for (int qi = 0; qi < 2; qi++) begin
            logic [127:0] q;
            bit           is_last;
            q = 128'd0;
            for (int i = 0; i < 16; i++) begin
                q[(i+ 0)*4 +: 4] = nibble_at(i, 2*qi);
                q[(i+16)*4 +: 4] = nibble_at(i, 2*qi + 1);
            end
            is_last = (qi == 1);
            push_image_qword(q, is_last);
        end

        wait (xfer_busy == 1'b0);
        repeat (10) @(posedge clk);

        // Phase 1 count check.
        if (trxdir_arms_seen != 1) begin
            $error("after P1: trxdir_wr_q pulses=%0d (expected 1)",
                   trxdir_arms_seen);
            errors = errors + 1;
        end
        if (xfer_writes_seen != NUM_PIXELS) begin
            $error("after P1: xfer VRAM writes=%0d (expected %0d)",
                   xfer_writes_seen, NUM_PIXELS);
            errors = errors + 1;
        end

        // Phase 1 per-pixel nibble readback at swizzled addresses.
        for (int yy = 0; yy < RRH; yy++) begin
            for (int xx = 0; xx < RRW; xx++) begin
                psmt4_pos_t  pos;
                logic [3:0]  expected;
                logic [3:0]  got;
                pos      = ref_pos_psmt4(0, DBW, xx, yy);
                expected = nibble_at(xx, yy);
                vram_raddr2 = pos.addr & ~32'd3;
                #1;
                case (pos.addr[1:0])
                    2'b00: got = pos.nibble_hi ? vram_rdata2[7:4]   : vram_rdata2[3:0];
                    2'b01: got = pos.nibble_hi ? vram_rdata2[15:12] : vram_rdata2[11:8];
                    2'b10: got = pos.nibble_hi ? vram_rdata2[23:20] : vram_rdata2[19:16];
                    default: got = pos.nibble_hi ? vram_rdata2[31:28] : vram_rdata2[27:24];
                endcase
                if (got !== expected) begin
                    $error("[P1-nibble] (%0d,%0d) @addr 0x%08x hi=%0b got 0x%01x expected 0x%01x",
                           xx, yy, pos.addr, pos.nibble_hi, got, expected);
                    errors = errors + 1;
                end
            end
        end

        // Strict separator: PSMT4 row stride at DBW=2 = DBW*32 =
        // 64 bytes. Linear y=1 starts at byte 64. Swizzled writes
        // for yb=0..3 fill nibble_idx [0..127] within block (0,0),
        // = bytes [0..63]. Byte 64 is strictly outside.
        check_byte_zero(32'd64, "P1-linear-y1-byte-still-zero");

        // ============================================================
        // Phase 3 — non-origin PSMT4 transfer. Re-arm with DBP=8,
        // DBW=2, DSAX=28, DSAY=12, RRW=8, RRH=8. Effective coords
        // (28..35, 12..19) cross block_x=0→1 at effective_x=32 AND
        // block_y=0→1 at effective_y=16.
        // ============================================================
        drive_reg(R_BITBLTBUF, bitbltbuf_pack(14'(P3_DBP), 6'(DBW), 6'h14)); // PSMT4
        drive_reg(R_TRXPOS,    trxpos_pack(11'(P3_DSAX), 11'(P3_DSAY)));
        drive_reg(R_TRXREG,    trxreg_pack(12'(P3_RRW), 12'(P3_RRH)));
        drive_reg(R_TRXDIR,    trxdir_pack(2'd0));
        drive_idle();

        @(posedge clk);
        @(posedge clk);

        // Phase 3: 8 px/row × 8 rows = 64 pixels. Engine drains
        // 32 lanes/qword. With RRW=8 only 8 lanes per row are
        // active, so 4 rows pack into 32 lanes per qword → 64/(8*4) = 2 qwords.
        for (int qi = 0; qi < 2; qi++) begin
            logic [127:0] q;
            bit           is_last;
            q = 128'd0;
            for (int row = 0; row < 4; row++) begin
                for (int i = 0; i < 8; i++) begin
                    q[(row*8 + i)*4 +: 4] = nibble_at(i, 4*qi + row);
                end
            end
            is_last = (qi == 1);
            push_image_qword(q, is_last);
        end

        wait (xfer_busy == 1'b0);
        repeat (10) @(posedge clk);

        // Total counts: 2 arms, 64+64 = 128 PSMT4 writes.
        if (trxdir_arms_seen != 2) begin
            $error("after P3: trxdir_wr_q pulses=%0d (expected 2)",
                   trxdir_arms_seen);
            errors = errors + 1;
        end
        if (xfer_writes_seen != (NUM_PIXELS + P3_PIXELS)) begin
            $error("after P3: xfer VRAM writes=%0d (expected %0d)",
                   xfer_writes_seen, NUM_PIXELS + P3_PIXELS);
            errors = errors + 1;
        end

        // Phase 3 per-pixel nibble readback at swizzled addresses.
        // Engine wires FBP=0 to the swizzle module and adds
        // dest_base_q = DBP*256 ON TOP, so the TB ref must match:
        // call ref_pos_psmt4 with fbp_v=0 and add DBP*256 to the
        // returned address.
        for (int yy = 0; yy < P3_RRH; yy++) begin
            for (int xx = 0; xx < P3_RRW; xx++) begin
                int          ex, ey;
                psmt4_pos_t  pos;
                logic [31:0] eff_addr;
                logic [3:0]  expected;
                logic [3:0]  got;
                ex   = P3_DSAX + xx;
                ey   = P3_DSAY + yy;
                pos  = ref_pos_psmt4(0, DBW, ex, ey);
                eff_addr = pos.addr + 32'(P3_DBP * 256);
                expected = nibble_at(xx, yy);
                vram_raddr2 = eff_addr & ~32'd3;
                #1;
                case (eff_addr[1:0])
                    2'b00: got = pos.nibble_hi ? vram_rdata2[7:4]   : vram_rdata2[3:0];
                    2'b01: got = pos.nibble_hi ? vram_rdata2[15:12] : vram_rdata2[11:8];
                    2'b10: got = pos.nibble_hi ? vram_rdata2[23:20] : vram_rdata2[19:16];
                    default: got = pos.nibble_hi ? vram_rdata2[31:28] : vram_rdata2[27:24];
                endcase
                if (got !== expected) begin
                    $error("[P3-nibble] xx=%0d yy=%0d (ex=%0d,ey=%0d) @addr 0x%08x hi=%0b got 0x%01x expected 0x%01x",
                           xx, yy, ex, ey, eff_addr, pos.nibble_hi, got, expected);
                    errors = errors + 1;
                end
            end
        end

        // P3 strict separator: linear formula puts effective coord
        // (28, 12) at byte 2830. Byte 2830 is inside block (1,1)'s
        // extent at DBP=8 (block_idx=3, block_base 2816..3071), but
        // no Phase-3 swizzled write hits byte 2830 specifically.
        // Stays 0 → fall-through to linear would have stomped it.
        check_byte_zero(32'd2830, "P3-linear-byte-still-zero");

        // ============================================================
        // Phase 2 — end-to-end agreement: enable PSMT4 swizzled
        // scanout (Ch138), capture the frame for Phase 1's region,
        // assert each visible pixel matches its uploaded nibble
        // (PCRTC PSMT4 grayscale fallback gives R=G=B={n, n}).
        // ============================================================
        // DISPFB1: FBP=0, FBW=2, PSM=PSMT4 (=0x14).
        //   FBW [14:9]   = 2   → 0x0400
        //   PSM [19:15]  = 0x14 → 0x14<<15 = 0xA_0000
        tb_dispfb1_q = 64'h0000_0000_000A_0400;
        // DISPLAY1: DX=0, DY=0, DW=15, DH=3, MAGH=MAGV=0
        tb_display1_q = 64'h0030_000F_0000_0000;
        // PMODE.EN1=1
        tb_pmode_q = 64'h0000_0000_0000_0001;

        repeat (40) @(posedge clk);
        capture_armed = 1'b1;
        repeat (300) @(posedge clk);

        for (int yy = 0; yy < PCRTC_V_ACTIVE; yy++) begin
            for (int xx = 0; xx < PCRTC_H_ACTIVE; xx++) begin
                logic [7:0] e;
                if (!cap_de[yy][xx]) begin
                    $error("[scanout] (%0d,%0d) DE never asserted", xx, yy);
                    errors = errors + 1;
                end else begin
                    e = gray_at(xx, yy);
                    if (cap_r[yy][xx] !== e
                        || cap_g[yy][xx] !== e
                        || cap_b[yy][xx] !== e) begin
                        $error("[scanout] (%0d,%0d) got R=%02x G=%02x B=%02x expected gray=%02x",
                               xx, yy,
                               cap_r[yy][xx], cap_g[yy][xx], cap_b[yy][xx], e);
                        errors = errors + 1;
                    end
                end
            end
        end

        $display("[tb_gs_image_xfer_swizzle_psmt4] arms=%0d writes=%0d errors=%0d",
                 trxdir_arms_seen, xfer_writes_seen, errors);
        if (errors == 0) $display("[tb_gs_image_xfer_swizzle_psmt4] PASS");
        else             $display("[tb_gs_image_xfer_swizzle_psmt4] FAIL");
        $finish;
    end

    initial begin
        #2000000;
        $error("[tb_gs_image_xfer_swizzle_psmt4] timeout");
        $finish;
    end

endmodule : tb_gs_image_xfer_swizzle_psmt4
