// retroDE_ps2 — tb_gs_image_xfer_swizzle_psmct16 (Ch127)
//
// Write-side wiring of the Ch125 PSMCT16 swizzle math into
// gif_image_xfer_stub. PSMCT16 TRXDIR uploads now route their
// per-pixel VRAM byte address through gs_swizzle_psmct16_stub
// when the new `PSMCT16_SWIZZLE=1` parameter is set. PSMCT32 is
// gated by its own PSMCT32_SWIZZLE parameter (Ch121); PSMT8/T4
// always linear. Default 0 keeps the legacy linear path for
// every existing PSMCT16 image-xfer TB (Ch116 etc.). Mirrors
// the Ch121 wiring shape but for the second PSM, and uses the
// Ch126 swizzled-pcrtc as the read-side cross-check.
//
// Topology — keeps the proof end-to-end:
//
//   TB.gif_reg_wr_*  →  gs_stub (TRX latches only)
//                     → gif_image_xfer_stub#(PSMCT16_SWIZZLE=1)
//                     → vram_stub
//                     → gs_pcrtc_stub#(PSMCT16_SWIZZLE=1) → r/g/b
//
// Phase 1 (write-side lock): TRXDIR upload of a 16×4 PSMCT16
// image at DBP=0, DBW=1, DSAX=DSAY=0. Each pixel has a unique
// RGB5A1 halfword. After the upload completes, the TB reads VRAM
// via the second read port at the SWIZZLED byte address
// (computed by an in-TB ref_addr16() that mirrors
// gs_swizzle_psmct16_stub — block table + column table both
// transcribed verbatim from the Ch125-locked PCSX2 source) and
// asserts each pixel halfword landed where the swizzle says.
//
// Strict linear-vs-swizzled separator: at FBP=0/DBW=1, the 16×4
// PSMCT16 image lives entirely in block (0,0) of page 0. Within-
// block hw indices for yb=0..3, xb=0..15 cover hw values 0..63
// (all within column-table rows yb=0..3, max value 63), so
// swizzled writes touch bytes [0..126]. Under the LINEAR
// formula, row stride = FBW*64*2 = 128, so y=1 row starts at
// byte 128. Byte 128 is strictly outside the swizzled write set,
// so a fall-through to linear would have placed pixel(0, 1) at
// byte 128. We assert byte 128 stays 0.
//
// Phase 2 (end-to-end agreement): without changing VRAM, enable
// the scanout path (PMODE/DISPFB1/DISPLAY1) with PSM=PSMCT16 and
// capture a full 16×4 frame. Both the upload (Ch127) and the
// scanout (Ch126) instantiate the same gs_swizzle_psmct16_stub,
// so this phase proves the two integrations agree at byte level
// — what was uploaded comes out on r/g/b (after PSMCT16's RGB5A1
// → RGB888 5→8 bit-replicate) at the same (x, y) position.
//
// Phase 3 (non-origin transfer): re-arm the engine with NONZERO
// DBP, DSAX, and DSAY. DBP=8 (= byte 2048 = one page after
// Phase 1's region — no overlap). DSAX=12, DSAY=6, RRW=8, RRH=4
// — effective coord range x∈[12..19], y∈[6..9] crosses
// block_x=0→1 at effective_x=16 AND block_y=0→1 at
// effective_y=8, exercising both block-table dimensions inside
// a single non-origin upload. Pins three contracts the origin
// transfer can't distinguish from a buggy implementation:
//   (a) `dest_base_q (= DBP*256)` is correctly ADDED ON TOP of
//       the swizzle output (with DBP=0 a missing-add regression
//       would still pass).
//   (b) The swizzle is fed the FULL effective coords (with
//       DSAX=DSAY=0 a "feeds only cur_x/cur_y" regression
//       would still pass).
//   (c) Both block_x and block_y propagate through the swizzle's
//       blockTable16[by][bx] lookup — a regression that tied
//       block_x or block_y to zero would surface here even
//       though origin would not.
// Phase 3 strict separator: linear formula puts effective coord
// (12, 8) at byte `DBP*256 + 8*FBW*64*2 + 12*2 = 2048 + 1024 +
// 24 = 3096`. The Phase-3 swizzled write set lives in bytes
// [2048..3071] (4 corner blocks of page (0,0) at DBP=8); byte
// 3096 is strictly outside. We assert byte 3096 stays 0 — proves
// a fall-through to linear would have stomped that byte.

`timescale 1ns/1ps

module tb_gs_image_xfer_swizzle_psmct16;

    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 (raster, prim, scanout)
    // is unused.
    // ------------------------------------------------------------
    logic        gif_reg_wr_en;
    logic [7:0]  gif_reg_num;
    logic [63:0] gif_reg_data;

    // PCRTC feeds — driven by the TB directly. Separate from
    // gs_stub's pmode_q / dispfb1_q / display1_q outputs (which
    // reflect gs_stub's priv-MMIO latches) because we don't drive
    // priv MMIO writes here; we just need pcrtc to see a valid
    // scanout config in Phase 2.
    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 Ch127 PSMCT16_SWIZZLE gate
    // ON (and the Ch121 PSMCT32_SWIZZLE gate explicitly OFF —
    // this TB exercises only the PSMCT16 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'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 in this TB:
    //   read_addr/data  → driven by pcrtc (Phase 2 scanout)
    //   read2_addr/data → driven by TB    (Phase 1 byte readback)
    // ------------------------------------------------------------
    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 Ch120 swizzle gate ON. Drives vram_raddr.
    // ------------------------------------------------------------
    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'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;
            if (xfer_wbe !== 4'b0011) begin
                $error("xfer write %0d: be=%b (expected 4'b0011 for PSMCT16)",
                       xfer_writes_seen, xfer_wbe);
                errors <= errors + 1;
            end
            if (xfer_wmask !== 32'hFFFF_FFFF) begin
                $error("xfer write %0d: mask=0x%08x (expected 0xFFFFFFFF)",
                       xfer_writes_seen, xfer_wmask);
                errors <= errors + 1;
            end
        end
    end

    // ------------------------------------------------------------
    // Frame capture (mirrors Ch120 pattern).
    // ------------------------------------------------------------
    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

    // ------------------------------------------------------------
    // PSMCT16 swizzle reference — block table + column table both
    // taken VERBATIM from PCSX2's pcsx2/GS/GSTables.cpp (master
    // HEAD 3d71e310, file-touch d983b2b0). Locked at byte level
    // by tb_gs_swizzle_psmct16 (Ch125): blockTable16 lines 29–39,
    // columnTable16 lines 91–109.
    // ------------------------------------------------------------
    function automatic int ref_block_idx16(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_idx16(input int yb, input int xb);
        case ({yb[2:0], xb[3:0]})
            // yb=0
            7'd0:   return 0;   7'd1:   return 2;   7'd2:   return 8;   7'd3:   return 10;
            7'd4:   return 16;  7'd5:   return 18;  7'd6:   return 24;  7'd7:   return 26;
            7'd8:   return 1;   7'd9:   return 3;   7'd10:  return 9;   7'd11:  return 11;
            7'd12:  return 17;  7'd13:  return 19;  7'd14:  return 25;  7'd15:  return 27;
            // yb=1
            7'd16:  return 4;   7'd17:  return 6;   7'd18:  return 12;  7'd19:  return 14;
            7'd20:  return 20;  7'd21:  return 22;  7'd22:  return 28;  7'd23:  return 30;
            7'd24:  return 5;   7'd25:  return 7;   7'd26:  return 13;  7'd27:  return 15;
            7'd28:  return 21;  7'd29:  return 23;  7'd30:  return 29;  7'd31:  return 31;
            // yb=2
            7'd32:  return 32;  7'd33:  return 34;  7'd34:  return 40;  7'd35:  return 42;
            7'd36:  return 48;  7'd37:  return 50;  7'd38:  return 56;  7'd39:  return 58;
            7'd40:  return 33;  7'd41:  return 35;  7'd42:  return 41;  7'd43:  return 43;
            7'd44:  return 49;  7'd45:  return 51;  7'd46:  return 57;  7'd47:  return 59;
            // yb=3
            7'd48:  return 36;  7'd49:  return 38;  7'd50:  return 44;  7'd51:  return 46;
            7'd52:  return 52;  7'd53:  return 54;  7'd54:  return 60;  7'd55:  return 62;
            7'd56:  return 37;  7'd57:  return 39;  7'd58:  return 45;  7'd59:  return 47;
            7'd60:  return 53;  7'd61:  return 55;  7'd62:  return 61;  7'd63:  return 63;
            // yb=4
            7'd64:  return 64;  7'd65:  return 66;  7'd66:  return 72;  7'd67:  return 74;
            7'd68:  return 80;  7'd69:  return 82;  7'd70:  return 88;  7'd71:  return 90;
            7'd72:  return 65;  7'd73:  return 67;  7'd74:  return 73;  7'd75:  return 75;
            7'd76:  return 81;  7'd77:  return 83;  7'd78:  return 89;  7'd79:  return 91;
            // yb=5
            7'd80:  return 68;  7'd81:  return 70;  7'd82:  return 76;  7'd83:  return 78;
            7'd84:  return 84;  7'd85:  return 86;  7'd86:  return 92;  7'd87:  return 94;
            7'd88:  return 69;  7'd89:  return 71;  7'd90:  return 77;  7'd91:  return 79;
            7'd92:  return 85;  7'd93:  return 87;  7'd94:  return 93;  7'd95:  return 95;
            // yb=6
            7'd96:  return 96;  7'd97:  return 98;  7'd98:  return 104; 7'd99:  return 106;
            7'd100: return 112; 7'd101: return 114; 7'd102: return 120; 7'd103: return 122;
            7'd104: return 97;  7'd105: return 99;  7'd106: return 105; 7'd107: return 107;
            7'd108: return 113; 7'd109: return 115; 7'd110: return 121; 7'd111: return 123;
            // yb=7
            7'd112: return 100; 7'd113: return 102; 7'd114: return 108; 7'd115: return 110;
            7'd116: return 116; 7'd117: return 118; 7'd118: return 124; 7'd119: return 126;
            7'd120: return 101; 7'd121: return 103; 7'd122: return 109; 7'd123: return 111;
            7'd124: return 117; 7'd125: return 119; 7'd126: return 125; default: return 127;
        endcase
    endfunction

    function automatic logic [31:0] ref_addr16(
        input int fbp_v, input int fbw_v, input int x_v, input int y_v);
        int page_x, page_y, page_idx, page_base;
        int by, bx, blk_idx, xb, yb, hw_idx;
        int addr_v;
        page_x   = x_v / 64;
        page_y   = y_v / 64;
        page_idx = page_y * fbw_v + page_x;
        page_base = fbp_v * 2048 + page_idx * 8192;
        by  = (y_v % 64) / 8;
        bx  = (x_v % 64) / 16;
        blk_idx = ref_block_idx16(by, bx);
        xb = x_v % 16;
        yb = y_v % 8;
        hw_idx = ref_col_idx16(yb, xb);
        addr_v = page_base + blk_idx * 256 + hw_idx * 2;
        return addr_v[31:0];
    endfunction

    // RGB5A1 unique per-pixel pattern. Each (x, y) gets a unique
    // 16-bit value:
    //   R5 = (x ^ y) & 0xF, G5 = x & 0xF, B5 = y & 0xF, A=0
    // Same shape as the Ch126 scanout TB pattern so a fall-through
    // to wrong-address would surface as wrong RGB after the
    // 5→8 bit-replicate at PCRTC output.
    function automatic logic [15:0] pixel_hw_at(input int x, input int y);
        logic [4:0] r5;
        logic [4:0] g5;
        logic [4:0] b5;
        r5 = 5'((x ^ y) & 5'h0F);
        g5 = 5'(x & 5'h0F);
        b5 = 5'(y & 5'h0F);
        return {1'b0, b5, g5, r5};
    endfunction
    function automatic logic [7:0] exp_r8(input int x, input int y);
        logic [4:0] r5;
        r5 = 5'((x ^ y) & 5'h0F);
        return {r5, r5[4:2]};
    endfunction
    function automatic logic [7:0] exp_g8(input int x, input int y);
        logic [4:0] g5;
        g5 = 5'(x & 5'h0F);
        return {g5, g5[4:2]};
    endfunction
    function automatic logic [7:0] exp_b8(input int x, input int y);
        logic [4:0] b5;
        b5 = 5'(y & 5'h0F);
        return {b5, b5[4:2]};
    endfunction

    // 8 PSMCT16 pixels per IMAGE qword (8 × 16 bits = 128 bits).
    function automatic logic [127:0] qword_for_pixels(
        input int x_base, input int y);
        logic [127:0] q;
        q = 128'd0;
        for (int i = 0; i < 8; i++) begin
            q[i*16 +: 16] = pixel_hw_at(x_base + i, y);
        end
        return q;
    endfunction

    // Phase 3 distinct-color pattern. Different bit-pattern from
    // Phase 1 so a Phase-3 pixel landing at a Phase-1 byte
    // (or vice versa) surfaces as wrong RGB.
    //   R5 = 0x10 | ((x ^ y) & 0xF)  -- top bit set, not in P1
    //   G5 = 0x10 | (x & 0xF)
    //   B5 = 0x10 | (y & 0xF)
    function automatic logic [15:0] p3_pixel_hw_at(input int x, input int y);
        logic [4:0] r5;
        logic [4:0] g5;
        logic [4:0] b5;
        r5 = 5'(5'h10 | 5'((x ^ y) & 5'h0F));
        g5 = 5'(5'h10 | 5'(x & 5'h0F));
        b5 = 5'(5'h10 | 5'(y & 5'h0F));
        return {1'b0, b5, g5, r5};
    endfunction

    function automatic logic [127:0] p3_qword_for_pixels(
        input int x_base, input int y);
        logic [127:0] q;
        q = 128'd0;
        for (int i = 0; i < 8; i++) begin
            q[i*16 +: 16] = p3_pixel_hw_at(x_base + i, y);
        end
        return q;
    endfunction

    // ------------------------------------------------------------
    // TB-side byte readback via the 2nd VRAM read port. PSMCT16 =
    // 2 bytes/pixel, so each readback is a 16-bit halfword
    // selected by addr[1] within the 32-bit word.
    // ------------------------------------------------------------
    task automatic check_pixel_at_swizzled(input int x, input int y);
        logic [31:0] addr;
        logic [15:0] expected;
        logic [15:0] got;
        addr     = ref_addr16(0, DBW, x, y);
        expected = pixel_hw_at(x, y);
        vram_raddr2 = addr & ~32'd3;
        #1;
        got = addr[1] ? vram_rdata2[31:16] : vram_rdata2[15:0];
        if (got !== expected) begin
            $error("[swizzled-hw] (%0d,%0d) @addr 0x%08x got 0x%04x expected 0x%04x",
                   x, y, addr, got, expected);
            errors = errors + 1;
        end
    endtask

    task automatic check_byte_zero(input logic [31:0] byte_addr,
                                   input string       tag);
        logic [31:0] got;
        vram_raddr2 = byte_addr & ~32'd3;
        #1;
        got = vram_rdata2;
        if (got !== 32'd0) begin
            $error("[%s] @byte 0x%08x got 0x%08x expected 0x00000000",
                   tag, byte_addr, got);
            errors = errors + 1;
        end
    endtask

    // Phase 3: verify a halfword uploaded with non-origin
    // DBP/DSAX/DSAY landed at `DBP*256 + swizzle16(0, DBW, DSAX+x,
    // DSAY+y)`. (x, y) is the upload-LOCAL coordinate (cur_x,
    // cur_y in the engine); the swizzle's input is the EFFECTIVE
    // coord.
    task automatic check_p3_pixel(input int x_local, input int y_local);
        logic [31:0] addr;
        logic [15:0] expected;
        logic [15:0] got;
        addr     = 32'(P3_DBP * 256)
                 + ref_addr16(0, DBW,
                              P3_DSAX + x_local,
                              P3_DSAY + y_local);
        expected = p3_pixel_hw_at(x_local, y_local);
        vram_raddr2 = addr & ~32'd3;
        #1;
        got = addr[1] ? vram_rdata2[31:16] : vram_rdata2[15:0];
        if (got !== expected) begin
            $error("[p3-swizzled-hw] (%0d,%0d) @addr 0x%08x got 0x%04x expected 0x%04x",
                   x_local, y_local, addr, got, expected);
            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).
    // PSMCT16: 8 pixels per IMAGE qword (8 × 16 bits = 128 bits).
    // The swizzle output from gs_swizzle_psmct16_stub IS the
    // absolute byte address (FBP=0 + dest_base_q=0).
    // ------------------------------------------------------------
    localparam int IMG_DBP = 0;
    localparam int DBW     = 1;
    localparam int RRW     = 16;
    localparam int RRH     = 4;
    localparam int NUM_PIXELS = RRW * RRH;     // 64
    localparam int NUM_QWORDS = NUM_PIXELS / 8; // 8 (8 px/qword)

    // ------------------------------------------------------------
    // Phase 3 transfer parameters (non-origin: nonzero DBP +
    // nonzero DSAX + nonzero DSAY). For PSMCT16, blocks are 16 px
    // wide × 8 px tall, so to exercise BOTH block_x crossing AND
    // block_y crossing inside a single non-origin upload we set
    // DSAX=12 (effective_x crosses 16 mid-row → block_x=0→1) and
    // DSAY=6 (effective_y crosses 8 mid-column → block_y=0→1).
    //
    //   DBP=8 → dest_base_q = 2048 (= one page after Phase 1's
    //           region; no overlap).
    //   DSAX=12, DSAY=6 → effective coords (12..19, 6..9).
    //   DBW=1, RRW=8, RRH=4 → 32 PSMCT16 pixels = 4 IMAGE qwords.
    //
    // Pins three contracts of the Ch127 wiring:
    //   (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=0 in P1 would mask a "feeds only cur_x/cur_y"
    //       regression).
    //   (c) Both block_x and block_y propagate through the
    //       blockTable16[by][bx] lookup — block_x=0 throughout
    //       Phase 1 would mask a tied-block_x regression.
    // ------------------------------------------------------------
    localparam int P3_DBP    = 8;
    localparam int P3_DSAX   = 12;
    localparam int P3_DSAY   = 6;
    localparam int P3_RRW    = 8;
    localparam int P3_RRH    = 4;
    localparam int P3_PIXELS = P3_RRW * P3_RRH; // 32
    localparam int P3_QWORDS = P3_PIXELS / 8;   // 4

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

        // ------------------------------------------------------------
        // Drive TRX setup via GIF A+D register writes.
        // ------------------------------------------------------------
        drive_reg(R_BITBLTBUF, bitbltbuf_pack(14'(IMG_DBP), 6'(DBW), 6'h02)); // PSMCT16
        drive_reg(R_TRXPOS,    trxpos_pack(11'd0, 11'd0));
        drive_reg(R_TRXREG,    trxreg_pack(12'(RRW), 12'(RRH)));
        // TRXDIR = host→local. trxdir_wr_q pulses next cycle, arms
        // gif_image_xfer_stub.
        drive_reg(R_TRXDIR,    trxdir_pack(2'd0));
        drive_idle();

        // Wait for the engine to enter S_RUN.
        @(posedge clk);
        @(posedge clk);

        // ------------------------------------------------------------
        // Push 8 IMAGE qwords (8 PSMCT16 pixels each, row-major).
        // ------------------------------------------------------------
        for (int yy = 0; yy < RRH; yy++) begin
            for (int qi = 0; qi < (RRW / 8); qi++) begin
                logic [127:0] q;
                bit           is_last;
                q       = qword_for_pixels(qi * 8, yy);
                is_last = (yy == RRH - 1) && (qi == (RRW / 8) - 1);
                push_image_qword(q, is_last);
            end
        end

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

        // ------------------------------------------------------------
        // Phase 1 count check (origin transfer only — Phase 3 will
        // add another arm + writes, asserted separately below).
        // ------------------------------------------------------------
        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 — write-side lock.
        // Verify each pixel landed at its SWIZZLED byte address.
        // ------------------------------------------------------------
        for (int yy = 0; yy < RRH; yy++)
            for (int xx = 0; xx < RRW; xx++)
                check_pixel_at_swizzled(xx, yy);

        // Strict separator: pick a byte address the LINEAR formula
        // would write but the SWIZZLED formula does NOT, for our
        // 16×4 PSMCT16 image at FBW=1, FBP=0.
        //
        //   Linear PSMCT16 addresses span (stride = FBW*64*2 = 128):
        //     y=0 → bytes 0..30, y=1 → 128..158,
        //     y=2 → 256..286,    y=3 → 384..414.
        //
        //   Swizzled addresses live entirely in block (0,0) of
        //   page 0 (image is 16 px wide × 4 px tall, all in
        //   by=0..3, bx=0). columnTable16 yb=0..3 covers hw
        //   indices 0..63, so swizzled writes bytes [0..126].
        //
        //   Linear y=1 row starts at byte 128 — strictly outside
        //   the swizzled write set. If the upload had silently
        //   fallen through to linear, byte 128 would hold pixel
        //   (0, 1). Assert it stays 0.
        //   Linear y=2 row starts at byte 256, also outside.
        check_byte_zero(32'd128, "linear-y1-byte-still-zero");
        check_byte_zero(32'd256, "linear-y2-byte-still-zero");

        // ------------------------------------------------------------
        // Phase 3 — non-origin PSMCT16 transfer. Re-arm with
        // DBP=8, DSAX=12, DSAY=6, RRW=8, RRH=4. Effective coords
        // (12..19, 6..9) cross block_x=0→1 at effective_x=16 AND
        // block_y=0→1 at effective_y=8, exercising both block-table
        // dimensions inside a single non-origin upload.
        //
        // Rationale: pins three contracts the origin Phase 1 can't
        // distinguish — `dest_base_q=DBP*256` is added on top, the
        // swizzle is fed the FULL effective coords, and BOTH
        // block_x and block_y propagate through the
        // blockTable16[by][bx] lookup.
        // ------------------------------------------------------------
        drive_reg(R_BITBLTBUF, bitbltbuf_pack(14'(P3_DBP), 6'(DBW), 6'h02)); // PSMCT16
        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);

        for (int yy = 0; yy < P3_RRH; yy++) begin
            for (int qi = 0; qi < (P3_RRW / 8); qi++) begin
                logic [127:0] q;
                bit           is_last;
                q       = p3_qword_for_pixels(qi * 8, yy);
                is_last = (yy == P3_RRH - 1) && (qi == (P3_RRW / 8) - 1);
                push_image_qword(q, is_last);
            end
        end

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

        // Total counts after Phase 3: 2 arms, 64+32=96 PSMCT16 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

        // Per-pixel readback for Phase 3.
        for (int yy = 0; yy < P3_RRH; yy++)
            for (int xx = 0; xx < P3_RRW; xx++)
                check_p3_pixel(xx, yy);

        // P3 strict separator: the LINEAR formula for the Phase-3
        // PSMCT16 upload would place effective coord (12, 8) at byte:
        //   dest_base + effective_y*row_stride + effective_x*2
        //   = 2048 + 8*128 + 24
        //   = 3096
        // (row_stride for PSMCT16 at FBW=1: 64 px × 2 bytes = 128).
        // The SWIZZLED write set for P3 fills 4 corner blocks of
        // page (0,0) at DBP=8: blockTable16[0][0]=0 (bytes 2048..),
        // [0][1]=2 (2560..), [1][0]=1 (2304..), [1][1]=3 (2816..).
        // Total range [2048..3071]. Byte 3096 is strictly outside.
        // A fall-through to LINEAR would place P3-pixel-(0, 2)'s
        // halfword there. Asserting it stays 0 strictly proves the
        // Ch127 PSMCT16 swizzle gate is live for non-origin
        // transfers.
        check_byte_zero(32'd3096, "p3-linear-y2-byte-still-zero");

        // ------------------------------------------------------------
        // Phase 2 — end-to-end agreement: enable scanout (Ch126's
        // PSMCT16_SWIZZLE=1 path), capture the frame, assert each
        // visible pixel matches its uploaded RGB5A1 → RGB888 5→8
        // bit-replicate. Both modules instantiate the same
        // gs_swizzle_psmct16_stub, so a successful capture proves
        // their integrations agree at byte level under DMAC-style
        // emit + scanout-style read.
        // ------------------------------------------------------------
        // DISPFB1: FBP=0, FBW=1, PSM=PSMCT16 (=2).
        //   FBW [14:9]   = 1   → 0x0200
        //   PSM [19:15]  = 2   → 0x10000
        tb_dispfb1_q = 64'h0000_0000_0001_0200;
        // 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]  er, eg, eb;
                if (!cap_de[yy][xx]) begin
                    $error("[scanout] (%0d,%0d) DE never asserted", xx, yy);
                    errors = errors + 1;
                end else begin
                    er = exp_r8(xx, yy);
                    eg = exp_g8(xx, yy);
                    eb = exp_b8(xx, yy);
                    if (cap_r[yy][xx] !== er
                        || cap_g[yy][xx] !== eg
                        || cap_b[yy][xx] !== eb) begin
                        $error("[scanout] (%0d,%0d) got R=%02x G=%02x B=%02x expected R=%02x G=%02x B=%02x",
                               xx, yy,
                               cap_r[yy][xx], cap_g[yy][xx], cap_b[yy][xx],
                               er, eg, eb);
                        errors = errors + 1;
                    end
                end
            end
        end

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

    initial begin
        #1000000;
        $error("[tb_gs_image_xfer_swizzle_psmct16] timeout");
        $finish;
    end

endmodule : tb_gs_image_xfer_swizzle_psmct16
