// retroDE_ps2 — tb_gs_raster_swizzle_psmt4 (Ch140)
//
// Third write-side wiring of the Ch137 PSMT4 swizzle math —
// this time into gs_stub's raster emit channel (the per-pixel
// store path that backs SPRITE/TRI primitives). With the new
// `PSMT4_SWIZZLE=1` parameter on gs_stub, the per-pixel VRAM
// byte address goes through `gs_swizzle_psmt4_stub` instead of
// the legacy linear formula `(FBW*64*y + x) >> 1`, AND the
// per-pixel nibble-mask selector keys on the swizzle's
// `nibble_hi` output (instead of the linear `s2_pixel_index[0]`)
// so the existing Ch106 nibble RMW machinery (write_be=4'b0001
// + write_mask 0x0F or 0xF0) hits the right nibble of the
// swizzled byte. PSMCT32 is gated by PSMCT32_SWIZZLE (Ch122);
// PSMCT16 by PSMCT16_SWIZZLE (Ch128); PSMT8 by PSMT8_SWIZZLE
// (Ch134). Default off → every existing PSMT4 raster TB (Ch106
// raster_psmt4, Ch107 PSMT4-e2e, Ch103 PSMT4+CLUT, Ch104 round-
// trip, etc.) is unaffected. Mirrors Ch134's PSMT8 raster
// wiring shape but for the fourth (and last) PSM, and uses
// Ch138's swizzled-pcrtc as the read-side cross-check.
//
// Topology — keeps the proof end-to-end (mirrors Ch128 / Ch134):
//
//   TB drives PRIM/FRAME_1/RGBAQ/XYZ2 via gif_reg_wr_*
//        →  gs_stub#(PSMT4_SWIZZLE=1)         (raster emit)
//        →  vram_stub                         (PSMT4 nibble RMW)
//        →  gs_pcrtc_stub#(PSMT4_SWIZZLE=1)   (swizzled scanout)
//        →  r/g/b
//
// PCRTC's CLUT is disabled (clut_enable=0) so PSMT4 grayscale
// fallback (Ch103) returns r=g=b={nibble, nibble} at scanout —
// for a single-nibble SPRITE the captured RGB is constant
// {n, n} across the whole sprite.
//
// PHASE 1 (write-side lock): drive a 16×4 SPRITE filling the
// rect (0,0)..(15,3) — 64 PSMT4 pixels at 0.5 bytes/pixel.
// PSMT4 blocks are 32×16 px, so the rect lies in the upper-
// left corner of block (0,0) of page (0,0). PSMT4 uses FBW=2
// here (PCSX2 asserts FBW must be even; 2 is the minimum even
// FBW). Single per-primitive RGBAQ.R = 0xAA (Ch106 PSMT4
// raster takes R[3:0] as the index → nibble = 0xA). Within-
// block columnTable4 entries for yb=0..3, xb=0..15 cover
// nibble_idx values [0..127] → byte_in_block ∈ [0..63]. Per-
// pixel nibble readback at the swizzled (addr, nibble_hi) slot
// asserts every pixel landed at the correct (byte, nibble).
//
// Strict P1 separator: under LINEAR, PSMT4 row stride at
// FBW=2 = FBW*64*0.5 = 64 bytes/row, so y=1 starts at byte 64.
// Swizzled writes for yb=0..3 fill bytes [0..63]. Byte 64 is
// strictly outside, so a fall-through to linear would have
// placed pixel (0, 1) there. Assert byte 64 stays 0.
//
// PHASE 2 (end-to-end agreement): without redrawing, enable
// the Ch138 swizzled-pcrtc path, capture the full 16×4 frame,
// assert every visible pixel reads back as {0xA, 0xA} = 0xAA.
// Both gs_stub (Ch140) and gs_pcrtc_stub (Ch138) instantiate
// the same gs_swizzle_psmt4_stub AND threading the nibble_hi
// output through their nibble selectors; success proves the
// two integrations agree at byte+nibble level.
//
// PHASE 3 (non-origin raster): re-arm gs_stub with FRAME_1 set
// to FBP=4, FBW=4 and draw an 8×4 SPRITE at (124, 4)..(131, 7)
// with R=0x55 (nibble = 0x5). Crosses PAGE-x boundary at x=128
// — page (0,0) for x∈[124..127], page (1,0) for x∈[128..131].
// 2 blocks visited:
//   page (0,0) block (0,3) → blockTable4[0][3]=10 → block_base
//     8192 + 10*256 = 10752.
//   page (1,0) block (0,0) → blockTable4[0][0]=0 → bw_pg=2,
//     page_idx=1, page_base=16384, block_base=16384.
// Pins three contracts Phase 1 can't:
//   (a) `ras_fbp` reaches the swizzle's `fbp` input (FBP=0 in
//       Phase 1 would mask a tied-zero regression).
//   (b) `ras_fbw` reaches the swizzle's `fbw` input (FBW=2
//       would mask a tied-two regression).
//   (c) The swizzle gets the FULL absolute pixel coords
//       (s2_x_q, s2_y_q) rather than bbox-local (Phase 1's
//       sprite started at (0,0) so absolute and local were
//       equal there). PSMT4's page-x boundary is at x=128
//       (same as PSMT8 — both have 128-px-wide pages).
//
// Phase 3 strict separator: under LINEAR, pixel (124, 4) lands
// at byte:
//   FBP*2048 + ((4*FBW*64) + 124) >> 1
//   = 8192 + ((1024) + 124) >> 1
//   = 8192 + 574
//   = 8766.
// Under SWIZZLED, P3 write set lives in 2 disjoint chunks:
//   block (0,3) at 10752..11007 (256-byte extent)
//   block (0,0) of page (1,0) at 16384..16639
// Byte 8766 is strictly outside both — sits between page 0's
// start (8192) and block (0,3)'s base (10752). If raster had
// fallen through to linear, byte 8766 would hold P3's nibble.
// Assert byte 8766 stays 0.

`timescale 1ns/1ps

module tb_gs_raster_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 I/O
    // ------------------------------------------------------------
    logic        gif_reg_wr_en;
    logic [7:0]  gif_reg_num;
    logic [63:0] gif_reg_data;

    logic        priv_reg_wr_en;
    logic [15:0] priv_reg_wr_addr;
    logic [63:0] priv_reg_wr_data;

    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 #(
        .PSMCT32_SWIZZLE(1'b0),
        .PSMCT16_SWIZZLE(1'b0),
        .PSMT8_SWIZZLE  (1'b0),
        .PSMT4_SWIZZLE  (1'b1)
    ) u_gs (
        .clk(clk), .rst_n(rst_n),
        .reg_wr_en  (priv_reg_wr_en),
        .reg_wr_addr(priv_reg_wr_addr),
        .reg_wr_data(priv_reg_wr_data),
        .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)
    );

    // ------------------------------------------------------------
    // vram_stub. Two readers: pcrtc (Phase 2) + TB (Phase 1/3).
    // ------------------------------------------------------------
    logic [31:0] vram_read_addr;
    logic [31:0] vram_read_data;
    logic [31:0] vram_read2_addr;
    logic [31:0] vram_read2_data;

    vram_stub #(.BYTES(65536)) u_vram (
        .clk(clk), .rst_n(rst_n),
        .write_en  (raster_pixel_emit),
        .write_addr(raster_pixel_fb_addr_q),
        .write_data(raster_pixel_color_q[31:0]),
        .write_be  (raster_pixel_be_q),
        .write_mask(raster_pixel_mask_q),
        .read_addr (vram_read_addr),
        .read_data (vram_read_data),
        .read2_addr(vram_read2_addr),
        .read2_data(vram_read2_data)
    );

    // ------------------------------------------------------------
    // 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       (pmode_q),
        .dispfb1_q     (dispfb1_q),
        .display1_q    (display1_q),
        .vram_read_addr(vram_read_addr),
        .vram_read_data(vram_read_data),
        .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)
    );

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

    int errors;
    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
        errors        = 0;
        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

    // ------------------------------------------------------------
    // Per-emit lane-shape observer. Ties the address-gate proof
    // to the intended PSM packing: raster_pixel_psm_q must be
    // PSMT4 (=0x14), be must be 4'b0001 (1-byte commit), mask
    // must be 0x0F or 0xF0 (nibble RMW).
    // ------------------------------------------------------------
    int raster_obs_seen;
    initial raster_obs_seen = 0;

    always_ff @(posedge clk) begin
        if (rst_n && raster_pixel_emit) begin
            raster_obs_seen <= raster_obs_seen + 1;
            if (raster_pixel_psm_q !== 6'h14) begin
                $error("emit %0d: psm=0x%02x (expected 0x14 PSMT4)",
                       raster_obs_seen, raster_pixel_psm_q);
                errors <= errors + 1;
            end
            if (raster_pixel_be_q !== 4'b0001) begin
                $error("emit %0d: be=%b (expected 4'b0001 for PSMT4)",
                       raster_obs_seen, raster_pixel_be_q);
                errors <= errors + 1;
            end
            if (raster_pixel_mask_q !== 32'h0000_000F
                && raster_pixel_mask_q !== 32'h0000_00F0) begin
                $error("emit %0d: mask=0x%08x (expected 0x0F or 0xF0 for PSMT4)",
                       raster_obs_seen, raster_pixel_mask_q);
                errors <= errors + 1;
            end
        end
    end

    // ------------------------------------------------------------
    // GIF / priv reg drive 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

    task automatic drive_priv(input logic [15:0] addr,
                              input logic [63:0] data);
        @(negedge clk);
        priv_reg_wr_en   = 1'b1;
        priv_reg_wr_addr = addr;
        priv_reg_wr_data = data;
        @(posedge clk);
        @(negedge clk);
        priv_reg_wr_en   = 1'b0;
        priv_reg_wr_addr = 16'd0;
        priv_reg_wr_data = 64'd0;
    endtask

    function automatic logic [63:0] xyz2_data(input logic [11:0] x_int,
                                              input logic [11:0] y_int);
        return {32'd0, y_int, 4'd0, x_int, 4'd0};
    endfunction

    // ------------------------------------------------------------
    // PSMT4 swizzle reference — block + column tables verbatim
    // from PCSX2 (Ch137 source-locked). Only rows touched by
    // Phase 1 (yb=0..3) and Phase 3 (xb=0..3 + 28..31, yb=4..7)
    // are needed.
    // ------------------------------------------------------------
    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);
        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=4
            9'd128: return 192; 9'd129: return 200; 9'd130: return 224; 9'd131: return 232;
            9'd132: return 128; 9'd133: return 136; 9'd134: return 160; 9'd135: return 168;
            9'd136: return 194; 9'd137: return 202; 9'd138: return 226; 9'd139: return 234;
            9'd140: return 130; 9'd141: return 138; 9'd142: return 162; 9'd143: return 170;
            9'd144: return 196; 9'd145: return 204; 9'd146: return 228; 9'd147: return 236;
            9'd148: return 132; 9'd149: return 140; 9'd150: return 164; 9'd151: return 172;
            9'd152: return 198; 9'd153: return 206; 9'd154: return 230; 9'd155: return 238;
            9'd156: return 134; 9'd157: return 142; 9'd158: return 166; 9'd159: return 174;
            // yb=5
            9'd160: return 208; 9'd161: return 216; 9'd162: return 240; 9'd163: return 248;
            9'd164: return 144; 9'd165: return 152; 9'd166: return 176; 9'd167: return 184;
            9'd168: return 210; 9'd169: return 218; 9'd170: return 242; 9'd171: return 250;
            9'd172: return 146; 9'd173: return 154; 9'd174: return 178; 9'd175: return 186;
            9'd176: return 212; 9'd177: return 220; 9'd178: return 244; 9'd179: return 252;
            9'd180: return 148; 9'd181: return 156; 9'd182: return 180; 9'd183: return 188;
            9'd184: return 214; 9'd185: return 222; 9'd186: return 246; 9'd187: return 254;
            9'd188: return 150; 9'd189: return 158; 9'd190: return 182; 9'd191: return 190;
            // yb=6
            9'd192: return 129; 9'd193: return 137; 9'd194: return 161; 9'd195: return 169;
            9'd196: return 193; 9'd197: return 201; 9'd198: return 225; 9'd199: return 233;
            9'd200: return 131; 9'd201: return 139; 9'd202: return 163; 9'd203: return 171;
            9'd204: return 195; 9'd205: return 203; 9'd206: return 227; 9'd207: return 235;
            9'd208: return 133; 9'd209: return 141; 9'd210: return 165; 9'd211: return 173;
            9'd212: return 197; 9'd213: return 205; 9'd214: return 229; 9'd215: return 237;
            9'd216: return 135; 9'd217: return 143; 9'd218: return 167; 9'd219: return 175;
            9'd220: return 199; 9'd221: return 207; 9'd222: return 231; 9'd223: return 239;
            // yb=7
            9'd224: return 145; 9'd225: return 153; 9'd226: return 177; 9'd227: return 185;
            9'd228: return 209; 9'd229: return 217; 9'd230: return 241; 9'd231: return 249;
            9'd232: return 147; 9'd233: return 155; 9'd234: return 179; 9'd235: return 187;
            9'd236: return 211; 9'd237: return 219; 9'd238: return 243; 9'd239: return 251;
            9'd240: return 149; 9'd241: return 157; 9'd242: return 181; 9'd243: return 189;
            9'd244: return 213; 9'd245: return 221; 9'd246: return 245; 9'd247: return 253;
            9'd248: return 151; 9'd249: return 159; 9'd250: return 183; 9'd251: return 191;
            9'd252: return 215; 9'd253: return 223; 9'd254: return 247; 9'd255: return 255;
            default: return 0;
        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-byte readback helper (extracts the targeted byte from
    // the 4-byte word vram_stub returns).
    // ------------------------------------------------------------
    task automatic read_byte_at(input logic [31:0] byte_addr,
                                output logic [7:0] b);
        vram_read2_addr = byte_addr & ~32'd3;
        #1;
        case (byte_addr[1:0])
            2'b00:   b = vram_read2_data[7:0];
            2'b01:   b = vram_read2_data[15:8];
            2'b10:   b = vram_read2_data[23:16];
            default: b = vram_read2_data[31:24];
        endcase
    endtask

    // ------------------------------------------------------------
    // Phase 1 / 2 constants.
    // ------------------------------------------------------------
    localparam logic [7:0]  R_PRIM     = 8'h00;
    localparam logic [7:0]  R_RGBAQ    = 8'h01;
    localparam logic [7:0]  R_XYZ2     = 8'h05;
    localparam logic [7:0]  R_FRAME_1  = 8'h4C;

    localparam logic [63:0] PRIM_SPRITE = 64'd6;
    // FRAME_1: FBP[8:0]=0, FBW[5:0]=2 (at bit 16), PSM=PSMT4(=0x14 at bit 24).
    //   FBW=2 → bits[21:16]=0x02 → 0x0002_0000.
    //   PSM=0x14 → bits[29:24]=0x14 → 0x1400_0000.
    localparam logic [63:0] FRAME_1_VAL = 64'h0000_0000_1402_0000;
    // RGBAQ: R=0xAA, G=0x00, B=0x00, A=0x00 → low 32 = 0x00_00_00_AA.
    // PSMT4 raster takes R[3:0] as the nibble → nibble = 0xA. The high
    // nibble of R (0xA) is unused for the index, but using 0xAA keeps
    // it as a uniform byte for any traces that might display R.
    localparam logic [63:0] RGBAQ_VAL   = 64'h0000_0000_0000_00AA;
    localparam logic [3:0]  PSMT4_NIB   = 4'hA;

    localparam int IMG_W = 16;
    localparam int IMG_H = 4;

    // ------------------------------------------------------------
    // Phase 3 constants.
    // FRAME_1: FBP=4, FBW=4, PSM=PSMT4.
    //   FBP[8:0]=4 → bits[8:0]=0x004.
    //   FBW=4 → bits[21:16]=0x04 → 0x0004_0000.
    //   PSM=0x14 → 0x1400_0000.
    // Combined: 0x0000_0000_1404_0004.
    localparam logic [63:0] P3_FRAME_1_VAL = 64'h0000_0000_1404_0004;
    localparam logic [63:0] P3_RGBAQ_VAL   = 64'h0000_0000_0000_0055;
    localparam logic [3:0]  P3_NIB         = 4'h5;
    localparam int P3_FBP = 4;
    localparam int P3_FBW = 4;
    localparam int P3_X0  = 124;
    localparam int P3_Y0  = 4;
    localparam int P3_X1  = 131;
    localparam int P3_Y1  = 7;
    localparam int P3_W   = (P3_X1 - P3_X0 + 1);  // 8
    localparam int P3_H   = (P3_Y1 - P3_Y0 + 1);  // 4
    localparam int P3_PIXELS = P3_W * P3_H;       // 32

    initial begin
        rst_n            = 1'b0;
        gif_reg_wr_en    = 1'b0;
        gif_reg_num      = 8'd0;
        gif_reg_data     = 64'd0;
        priv_reg_wr_en   = 1'b0;
        priv_reg_wr_addr = 16'd0;
        priv_reg_wr_data = 64'd0;
        vram_read2_addr  = 32'd0;

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

        // ============================================================
        // Phase 1 — origin SPRITE (16×4 PSMT4 at FBP=0, FBW=2,
        // single nibble = 0xA).
        // ============================================================
        drive_reg(R_PRIM,    PRIM_SPRITE);
        drive_reg(R_FRAME_1, FRAME_1_VAL);
        drive_reg(R_RGBAQ,   RGBAQ_VAL);
        drive_reg(R_XYZ2,    xyz2_data(12'd0, 12'd0));
        drive_reg(R_XYZ2,    xyz2_data(12'(IMG_W - 1),
                                       12'(IMG_H - 1)));
        drive_idle();

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

        if (raster_pixel_emit_count != 32'(IMG_W * IMG_H)) begin
            $error("expected %0d raster emits, got %0d",
                   IMG_W * IMG_H, raster_pixel_emit_count);
            errors = errors + 1;
        end

        // Phase 1 — write-side lock. Each pixel's nibble at the
        // swizzled (addr, nibble_hi) slot must equal 0xA.
        for (int yy = 0; yy < IMG_H; yy++) begin
            for (int xx = 0; xx < IMG_W; xx++) begin
                psmt4_pos_t pos;
                logic [7:0] byte_v;
                logic [3:0] got;
                pos = ref_pos_psmt4(0, 2, xx, yy);
                read_byte_at(pos.addr, byte_v);
                got = pos.nibble_hi ? byte_v[7:4] : byte_v[3:0];
                if (got !== PSMT4_NIB) 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, PSMT4_NIB);
                    errors = errors + 1;
                end
            end
        end

        // Strict separator: byte 64 (linear y=1 at PSMT4
        // FBW=2 stride 64) outside the swizzled write set
        // [0..63]. Read targeted byte (not full word).
        begin
            logic [7:0] sep;
            read_byte_at(32'd64, sep);
            if (sep !== 8'd0) begin
                $error("[P1-linear-y1-byte-still-zero] @byte 64 got 0x%02x expected 0x00", sep);
                errors = errors + 1;
            end
        end

        // ============================================================
        // Phase 2 — end-to-end agreement: enable swizzled scanout
        // (PSMT4_SWIZZLE on PCRTC=Ch138), capture frame, assert
        // grayscale R=G=B={0xA, 0xA} = 0xAA across the 16×4 image.
        // ============================================================
        // DISPFB1: FBP=0, FBW=2, PSM=PSMT4 (=0x14).
        //   FBW [14:9]   = 2   → 0x0400
        //   PSM [19:15]  = 0x14 → 0x14<<15 = 0xA_0000
        drive_priv(16'h0070, 64'h0000_0000_000A_0400);
        // DISPLAY1: DX=0, DY=0, DW=15, DH=3, MAGH=MAGV=0.
        drive_priv(16'h0080, 64'h0030_000F_0000_0000);
        // PMODE.EN1=1.
        drive_priv(16'h0000, 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
                if (!cap_de[yy][xx]) begin
                    $error("[scanout] (%0d,%0d) DE never asserted", xx, yy);
                    errors = errors + 1;
                end else if (cap_r[yy][xx] !== 8'hAA
                          || cap_g[yy][xx] !== 8'hAA
                          || cap_b[yy][xx] !== 8'hAA) begin
                    $error("[scanout] (%0d,%0d) got R=%02x G=%02x B=%02x expected gray=AA",
                           xx, yy,
                           cap_r[yy][xx], cap_g[yy][xx], cap_b[yy][xx]);
                    errors = errors + 1;
                end
            end
        end

        // ============================================================
        // Phase 3 — non-origin SPRITE (8×4 at (124,4)..(131,7),
        // FBP=4, FBW=4). Crosses page-x boundary at x=128.
        // ============================================================
        drive_reg(R_PRIM,    PRIM_SPRITE);
        drive_reg(R_FRAME_1, P3_FRAME_1_VAL);
        drive_reg(R_RGBAQ,   P3_RGBAQ_VAL);
        drive_reg(R_XYZ2,    xyz2_data(12'(P3_X0), 12'(P3_Y0)));
        drive_reg(R_XYZ2,    xyz2_data(12'(P3_X1), 12'(P3_Y1)));
        drive_idle();

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

        // Total emit count after Phase 3: 64 (P1 16×4) + 32 (P3 8×4).
        if (raster_pixel_emit_count != 32'(IMG_W * IMG_H + P3_PIXELS)) begin
            $error("after P3: expected %0d raster emits, got %0d",
                   IMG_W * IMG_H + P3_PIXELS, raster_pixel_emit_count);
            errors = errors + 1;
        end

        // Phase 3 per-pixel nibble readback at swizzled addresses.
        for (int yy = P3_Y0; yy <= P3_Y1; yy++) begin
            for (int xx = P3_X0; xx <= P3_X1; xx++) begin
                psmt4_pos_t pos;
                logic [7:0] byte_v;
                logic [3:0] got;
                pos = ref_pos_psmt4(P3_FBP, P3_FBW, xx, yy);
                read_byte_at(pos.addr, byte_v);
                got = pos.nibble_hi ? byte_v[7:4] : byte_v[3:0];
                if (got !== P3_NIB) begin
                    $error("[P3-nibble] (%0d,%0d) @addr 0x%08x hi=%0b got 0x%01x expected 0x%01x",
                           xx, yy, pos.addr, pos.nibble_hi, got, P3_NIB);
                    errors = errors + 1;
                end
            end
        end

        // P3 strict separator: byte 8766 (linear formula's place
        // for (124, 4) at FBP=4/FBW=4) is outside both swizzled
        // chunks [10752..11007] and [16384..16639]. Stays 0.
        begin
            logic [7:0] sep;
            read_byte_at(32'd8766, sep);
            if (sep !== 8'd0) begin
                $error("[P3-linear-byte-still-zero] @byte 8766 got 0x%02x expected 0x00", sep);
                errors = errors + 1;
            end
        end

        $display("[tb_gs_raster_swizzle_psmt4] emits=%0d errors=%0d",
                 raster_pixel_emit_count, errors);
        if (errors == 0) $display("[tb_gs_raster_swizzle_psmt4] PASS");
        else             $display("[tb_gs_raster_swizzle_psmt4] FAIL");
        $finish;
    end

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

endmodule : tb_gs_raster_swizzle_psmt4
