Merge remote-tracking branch 'rv64cpu-import/master'

40 files changed, 11937 insertions, 0 deletions

diff --git a/rv64cpu/.github/workflows/ci.yml b/rv64cpu/.github/workflows/ci.yml
new file mode 100644
index 0000000..03551fa
--- /dev/null
+++ b/rv64cpu/.github/workflows/ci.yml
@@ -0,0 +1,64 @@
+name: CI
+
+on:
+  push:
+    branches: [ main, develop ]
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  verilator-build:
+    name: Verilator Build & Test
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+        with:
+          submodules: recursive
+
+      - name: Install dependencies
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y \
+            verilator \
+            cmake \
+            build-essential \
+            git \
+            autoconf \
+            flex \
+            bison \
+            libfl-dev
+
+      - name: Check Verilator version
+        run: verilator --version
+
+      - name: Configure
+        run: cmake -B build -DENABLE_TRACE=OFF
+
+      - name: Build
+        run: cmake --build build -j$(nproc)
+
+      - name: Test
+        run: ctest --test-dir build --output-on-failure --timeout 120
+
+  lint:
+    name: Verilator Lint
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+
+      - name: Install Verilator
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y verilator
+
+      - name: Lint RTL
+        run: |
+          verilator --lint-only --sv -Wall \
+            -Irtl/core/include \
+            rtl/soc/richard_soc.sv \
+            $(find rtl -name '*.sv' ! -name 'richard_soc.sv') \
+            || echo "Lint completed with warnings"
diff --git a/rv64cpu/.gitignore b/rv64cpu/.gitignore
new file mode 100644
index 0000000..12107e5
--- /dev/null
+++ b/rv64cpu/.gitignore
@@ -0,0 +1,57 @@
+# Build directories
+build*/
+cmake-build-*/
+obj_dir/
+
+# CMake generated files
+CMakeFiles/
+CMakeCache.txt
+cmake_install.cmake
+compile_commands.json
+Makefile
+
+# Generated binaries and images
+*.elf
+*.bin
+*.hex
+*.dump
+*.map
+*.sig
+
+# Waveforms
+waves/
+
+# Python
+__pycache__/
+*.py[cod]
+.pytest_cache/
+.mypy_cache/
+.ruff_cache/
+.venv/
+
+# Coverage
+coverage/
+*.gcda
+*.gcno
+*.lcov
+
+# IDE and editor
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+
+# FPGA output
+fpga/xilinx/.Xil/
+fpga/xilinx/vivado_proj/
+fpga/xilinx/*.bit
+fpga/xilinx/*.ltx
+fpga/xilinx/*.jou
+fpga/xilinx/*.log
+fpga/gowin/*.fs
+
+# Local caches and OS files
+.cache/
+.DS_Store
+Thumbs.db
diff --git a/rv64cpu/CMakeLists.txt b/rv64cpu/CMakeLists.txt
new file mode 100644
index 0000000..f16aa14
--- /dev/null
+++ b/rv64cpu/CMakeLists.txt
@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.20)
+
+if(POLICY CMP0144)
+    cmake_policy(SET CMP0144 NEW)
+endif()
+
+project(rv64cpu
+    VERSION 0.1.0
+    DESCRIPTION "RV64 RISC-V processor core written in SystemVerilog"
+    LANGUAGES CXX
+)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+option(ENABLE_TRACE    "Enable VCD waveform tracing" OFF)
+option(ENABLE_COVERAGE "Enable RTL code coverage" OFF)
+
+find_package(verilator REQUIRED HINTS $ENV{VERILATOR_ROOT})
+message(STATUS "Found Verilator: ${verilator_DIR}")
+
+set(VERILATOR_COMMON_FLAGS
+    --sv
+    -Wall
+    -Wno-DECLFILENAME
+    -Wno-UNUSED
+    --x-initial unique
+    --x-assign unique
+)
+
+if(ENABLE_TRACE)
+    list(APPEND VERILATOR_COMMON_FLAGS --trace)
+    add_compile_definitions(VM_TRACE=1)
+endif()
+
+if(ENABLE_COVERAGE)
+    list(APPEND VERILATOR_COMMON_FLAGS --coverage)
+endif()
+
+include(${CMAKE_SOURCE_DIR}/cmake/verilator.cmake)
+
+add_subdirectory(rtl)
+
+message(STATUS "")
+message(STATUS "RV64CPU configuration")
+message(STATUS "  Trace:    ${ENABLE_TRACE}")
+message(STATUS "  Coverage: ${ENABLE_COVERAGE}")
+message(STATUS "  RTL files: ${RV64CPU_RTL_SOURCE_COUNT}")
+message(STATUS "")
diff --git a/rv64cpu/COPYING b/rv64cpu/COPYING
new file mode 100644
index 0000000..f288702
--- /dev/null
+++ b/rv64cpu/COPYING
@@ -0,0 +1,674 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+  The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works.  By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users.  We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors.  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+  To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights.  Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received.  You must make sure that they, too, receive
+or can get the source code.  And you must show them these terms so they
+know their rights.
+
+  Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+  For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software.  For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+  Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so.  This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software.  The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable.  Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products.  If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+  Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary.  To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                       TERMS AND CONDITIONS
+
+  0. Definitions.
+
+  "This License" refers to version 3 of the GNU General Public License.
+
+  "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+  "The Program" refers to any copyrightable work licensed under this
+License.  Each licensee is addressed as "you".  "Licensees" and
+"recipients" may be individuals or organizations.
+
+  To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy.  The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+  A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+  To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy.  Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+  To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies.  Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+  An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License.  If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+  1. Source Code.
+
+  The "source code" for a work means the preferred form of the work
+for making modifications to it.  "Object code" means any non-source
+form of a work.
+
+  A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+  The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form.  A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+  The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities.  However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work.  For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+  The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+  The Corresponding Source for a work in source code form is that
+same work.
+
+  2. Basic Permissions.
+
+  All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met.  This License explicitly affirms your unlimited
+permission to run the unmodified Program.  The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work.  This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+  You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force.  You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright.  Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+  Conveying under any other circumstances is permitted solely under
+the conditions stated below.  Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+  3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+  No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+  When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+  4. Conveying Verbatim Copies.
+
+  You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+  You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+  5. Conveying Modified Source Versions.
+
+  You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+    a) The work must carry prominent notices stating that you modified
+    it, and giving a relevant date.
+
+    b) The work must carry prominent notices stating that it is
+    released under this License and any conditions added under section
+    7.  This requirement modifies the requirement in section 4 to
+    "keep intact all notices".
+
+    c) You must license the entire work, as a whole, under this
+    License to anyone who comes into possession of a copy.  This
+    License will therefore apply, along with any applicable section 7
+    additional terms, to the whole of the work, and all its parts,
+    regardless of how they are packaged.  This License gives no
+    permission to license the work in any other way, but it does not
+    invalidate such permission if you have separately received it.
+
+    d) If the work has interactive user interfaces, each must display
+    Appropriate Legal Notices; however, if the Program has interactive
+    interfaces that do not display Appropriate Legal Notices, your
+    work need not make them do so.
+
+  A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit.  Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+  6. Conveying Non-Source Forms.
+
+  You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+    a) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by the
+    Corresponding Source fixed on a durable physical medium
+    customarily used for software interchange.
+
+    b) Convey the object code in, or embodied in, a physical product
+    (including a physical distribution medium), accompanied by a
+    written offer, valid for at least three years and valid for as
+    long as you offer spare parts or customer support for that product
+    model, to give anyone who possesses the object code either (1) a
+    copy of the Corresponding Source for all the software in the
+    product that is covered by this License, on a durable physical
+    medium customarily used for software interchange, for a price no
+    more than your reasonable cost of physically performing this
+    conveying of source, or (2) access to copy the
+    Corresponding Source from a network server at no charge.
+
+    c) Convey individual copies of the object code with a copy of the
+    written offer to provide the Corresponding Source.  This
+    alternative is allowed only occasionally and noncommercially, and
+    only if you received the object code with such an offer, in accord
+    with subsection 6b.
+
+    d) Convey the object code by offering access from a designated
+    place (gratis or for a charge), and offer equivalent access to the
+    Corresponding Source in the same way through the same place at no
+    further charge.  You need not require recipients to copy the
+    Corresponding Source along with the object code.  If the place to
+    copy the object code is a network server, the Corresponding Source
+    may be on a different server (operated by you or a third party)
+    that supports equivalent copying facilities, provided you maintain
+    clear directions next to the object code saying where to find the
+    Corresponding Source.  Regardless of what server hosts the
+    Corresponding Source, you remain obligated to ensure that it is
+    available for as long as needed to satisfy these requirements.
+
+    e) Convey the object code using peer-to-peer transmission, provided
+    you inform other peers where the object code and Corresponding
+    Source of the work are being offered to the general public at no
+    charge under subsection 6d.
+
+  A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+  A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling.  In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage.  For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product.  A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+  "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source.  The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+  If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information.  But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+  The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed.  Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+  Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+  7. Additional Terms.
+
+  "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law.  If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+  When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it.  (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.)  You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+  Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+    a) Disclaiming warranty or limiting liability differently from the
+    terms of sections 15 and 16 of this License; or
+
+    b) Requiring preservation of specified reasonable legal notices or
+    author attributions in that material or in the Appropriate Legal
+    Notices displayed by works containing it; or
+
+    c) Prohibiting misrepresentation of the origin of that material, or
+    requiring that modified versions of such material be marked in
+    reasonable ways as different from the original version; or
+
+    d) Limiting the use for publicity purposes of names of licensors or
+    authors of the material; or
+
+    e) Declining to grant rights under trademark law for use of some
+    trade names, trademarks, or service marks; or
+
+    f) Requiring indemnification of licensors and authors of that
+    material by anyone who conveys the material (or modified versions of
+    it) with contractual assumptions of liability to the recipient, for
+    any liability that these contractual assumptions directly impose on
+    those licensors and authors.
+
+  All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10.  If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term.  If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+  If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+  Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+  8. Termination.
+
+  You may not propagate or modify a covered work except as expressly
+provided under this License.  Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+  However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+  Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+  Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License.  If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+  9. Acceptance Not Required for Having Copies.
+
+  You are not required to accept this License in order to receive or
+run a copy of the Program.  Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance.  However,
+nothing other than this License grants you permission to propagate or
+modify any covered work.  These actions infringe copyright if you do
+not accept this License.  Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+  10. Automatic Licensing of Downstream Recipients.
+
+  Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License.  You are not responsible
+for enforcing compliance by third parties with this License.
+
+  An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations.  If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+  You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License.  For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+  11. Patents.
+
+  A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based.  The
+work thus licensed is called the contributor's "contributor version".
+
+  A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version.  For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+  Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+  In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement).  To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+  If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients.  "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+  If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+  A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License.  You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+  Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+  12. No Surrender of Others' Freedom.
+
+  If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all.  For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+  13. Use with the GNU Affero General Public License.
+
+  Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work.  The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+  14. Revised Versions of this License.
+
+  The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+  Each version is given a distinguishing version number.  If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation.  If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+  If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+  Later license versions may give you additional or different
+permissions.  However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+  15. Disclaimer of Warranty.
+
+  THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW.  EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU.  SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. Limitation of Liability.
+
+  IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+  17. Interpretation of Sections 15 and 16.
+
+  If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the program's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+  If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+    <program>  Copyright (C) <year>  <name of author>
+    This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+  You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+<https://www.gnu.org/licenses/>.
+
+  The GNU General Public License does not permit incorporating your program
+into proprietary programs.  If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.  But first, please read
+<https://www.gnu.org/licenses/why-not-lgpl.html>.
diff --git a/rv64cpu/README b/rv64cpu/README
new file mode 100644
index 0000000..d6118cc
--- /dev/null
+++ b/rv64cpu/README
@@ -0,0 +1,7 @@
+RV64CPU
+=======
+
+A RISC-V processor core project written in SystemVerilog.
+
+RV64CPU is free software.  See the file COPYING for
+copying permission.
diff --git a/rv64cpu/cmake/verilator.cmake b/rv64cpu/cmake/verilator.cmake
new file mode 100644
index 0000000..8c0a006
--- /dev/null
+++ b/rv64cpu/cmake/verilator.cmake
@@ -0,0 +1,60 @@
+# ─── Verilator CMake Helper ──────────────────────────────────────────────────────
+#
+# Provides the `add_verilator_target()` function to simplify creating
+# Verilator simulation executables.
+#
+# Usage:
+#   add_verilator_target(
+#       NAME        tb_alu
+#       TOP_MODULE  alu
+#       SOURCES     ${RTL_CORE_DIR}/alu.sv
+#       INCLUDE_DIRS ${RTL_INCLUDE_DIR}
+#       TB_SOURCES  ${CMAKE_CURRENT_SOURCE_DIR}/unit/tb_alu.cpp
+#       PREFIX      Valu
+#   )
+
+function(add_verilator_target)
+    cmake_parse_arguments(VT
+        ""                                          # Options
+        "NAME;TOP_MODULE;PREFIX"                     # Single-value args
+        "SOURCES;INCLUDE_DIRS;TB_SOURCES;EXTRA_FLAGS" # Multi-value args
+        ${ARGN}
+    )
+
+    if(NOT VT_PREFIX)
+        set(VT_PREFIX "V${VT_TOP_MODULE}")
+    endif()
+
+    # Create C++ executable for the testbench
+    add_executable(${VT_NAME} ${VT_TB_SOURCES})
+
+    target_include_directories(${VT_NAME} PRIVATE
+        ${CMAKE_CURRENT_SOURCE_DIR}/common
+    )
+
+    # Build include dir flags
+    set(_INCLUDE_FLAGS "")
+    foreach(_dir ${VT_INCLUDE_DIRS})
+        list(APPEND _INCLUDE_FLAGS "-I${_dir}")
+    endforeach()
+
+    # Verilate the RTL sources
+    verilate(${VT_NAME}
+        PREFIX      ${VT_PREFIX}
+        SOURCES     ${VT_SOURCES}
+        TOP_MODULE  ${VT_TOP_MODULE}
+        VERILATOR_ARGS
+            ${VERILATOR_COMMON_FLAGS}
+            ${_INCLUDE_FLAGS}
+            ${VT_EXTRA_FLAGS}
+    )
+
+    # Register as CTest
+    add_test(
+        NAME    ${VT_NAME}
+        COMMAND ${VT_NAME}
+    )
+
+    # Set test timeout
+    set_tests_properties(${VT_NAME} PROPERTIES TIMEOUT 60)
+endfunction()
diff --git a/rv64cpu/docs/architecture.md b/rv64cpu/docs/architecture.md
new file mode 100644
index 0000000..9df23d4
--- /dev/null
+++ b/rv64cpu/docs/architecture.md
@@ -0,0 +1,204 @@
+# Microarchitecture Design Document
+
+## Overview
+
+| Parameter | Value |
+|---|---|
+| ISA | RV64GCZicsr_Zifencei (I+M+A+F+D+C) |
+| Pipeline | 5-stage in-order single-issue (IF → ID → EX → MEM → WB) |
+| Privilege Levels | M / S / U |
+| Virtual Memory | Sv39 (39-bit VA, 56-bit PA, three-level page table) |
+| Bus | AXI4-Full (Cache) + AXI4-Lite (Peripherals) |
+| Cache | I-Cache 4KB + D-Cache 4KB (direct-mapped, upgradeable to set-associative) |
+| Debug | JTAG Debug (reserved) |
+
+---
+
+## Pipeline Timing
+
+```
+Cycle:    1     2     3     4     5     6     7
+         ┌─────┬─────┬─────┬─────┬─────┐
+Instr 1  │ IF  │ ID  │ EX  │ MEM │ WB  │
+         └─────┼─────┼─────┼─────┼─────┼─────┐
+Instr 2        │ IF  │ ID  │ EX  │ MEM │ WB  │
+               └─────┼─────┼─────┼─────┼─────┼─────┐
+Instr 3              │ IF  │ ID  │ EX  │ MEM │ WB  │
+                     └─────┴─────┴─────┴─────┴─────┘
+```
+
+### Stage Functions
+
+| Stage | Module | Description |
+|---|---|---|
+| **IF** | `if_stage.sv` | PC generation, iTLB virtual-to-physical translation, I-Cache read, instruction alignment, branch prediction interface |
+| **ID** | `id_stage.sv` | Instruction decode (`decoder.sv`), register file read (`regfile.sv`), immediate extension, CSR read, control signal generation |
+| **EX** | `ex_stage.sv` | ALU operations (`alu.sv`), branch resolution (`branch_unit.sv`), multiply/divide (`muldiv.sv`), FPU operations (`fpu.sv`), effective address calculation |
+| **MEM** | `mem_stage.sv` | dTLB virtual-to-physical translation, D-Cache read/write, load data alignment/sign extension, exception detection |
+| **WB** | `wb_stage.sv` | Register file writeback, CSR writeback, final exception/interrupt handling |
+
+### Hazard Handling (`hazard_unit.sv`)
+
+| Type | Strategy |
+|---|---|
+| RAW Data Hazard | EX→EX forwarding, MEM→EX forwarding |
+| Load-use | Stall 1 cycle (insert bubble at IF/ID stage) |
+| Control Hazard | Static not-taken prediction; branch resolved at EX stage, flush IF/ID on misprediction |
+| Structural Hazard | Eliminated by separate I-Cache / D-Cache |
+| FPU Long Latency | Stall EX stage until FPU result ready (or multi-cycle EX) |
+
+---
+
+## Privilege Level Architecture
+
+### Mode Switching
+
+```mermaid
+stateDiagram-v2
+    [*] --> M_Mode : Reset
+    M_Mode --> S_Mode : mret (mstatus.MPP=S)
+    M_Mode --> U_Mode : mret (mstatus.MPP=U)
+    S_Mode --> M_Mode : ecall / exception / interrupt (not delegated)
+    S_Mode --> U_Mode : sret (sstatus.SPP=U)
+    U_Mode --> S_Mode : ecall / exception / interrupt (delegated)
+    U_Mode --> M_Mode : ecall / exception / interrupt (not delegated)
+```
+
+### CSR Register List
+
+#### M-Mode CSRs
+| CSR | Address | Purpose |
+|---|---|---|
+| `mstatus` | 0x300 | Global status (MIE, MPIE, MPP, FS, etc.) |
+| `misa` | 0x301 | ISA capability descriptor (RV64IMAFD) |
+| `medeleg` | 0x302 | Exception delegation to S-mode |
+| `mideleg` | 0x303 | Interrupt delegation to S-mode |
+| `mie` | 0x304 | Interrupt enable |
+| `mtvec` | 0x305 | Trap vector base address |
+| `mscratch` | 0x340 | M-mode scratch register |
+| `mepc` | 0x341 | Exception PC |
+| `mcause` | 0x342 | Trap cause |
+| `mtval` | 0x343 | Trap value |
+| `mip` | 0x344 | Interrupt pending |
+| `mcycle` | 0xB00 | Cycle counter |
+| `minstret` | 0xB02 | Retired instruction counter |
+| `mvendorid` | 0xF11 | Vendor ID |
+| `marchid` | 0xF12 | Architecture ID |
+| `mimpid` | 0xF13 | Implementation ID |
+| `mhartid` | 0xF14 | Hart ID |
+
+#### S-Mode CSRs
+| CSR | Address | Purpose |
+|---|---|---|
+| `sstatus` | 0x100 | S-mode status (subset of mstatus) |
+| `sie` | 0x104 | S-mode interrupt enable |
+| `stvec` | 0x105 | S-mode trap vector |
+| `sscratch` | 0x140 | S-mode scratch register |
+| `sepc` | 0x141 | S-mode exception PC |
+| `scause` | 0x142 | S-mode trap cause |
+| `stval` | 0x143 | S-mode trap value |
+| `sip` | 0x144 | S-mode interrupt pending |
+| `satp` | 0x180 | Page table base address + MODE (Sv39) |
+
+---
+
+## MMU (Sv39)
+
+### Address Translation
+
+```
+63        39 38        30 29        21 20        12 11         0
+┌──────────┬────────────┬────────────┬────────────┬────────────┐
+│  (sign)  │  VPN[2]    │  VPN[1]    │  VPN[0]    │  Offset    │
+│ 25 bits  │  9 bits    │  9 bits    │  9 bits    │  12 bits   │
+└──────────┴────────────┴────────────┴────────────┴────────────┘
+                    39-bit Virtual Address
+```
+
+- **iTLB** (IF stage): 16-entry fully associative, for instruction fetch virtual-to-physical translation
+- **dTLB** (MEM stage): 16-entry fully associative, for Load/Store virtual-to-physical translation
+- **PTW** (`ptw.sv`): On TLB miss, hardware walks the three-level page table, reading page table entries via D-Cache
+- **sfence.vma**: Flush TLB (all entries or selective by ASID/VPN)
+
+### Module Organization
+
+```
+mmu/
+├── mmu_top.sv      ← Top-level: arbitrates iTLB/dTLB PTW requests
+├── tlb.sv          ← Parameterized TLB: ENTRIES, ASSOC, ASID support
+└── ptw.sv          ← Page Table Walker: three-level lookup state machine
+```
+
+---
+
+## FPU Architecture (F+D Extensions)
+
+### Module Organization
+
+```
+rtl/core/fpu/
+├── fpu_top.sv          ← FPU top-level (dispatch + result selection)
+├── fp_regfile.sv       ← Floating-point register file f0-f31 (64-bit)
+├── fp_add.sv           ← Addition/subtraction unit
+├── fp_mul.sv           ← Multiplication unit
+├── fp_div_sqrt.sv      ← Division/square root (multi-cycle)
+├── fp_fma.sv           ← Fused multiply-add (optional, high performance)
+├── fp_conv.sv          ← Format conversion (int↔fp, f32↔f64)
+├── fp_cmp.sv           ← Compare/classify (FEQ/FLT/FLE/FCLASS)
+└── include/
+    └── fp_pkg.sv       ← Floating-point types, rounding modes, exception flags
+```
+
+> **Note**: FPU operations typically require multiple cycles (especially division/square root), requiring FPU long-latency stall handling in hazard_unit. CSRs `fcsr` / `frm` / `fflags` are implemented in `csr_unit.sv`.
+
+---
+
+## Cache Subsystem
+
+See [memory_map.md](memory_map.md) for address mapping.
+
+| Parameter | I-Cache | D-Cache |
+|---|---|---|
+| Capacity | 4 KB | 4 KB |
+| Line Size | 64 B (8 × 64-bit) | 64 B |
+| Associativity | Direct-mapped (initial) | Direct-mapped (initial) |
+| Write Policy | N/A | Write-back + write-allocate |
+| Refill Bus | AXI4-Full Burst | AXI4-Full Burst |
+
+### Cache Line Calculation
+
+- 4 KB / 64 B = 64 lines
+- Tag = PA[55:12], Index = PA[11:6], Offset = PA[5:0]
+
+---
+
+## Bus Interconnect
+
+```
+┌────────────────┐     ┌────────────────┐
+│   I-Cache      │     │   D-Cache      │
+│  AXI4 Master   │     │  AXI4 Master   │
+└───────┬────────┘     └───────┬────────┘
+        │                      │
+        └──────┐    ┌──────────┘
+               ▼    ▼
+        ┌──────────────┐
+        │  AXI4 Arbiter │
+        └──────┬───────┘
+               ▼
+        ┌──────────────┐
+        │  AXI4 Xbar   │ ← Address decode
+        └──┬───┬───┬───┘
+           │   │   │
+    ┌──────┘   │   └──────┐
+    ▼          ▼          ▼
+┌───────┐ ┌───────────┐ ┌─────────────────┐
+│ SRAM  │ │AXI4→Lite  │ │ (Expandable     │
+│  Ctrl │ │  Bridge   │ │      Slave)     │
+└───────┘ └─────┬─────┘ └─────────────────┘
+                │
+        ┌───┬───┴───┬───┐
+        ▼   ▼       ▼   ▼
+     CLINT PLIC   UART GPIO
+     (AXI4-Lite Slaves)
+```
diff --git a/rv64cpu/docs/isa_support.md b/rv64cpu/docs/isa_support.md
new file mode 100644
index 0000000..0173a58
--- /dev/null
+++ b/rv64cpu/docs/isa_support.md
@@ -0,0 +1,224 @@
+# ISA Support Matrix
+
+## Target ISA String
+
+```
+RV64GCZicsr_Zifencei
+```
+
+Equivalent to `RV64IMAFDCZicsr_Zifencei`
+
+---
+
+## Extension Implementation Status
+
+| Extension | Full Name | Status | Description |
+|---|---|---|---|
+| **I** | Base Integer (RV64I) | 🟢 Implemented | Strict ACT4 compliance suite passes (`rv64i`) |
+| **M** | Multiply/Divide | 🟢 Implemented | Strict ACT4 compliance suite passes (`rv64im`) |
+| **A** | Atomic | 🟢 Implemented | Strict ACT4 compliance suite passes (`rv64ia`) |
+| **F** | Single-Precision Float | 🟢 Implemented | Strict ACT4 compliance suite passes (`rv64if`) |
+| **D** | Double-Precision Float | 🟢 Implemented | Strict ACT4 compliance suite passes (`rv64id`) |
+| **C** | Compressed | 🟢 Implemented | Strict ACT4 compliance suite passes (`rv64ic`) |
+| **Zicsr** | CSR Instructions | 🟢 Implemented | Full CSR unit with delegation, FCSR, and dedicated testbench ([rtl/core/csr_unit.sv](rtl/core/csr_unit.sv), [tb/unit/tb_csr.cc](tb/unit/tb_csr.cc)) |
+| **Zifencei** | Instruction Fence | 🟢 Implemented | Included in strict ACT4 passing suites |
+
+Privilege-related tests (`S`, `Sm`) also pass through `rv64priv` strict suite.
+
+---
+
+## Recent Progress Highlights (2026-04)
+
+- `riscv-arch-test` ACT4 flow integrated in `scripts/run_compliance.py`.
+- Dedicated `tb/compliance/compliance_runner.cc` executes ELF tests and dumps signatures.
+- Strict suite passes with reference comparison enabled (`--require-reference`):
+	- `rv64i` (51/51)
+	- `rv64im` (64/64)
+	- `rv64ia` (51/51)
+	- `rv64ic` (51/51)
+	- `rv64if` (133/133)
+	- `rv64id` (165/165)
+	- `rv64priv` (2/2)
+	- `rv64full` aggregate (262/262)
+
+## Detailed Instruction Tables
+
+The detailed per-instruction checklist sections below are retained as reference,
+but are not the primary source of truth for completion status at this stage.
+Current completion is determined by strict ACT4 suite pass results listed above.
+
+---
+
+## RV64I Instruction List
+
+### R-type Arithmetic/Logic
+| Instruction | Operation | Status |
+|---|---|---|
+| ADD | rd = rs1 + rs2 | ⬜ |
+| SUB | rd = rs1 - rs2 | ⬜ |
+| AND | rd = rs1 & rs2 | ⬜ |
+| OR | rd = rs1 \| rs2 | ⬜ |
+| XOR | rd = rs1 ^ rs2 | ⬜ |
+| SLL | rd = rs1 << rs2[5:0] | ⬜ |
+| SRL | rd = rs1 >> rs2[5:0] (logical) | ⬜ |
+| SRA | rd = rs1 >> rs2[5:0] (arith) | ⬜ |
+| SLT | rd = (rs1 < rs2) ? 1 : 0 (signed) | ⬜ |
+| SLTU | rd = (rs1 < rs2) ? 1 : 0 (unsigned) | ⬜ |
+
+### R-type RV64 Word Operations
+| Instruction | Operation | Status |
+|---|---|---|
+| ADDW | rd = sext((rs1+rs2)[31:0]) | ⬜ |
+| SUBW | rd = sext((rs1-rs2)[31:0]) | ⬜ |
+| SLLW | rd = sext((rs1 << rs2[4:0])[31:0]) | ⬜ |
+| SRLW | rd = sext((rs1[31:0] >> rs2[4:0])) | ⬜ |
+| SRAW | rd = sext((rs1[31:0] >>> rs2[4:0])) | ⬜ |
+
+### I-type Immediate Arithmetic
+| Instruction | Operation | Status |
+|---|---|---|
+| ADDI | rd = rs1 + imm | ⬜ |
+| ANDI | rd = rs1 & imm | ⬜ |
+| ORI | rd = rs1 \| imm | ⬜ |
+| XORI | rd = rs1 ^ imm | ⬜ |
+| SLLI | rd = rs1 << shamt[5:0] | ⬜ |
+| SRLI | rd = rs1 >> shamt[5:0] (logical) | ⬜ |
+| SRAI | rd = rs1 >> shamt[5:0] (arith) | ⬜ |
+| SLTI | rd = (rs1 < imm) ? 1 : 0 (signed) | ⬜ |
+| SLTIU | rd = (rs1 < imm) ? 1 : 0 (unsigned) | ⬜ |
+| ADDIW | rd = sext((rs1+imm)[31:0]) | ⬜ |
+| SLLIW | rd = sext((rs1 << shamt[4:0])[31:0]) | ⬜ |
+| SRLIW | rd = sext((rs1[31:0] >> shamt[4:0])) | ⬜ |
+| SRAIW | rd = sext((rs1[31:0] >>> shamt[4:0])) | ⬜ |
+
+### Load
+| Instruction | Width | Sign Extension | Status |
+|---|---|---|---|
+| LB | 8-bit | Signed | ⬜ |
+| LBU | 8-bit | Zero-extended | ⬜ |
+| LH | 16-bit | Signed | ⬜ |
+| LHU | 16-bit | Zero-extended | ⬜ |
+| LW | 32-bit | Signed | ⬜ |
+| LWU | 32-bit | Zero-extended | ⬜ |
+| LD | 64-bit | — | ⬜ |
+
+### Store
+| Instruction | Width | Status |
+|---|---|---|
+| SB | 8-bit | ⬜ |
+| SH | 16-bit | ⬜ |
+| SW | 32-bit | ⬜ |
+| SD | 64-bit | ⬜ |
+
+### Branch
+| Instruction | Condition | Status |
+|---|---|---|
+| BEQ | rs1 == rs2 | ⬜ |
+| BNE | rs1 != rs2 | ⬜ |
+| BLT | rs1 < rs2 (signed) | ⬜ |
+| BGE | rs1 >= rs2 (signed) | ⬜ |
+| BLTU | rs1 < rs2 (unsigned) | ⬜ |
+| BGEU | rs1 >= rs2 (unsigned) | ⬜ |
+
+### Upper Immediate / Jump
+| Instruction | Operation | Status |
+|---|---|---|
+| LUI | rd = imm << 12 | ⬜ |
+| AUIPC | rd = pc + (imm << 12) | ⬜ |
+| JAL | rd = pc+4; pc = pc + imm | ⬜ |
+| JALR | rd = pc+4; pc = (rs1 + imm) & ~1 | ⬜ |
+
+### System
+| Instruction | Status |
+|---|---|
+| ECALL | ⬜ |
+| EBREAK | ⬜ |
+| MRET | ⬜ |
+| SRET | ⬜ |
+| WFI | ⬜ |
+| FENCE | ⬜ |
+| FENCE.I | ⬜ |
+| SFENCE.VMA | ⬜ |
+
+---
+
+## M Extension (Multiply/Divide)
+
+| Instruction | Operation | Status |
+|---|---|---|
+| MUL | rd = (rs1 × rs2)[63:0] | ⬜ |
+| MULH | rd = (rs1 × rs2)[127:64] signed | ⬜ |
+| MULHSU | rd = (rs1 × rs2)[127:64] signed×unsigned | ⬜ |
+| MULHU | rd = (rs1 × rs2)[127:64] unsigned | ⬜ |
+| DIV | rd = rs1 / rs2 signed | ⬜ |
+| DIVU | rd = rs1 / rs2 unsigned | ⬜ |
+| REM | rd = rs1 % rs2 signed | ⬜ |
+| REMU | rd = rs1 % rs2 unsigned | ⬜ |
+| MULW | rd = sext((rs1×rs2)[31:0]) | ⬜ |
+| DIVW / DIVUW | 32-bit division | ⬜ |
+| REMW / REMUW | 32-bit remainder | ⬜ |
+
+---
+
+## A Extension (Atomic Instructions)
+
+| Instruction | Status |
+|---|---|
+| LR.W / LR.D | ⬜ |
+| SC.W / SC.D | ⬜ |
+| AMOSWAP.W/D | ⬜ |
+| AMOADD.W/D | ⬜ |
+| AMOAND.W/D | ⬜ |
+| AMOOR.W/D | ⬜ |
+| AMOXOR.W/D | ⬜ |
+| AMOMAX[U].W/D | ⬜ |
+| AMOMIN[U].W/D | ⬜ |
+
+---
+
+## F/D Extension (Floating-Point)
+
+| Category | F (32-bit) Instructions | D (64-bit) Instructions | Status |
+|---|---|---|---|
+| Load/Store | FLW, FSW | FLD, FSD | ⬜ |
+| Arithmetic | FADD.S, FSUB.S, FMUL.S, FDIV.S, FSQRT.S | FADD.D, ... | ⬜ |
+| Fused Multiply-Add | FMADD.S, FMSUB.S, FNMADD.S, FNMSUB.S | FMADD.D, ... | ⬜ |
+| Sign Manipulation | FSGNJ[N/X].S | FSGNJ[N/X].D | ⬜ |
+| Compare | FEQ.S, FLT.S, FLE.S | FEQ.D, FLT.D, FLE.D | ⬜ |
+| Classify | FCLASS.S | FCLASS.D | ⬜ |
+| Conversion | FCVT.W/WU/L/LU.S, FCVT.S.W/WU/L/LU | FCVT.*.D | ⬜ |
+| F↔D Conversion | — | FCVT.S.D, FCVT.D.S | ⬜ |
+| Move | FMV.X.W, FMV.W.X | FMV.X.D, FMV.D.X | ⬜ |
+| MIN/MAX | FMIN.S, FMAX.S | FMIN.D, FMAX.D | ⬜ |
+
+### Floating-Point CSRs
+| CSR | Address | Description |
+|---|---|---|
+| `fflags` | 0x001 | Floating-point exception flags (NV, DZ, OF, UF, NX) |
+| `frm` | 0x002 | Floating-point rounding mode |
+| `fcsr` | 0x003 | fflags + frm combined |
+
+---
+
+## C Extension (Compressed Instructions)
+
+16-bit encoded subset of common instructions, improving code density. Requires handling 16/32-bit mixed instruction stream alignment at the IF stage.
+
+| Quadrant | Selected Instructions | Status |
+|---|---|---|
+| C0 | C.LW, C.LD, C.SW, C.SD, C.ADDI4SPN | ⬜ |
+| C1 | C.ADDI, C.LI, C.LUI, C.SRLI, C.SRAI, C.ANDI, C.SUB, C.XOR, C.OR, C.AND, C.J, C.BEQZ, C.BNEZ | ⬜ |
+| C2 | C.SLLI, C.LDSP, C.LWSP, C.JR, C.MV, C.JALR, C.ADD, C.SDSP, C.SWSP | ⬜ |
+
+---
+
+## Zicsr Instructions
+
+| Instruction | Operation | Status |
+|---|---|---|
+| CSRRW | t=CSR; CSR=rs1; rd=t | ⬜ |
+| CSRRS | t=CSR; CSR=t\|rs1; rd=t | ⬜ |
+| CSRRC | t=CSR; CSR=t&~rs1; rd=t | ⬜ |
+| CSRRWI | t=CSR; CSR=zimm; rd=t | ⬜ |
+| CSRRSI | t=CSR; CSR=t\|zimm; rd=t | ⬜ |
+| CSRRCI | t=CSR; CSR=t&~zimm; rd=t | ⬜ |
diff --git a/rv64cpu/docs/memory_map.md b/rv64cpu/docs/memory_map.md
new file mode 100644
index 0000000..9bf529b
--- /dev/null
+++ b/rv64cpu/docs/memory_map.md
@@ -0,0 +1,113 @@
+# Memory Map
+
+## Physical Address Space
+
+```
+0xFFFF_FFFF_FFFF_FFFF ┌─────────────────────────┐
+                      │                         │
+                      │      (unused)            │
+                      │                         │
+0x0000_0000_1000_2000 ├─────────────────────────┤
+                      │  GPIO          4 KB     │
+0x0000_0000_1000_1000 ├─────────────────────────┤
+                      │  UART0         4 KB     │
+0x0000_0000_1000_0000 ├─────────────────────────┤
+                      │                         │
+                      │      (unused)            │
+                      │                         │
+0x0000_0000_0C00_0000 ├─────────────────────────┤
+                      │  PLIC          64 MB    │
+0x0000_0000_0BFF_FFFF ├─────────────────────────┤
+                      │                         │
+                      │      (unused)            │
+                      │                         │
+0x0000_0000_0200_FFFF ├─────────────────────────┤
+                      │  CLINT         64 KB    │
+0x0000_0000_0200_0000 ├─────────────────────────┤
+                      │                         │
+                      │      (unused)            │
+                      │                         │
+0x0000_0000_0FFF_FFFF ├─────────────────────────┤
+                      │                         │
+                      │  SRAM / RAM    ~256 MB  │
+                      │  (Boot ROM at lowest    │
+                      │        64KB)            │
+                      │                         │
+0x0000_0000_0000_0000 └─────────────────────────┘
+```
+
+## Address Decode Table
+
+| Start Address | End Address | Size | Peripheral | Bus | Description |
+|---|---|---|---|---|---|
+| `0x0000_0000` | `0x0FFF_FFFF` | 256 MB | SRAM Controller | AXI4-Full | Boot ROM (0x0~0xFFFF) + RAM |
+| `0x0200_0000` | `0x0200_FFFF` | 64 KB | CLINT | AXI4-Lite | mtime, mtimecmp, MSIP |
+| `0x0C00_0000` | `0x0FFF_FFFF` | 64 MB | PLIC | AXI4-Lite | External interrupt controller |
+| `0x1000_0000` | `0x1000_0FFF` | 4 KB | UART0 | AXI4-Lite | 16550 compatible |
+| `0x1000_1000` | `0x1000_1FFF` | 4 KB | GPIO | AXI4-Lite | General-purpose I/O |
+
+> **Note**: The address ranges for CLINT and PLIC follow the RISC-V community recommended layout, compatible with Linux/OpenSBI.
+
+## CLINT Register Layout
+
+| Offset | Register | Description |
+|---|---|---|
+| `0x0000` | `msip[0]` | Machine software interrupt pending |
+| `0x4000` | `mtimecmp[0]` | Machine timer compare value (64-bit) |
+| `0xBFF8` | `mtime` | Machine timer current value (64-bit) |
+
+## PLIC Register Layout (Simplified)
+
+| Offset | Register | Description |
+|---|---|---|
+| `0x000000` | `priority[1..N]` | Interrupt source priority |
+| `0x001000` | `pending[0..N/32]` | Interrupt pending bitfield |
+| `0x002000` | `enable[ctx][0..N/32]` | Interrupt enable bitfield |
+| `0x200000` | `threshold[ctx]` | Priority threshold |
+| `0x200004` | `claim/complete[ctx]` | Interrupt claim/complete |
+
+## UART0 Register Layout (16550)
+
+| Offset | Register | Read/Write | Description |
+|---|---|---|---|
+| `0x00` | RBR / THR | R / W | Receive/transmit data |
+| `0x01` | IER | R/W | Interrupt enable |
+| `0x02` | IIR / FCR | R / W | Interrupt identification / FIFO control |
+| `0x03` | LCR | R/W | Line control |
+| `0x05` | LSR | R | Line status |
+
+## Linker Script Correspondence
+
+The MEMORY regions in `sw/common/link.ld` correspond to this table:
+- `ROM`: `0x0000_0000`, 64 KB
+- `RAM`: `0x0001_0000`, 256 MB - 64 KB
+
+## Address Decode Implementation
+
+The address decode logic in RTL is located in `rtl/soc/mem_map.sv` and `rtl/bus/axi4_xbar.sv`.
+
+Constants are defined in `mem_map.sv`:
+
+```systemverilog
+package mem_map_pkg;
+    // SRAM
+    localparam logic [63:0] SRAM_BASE   = 64'h0000_0000;
+    localparam logic [63:0] SRAM_SIZE   = 64'h1000_0000;   // 256 MB
+
+    // CLINT
+    localparam logic [63:0] CLINT_BASE  = 64'h0200_0000;
+    localparam logic [63:0] CLINT_SIZE  = 64'h0001_0000;   // 64 KB
+
+    // PLIC
+    localparam logic [63:0] PLIC_BASE   = 64'h0C00_0000;
+    localparam logic [63:0] PLIC_SIZE   = 64'h0400_0000;   // 64 MB
+
+    // UART0
+    localparam logic [63:0] UART0_BASE  = 64'h1000_0000;
+    localparam logic [63:0] UART0_SIZE  = 64'h0000_1000;   // 4 KB
+
+    // GPIO
+    localparam logic [63:0] GPIO_BASE   = 64'h1000_1000;
+    localparam logic [63:0] GPIO_SIZE   = 64'h0000_1000;   // 4 KB
+endpackage
+```
diff --git a/rv64cpu/docs/naming_convention.md b/rv64cpu/docs/naming_convention.md
new file mode 100644
index 0000000..830c5e5
--- /dev/null
+++ b/rv64cpu/docs/naming_convention.md
@@ -0,0 +1,197 @@
+# RTL Signal Naming Convention
+
+This document defines the naming rules for all SystemVerilog signals, ports, types,
+and modules in the Richard-CPU project. Follow these rules when writing new RTL or
+modifying existing code.
+
+## 1. General Rules
+
+| Rule | Example |
+|---|---|
+| All names use `snake_case` (lowercase + underscores) | `alu_result`, `rs1_addr` |
+| Names should be descriptive; avoid single-letter names except loop indices | `i` in `for(int i = ...)` is acceptable |
+| Abbreviations are allowed **only** from the approved list (Section 7) | `addr` not `address`, `imm` not `immediate` |
+| No trailing numbers for disambiguation; use descriptive suffixes instead | `read_port_a` not `read_port_1` |
+
+### 1.1 Semantic Clarity Priority
+
+When multiple names are technically valid, prefer the name with the clearest
+semantic meaning:
+
+1. Prefer full words over unclear abbreviations unless listed in Section 7.
+2. Prefer domain-accurate nouns over generic placeholders.
+3. Prefer names that communicate role and timing (source, destination, stage).
+
+Examples:
+- `interrupt_cause` is preferred over `int_cause`
+- `trap_target_pc` is preferred over `trap_next_value`
+- `csr_write_data` is preferred over `csr_write_val`
+
+## 2. Module & Package Names
+
+| Item | Rule | Example |
+|---|---|---|
+| Module | `snake_case`, named after function | `alu`, `regfile`, `decoder`, `if_stage` |
+| Package | `snake_case` ending in `_pkg` | `rv64_pkg` |
+| Typedef | `snake_case` ending in `_t` | `alu_op_t`, `opcode_t` |
+| Enum members | `UPPER_SNAKE_CASE` with a common prefix | `ALU_ADD`, `OPCODE_LUI` |
+| Parameters / localparams | `UPPER_SNAKE_CASE` | `XLEN`, `INST_WIDTH` |
+
+## 3. Port & Signal Names
+
+### 3.1 Standard Suffixes
+
+Use these suffixes consistently to indicate the **role** of a signal:
+
+| Suffix | Meaning | Example |
+|---|---|---|
+| `_addr` | Address (register index or memory address) | `rs1_addr`, `rd_addr` |
+| `_data` | Data bus (generic read/write payload) | `mem_rdata`, `mem_wdata` |
+| `_rdata` | Read data (output from a storage element) | `rs1_rdata`, `rs2_rdata` |
+| `_wdata` | Write data (input to a storage element) | `rd_wdata` |
+| `_op` | Operation selector (typically an enum or funct3) | `alu_op`, `branch_op`, `csr_op` |
+| `_sel` | Mux select signal | `alu_src1_sel`, `wb_sel` |
+| `_en` | Enable (active-high gating signal) | `reg_write_en`, `mem_read_en` |
+| `_val` | **Deprecated — do not use for new code.** Use `_rdata`/`_wdata`/`_data` instead | — |
+
+Additional clarity rules:
+- `*_value` is allowed for architectural/state snapshots when `*_data` would be
+	ambiguous. Example: `csr_mstatus_value`.
+- For storage read/write payloads, always prefer `_rdata`/`_wdata`/`_data` over
+	`_value`.
+- `_flag` is deprecated for new boolean signals. Use `is_<noun>`, `has_<noun>`,
+	or `<adjective>_<noun>`.
+
+### 3.2 Standard Prefixes
+
+| Prefix | Meaning | Example |
+|---|---|---|
+| `is_` | Boolean flag indicating instruction type or state | `is_load`, `is_branch`, `is_csr` |
+| `has_` | Boolean flag indicating a condition is met | `has_exception`, `has_interrupt` |
+
+### 3.3 Rules for Boolean / Control Signals
+
+All single-bit control signals should follow **one** of these patterns:
+
+| Pattern | When to Use | Example |
+|---|---|---|
+| `is_<noun>` | Classifying an instruction or state | `is_load`, `is_jump`, `is_ecall` |
+| `<noun>_en` | Enabling a write or action | `reg_write_en`, `mem_write_en` |
+| `<adjective>_<noun>` | Describing a property | `illegal_instr`, `branch_taken` |
+
+> **Consistency rule**: For write-enable signals on storage elements (register file,
+> memory, CSR), always use the `_en` suffix. Example: `reg_write_en`, not `reg_write`.
+
+Additional boolean rules:
+- Avoid suffix-only boolean naming such as `*_flag` and `*_bool` in new code.
+- `has_` is for condition presence (`has_interrupt`), `is_` is for classification
+	(`is_interrupt` for a classified event or type).
+
+### 3.4 Clock & Reset
+
+| Signal | Name | Description |
+|---|---|---|
+| Clock | `clk` | Single system clock |
+| Reset | `rst_n` (preferred) or `rst` | Active-low (`rst_n`) is preferred; if active-high, use `rst` |
+
+> Current codebase uses active-high `rst`. If you later switch to active-low, rename
+> to `rst_n` everywhere in a single commit.
+
+## 4. Pipeline Stage Prefixes
+
+When signals cross pipeline stages, prefix them with the stage abbreviation:
+
+| Stage | Prefix | Example |
+|---|---|---|
+| Instruction Fetch | `if_` | `if_pc`, `if_instr` |
+| Instruction Decode | `id_` | `id_rs1_addr`, `id_imm` |
+| Execute | `ex_` | `ex_alu_result`, `ex_branch_taken` |
+| Memory Access | `mem_` | `mem_rdata`, `mem_addr` |
+| Write Back | `wb_` | `wb_rd_wdata`, `wb_rd_addr` |
+
+Inter-stage pipeline registers use a combined prefix: `id_ex_reg_write_en`,
+`ex_mem_alu_result`.
+
+## 5. ALU Port Naming
+
+The ALU is a **generic compute unit**. Its inputs are not always register values —
+they may come from the PC or an immediate after mux selection. Use neutral names:
+
+| Port | Current (non-conforming) | Recommended |
+|---|---|---|
+| Operand 1 input | `rs1_val` | `operand_a` |
+| Operand 2 input | `rs2_val` | `operand_b` |
+| Result output | `alu_res` | `alu_result` |
+
+## 6. Register File Port Naming
+
+| Port | Name | Description |
+|---|---|---|
+| Read address 1 | `rs1_addr` | Source register 1 index |
+| Read data 1 | `rs1_rdata` | Source register 1 data output |
+| Read address 2 | `rs2_addr` | Source register 2 index |
+| Read data 2 | `rs2_rdata` | Source register 2 data output |
+| Write address | `rd_addr` | Destination register index |
+| Write data | `rd_wdata` | Destination register data input |
+| Write enable | `reg_write_en` | Write enable (active high) |
+
+## 7. Approved Abbreviations
+
+Only these abbreviations are permitted. All other words should be spelled in full.
+
+| Abbreviation | Full Form |
+|---|---|
+| `addr` | address |
+| `alu` | arithmetic logic unit |
+| `clk` | clock |
+| `csr` | control and status register |
+| `en` | enable |
+| `ex` | execute (pipeline stage only) |
+| `fpu` | floating-point unit |
+| `id` | instruction decode (pipeline stage only) |
+| `if` | instruction fetch (pipeline stage only) |
+| `imm` | immediate |
+| `instr` | instruction |
+| `mem` | memory |
+| `mmu` | memory management unit |
+| `mux` | multiplexer |
+| `op` | operation |
+| `pc` | program counter |
+| `rd` | destination register |
+| `rdata` | read data |
+| `reg` | register |
+| `res` | **Deprecated — use `result`** |
+| `rs1` / `rs2` | source register 1 / 2 |
+| `rst` | reset |
+| `sel` | select |
+| `tlb` | translation lookaside buffer |
+| `val` | **Deprecated — use `rdata`/`wdata`/`data`** |
+| `wb` | write back (pipeline stage only) |
+| `wdata` | write data |
+
+Not approved for new code:
+- `int` (use `interrupt`)
+- `flag` suffix for booleans (use Section 3.3 patterns)
+
+## 7.1 Architecture-Defined Bit Names (Exception)
+
+Some names come directly from ISA-defined bit semantics and are allowed in narrow
+scopes even if they are single-letter:
+
+- PTE-like permission bits: `u`, `r`, `w`, `x`, `a`, `d`, `g`
+
+Scope limits:
+1. Allowed only inside tightly scoped structs, packed fields, or helper function
+	parameters tied to ISA semantics.
+2. For module ports and cross-module buses, prefer descriptive names such as
+	`pte_user`, `pte_read`, `pte_write`, `pte_exec`, `pte_accessed`, `pte_dirty`,
+	`pte_global`.
+
+## 8. Current Codebase Non-Conformances
+
+The following table lists signals in the existing codebase that do not yet follow
+this convention. These should be renamed during the next refactoring pass.
+
+| File | Current Name | Recommended Name | Reason |
+|---|---|---|---|
+|None|None|None|None|
diff --git a/rv64cpu/docs/pipeline_diagram.md b/rv64cpu/docs/pipeline_diagram.md
new file mode 100644
index 0000000..e2f6bdb
--- /dev/null
+++ b/rv64cpu/docs/pipeline_diagram.md
@@ -0,0 +1,156 @@
+# Pipeline Timing and Datapath
+
+## Five-Stage Pipeline Datapath
+
+```
+                 ┌────────────────────────────────────────────────────────────────┐
+                 │                    HAZARD UNIT                                 │
+                 │  Forward A/B, Stall, Flush, FPU interlock                     │
+                 └──┬─────────┬──────────┬──────────┬──────────┬────────────────┘
+                    │         │          │          │          │
+              ┌─────▼──┐ ┌───▼───┐ ┌────▼───┐ ┌───▼────┐ ┌──▼─────┐
+              │   IF   │ │  ID   │ │   EX   │ │  MEM   │ │   WB   │
+              │        │ │       │ │        │ │        │ │        │
+  ┌──────┐    │ PC Gen │ │Decode │ │  ALU   │ │D-Cache │ │ Write  │
+  │ iTLB ├───►│   ↓    │ │   ↓   │ │ Branch │ │  LSU   │ │  Back  │
+  │      │    │I-Cache │ │RegFile│ │ MulDiv │ │  dTLB  │ │        │
+  └──────┘    │ Fetch  │ │ Read  │ │  FPU   │ │        │ │        │
+              │        │ │ ImmEx │ │        │ │        │ │        │
+              └───┬────┘ └───┬───┘ └────┬───┘ └───┬────┘ └───┬────┘
+                  │          │          │          │          │
+              ┌───▼──────────▼──┐ ┌─────▼──────────▼──┐ ┌────▼─────┐
+              │  IF/ID Reg     │ │  EX/MEM Reg       │ │  MEM/WB  │
+              │                │ │                    │ │   Reg    │
+              └────────────────┘ └────────────────────┘ └──────────┘
+                                      ↑
+                              ┌───────┴───────┐
+                              │   CSR Unit    │
+                              │  Trap Ctrl    │
+                              └───────────────┘
+```
+
+---
+
+## Inter-Stage Registers
+
+### IF/ID Register
+
+| Field | Width | Description |
+|---|---|---|
+| `pc` | 64 | Current instruction PC |
+| `instr` | 32 | Fetched instruction |
+| `valid` | 1 | Instruction valid |
+| `exception` | 1 | IF stage exception (e.g., instruction page fault) |
+| `exc_code` | 4 | Exception code |
+| `predicted_taken` | 1 | Branch prediction result |
+| `predicted_target` | 64 | Predicted target address |
+
+### ID/EX Register
+
+| Field | Width | Description |
+|---|---|---|
+| `pc` | 64 | |
+| `rs1_data` | 64 | Register source 1 value |
+| `rs2_data` | 64 | Register source 2 value |
+| `rs3_data` | 64 | FPU source 3 value (FMA) |
+| `imm` | 64 | Sign-extended immediate |
+| `rd_addr` | 5 | Destination register address |
+| `rs1_addr` | 5 | Source 1 address (for forwarding) |
+| `rs2_addr` | 5 | Source 2 address (for forwarding) |
+| `ctrl` | struct | Control signal bundle (ALU op, mem r/w, branch type, etc.) |
+| `valid` | 1 | |
+| `is_fp` | 1 | Floating-point instruction flag |
+
+### EX/MEM Register
+
+| Field | Width | Description |
+|---|---|---|
+| `pc` | 64 | |
+| `alu_result` | 64 | ALU/FPU computation result |
+| `rs2_data` | 64 | Store data |
+| `rd_addr` | 5 | |
+| `ctrl_mem` | struct | Memory access control (read/write, size, sign_ext) |
+| `ctrl_wb` | struct | Writeback control (reg_write, wb_sel) |
+| `valid` | 1 | |
+
+### MEM/WB Register
+
+| Field | Width | Description |
+|---|---|---|
+| `pc` | 64 | |
+| `alu_result` | 64 | |
+| `mem_data` | 64 | Data read from D-Cache |
+| `rd_addr` | 5 | |
+| `ctrl_wb` | struct | Writeback control |
+| `valid` | 1 | |
+
+---
+
+## Forwarding Paths
+
+```
+          EX Stage                MEM Stage               WB Stage
+         ┌────────┐             ┌─────────┐             ┌────────┐
+         │ ALU    │             │alu_result│             │ wb_data│
+         │ result ├────────┐   │ or       ├────────┐   │        │
+         └────────┘        │   │ mem_data │        │   └────┬───┘
+                           │   └─────────┘        │        │
+                     ┌─────▼──────────────────────▼────────▼─┐
+                     │           Forward MUX                  │
+                     │  sel = hazard_unit.forward_a/b         │
+                     └─────────────────┬─────────────────────┘
+                                       │
+                                       ▼
+                              EX Stage rs1/rs2 input
+```
+
+| Forwarding Source | Condition |
+|---|---|
+| EX→EX | `ex_mem.rd == id_ex.rs1/rs2` and `ex_mem.reg_write` |
+| MEM→EX | `mem_wb.rd == id_ex.rs1/rs2` and `mem_wb.reg_write` and no EX forwarding |
+| Load-use | `id_ex.mem_read` and `id_ex.rd == if_id.rs1/rs2` → **stall** |
+
+---
+
+## Branch Handling Timing
+
+```
+Cycle:    1     2     3     4     5
+         ┌─────┬─────┬─────┬─────┬─────┐
+Branch   │ IF  │ ID  │ EX  │ MEM │ WB  │  ← Branch resolved at EX stage
+         └─────┼─────┼─────┼─────┼─────┘
+Instr+1        │ IF  │ ID  │ ██  │        ← Misprediction: flush (2-cycle penalty)
+               └─────┼─────┼─────┘
+Instr+2              │ IF  │ ██  │        ← flush
+                     └─────┴─────┘
+Target                     │ IF  │ ID  │ EX  │ ...  ← Correct path begins
+                           └─────┴─────┴─────┘
+```
+
+---
+
+## Cache Miss Timing
+
+```
+Cycle:   1    2    3    4    5    6    7    8    ...   N    N+1
+        ┌────┬────┬────┬────────────────────────────┬────┬────┐
+Instr   │ IF │ ID │ EX │ MEM (D-Cache MISS)         │    │ WB │
+        └────┴────┴────┤    Stall pipeline...        ├────┴────┘
+                       │    AXI4 Burst read (8 beats)│
+                       │    Fill cache line           │
+                       └─────────────────────────────┘
+```
+
+---
+
+## TLB Miss → PTW Timing
+
+```
+Cycle:   1    2    3    ...        N    N+1   N+2
+        ┌────┬────────────────────────┬────┬────┐
+Instr   │ IF │ iTLB MISS → PTW walk  │ IF │ ID │ ...
+        └────┤  (3-level page table   ├────┴────┘
+             │   × memory latency)    │
+             │  Pipeline stalled      │
+             └────────────────────────┘
+```
diff --git a/rv64cpu/rtl/CMakeLists.txt b/rv64cpu/rtl/CMakeLists.txt
new file mode 100644
index 0000000..96560fc
--- /dev/null
+++ b/rv64cpu/rtl/CMakeLists.txt
@@ -0,0 +1,44 @@
+set(RV64CPU_RTL_SOURCES
+    ${CMAKE_CURRENT_SOURCE_DIR}/packages/rv64_pkg.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/packages/csr_pkg.sv
+
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/execute/integer_alu.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/execute/branch_unit.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/execute/instruction_decoder.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/execute/compressed_decoder.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/execute/hazard_unit.sv
+
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/registers/gpr_file.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/registers/fpr_file.sv
+
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/if_stage.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/if_id_reg.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/id_stage.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/id_ex_reg.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/ex_stage.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/ex_mem_reg.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/mem_stage.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/mem_wb_reg.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/pipeline/wb_stage.sv
+
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/privileged/csr_unit.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/privileged/trap_controller.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/privileged/pmp_checker.sv
+
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/mmu/tlb.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/mmu/ptw.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/mmu/mmu.sv
+
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/fpu/fpu.sv
+    ${CMAKE_CURRENT_SOURCE_DIR}/core/rv64_core.sv
+)
+
+set(RV64CPU_RTL_INCLUDE_DIRS
+    ${CMAKE_CURRENT_SOURCE_DIR}/packages
+)
+
+list(LENGTH RV64CPU_RTL_SOURCES RV64CPU_RTL_SOURCE_COUNT)
+
+set(RV64CPU_RTL_SOURCES ${RV64CPU_RTL_SOURCES} PARENT_SCOPE)
+set(RV64CPU_RTL_INCLUDE_DIRS ${RV64CPU_RTL_INCLUDE_DIRS} PARENT_SCOPE)
+set(RV64CPU_RTL_SOURCE_COUNT ${RV64CPU_RTL_SOURCE_COUNT} PARENT_SCOPE)
diff --git a/rv64cpu/rtl/core/execute/branch_unit.sv b/rv64cpu/rtl/core/execute/branch_unit.sv
new file mode 100644
index 0000000..c9662ce
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/branch_unit.sv
@@ -0,0 +1,52 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/branch_unit.sv
+// Description: Branch Unit, responsible for determining whether conditional jump instructions are effective
+
+module branch_unit 
+    import rv64_pkg::*;
+(
+    input   logic              is_branch,      // Indicates whether the current instruction is a branch instruction
+    input   logic [2:0]        branch_op,      // Specific branch types func3
+    input   logic [XLEN-1:0]   operand_a,      // operand 1
+    input   logic [XLEN-1:0]   operand_b,      // operand 2
+    output  logic              branch_taken    // Branch taken signal
+);
+
+    always_comb begin
+        branch_taken = 1'b0;
+        
+        if (is_branch) begin
+            case (branch_op)
+                // 3'b000: BEQ (Branch if Equal)
+                BRANCH_EQ: branch_taken = (operand_a == operand_b);
+                // 3'b001: BNE (Branch if Not Equal)
+                BRANCH_NE: branch_taken = (operand_a != operand_b);
+                // 3'b100: BLT (Branch if Less Than, Signed)
+                BRANCH_LT: branch_taken = ($signed(operand_a) < $signed(operand_b));
+                // 3'b101: BGE (Branch if Greater Than or Equal, Signed)
+                BRANCH_GE: branch_taken = ($signed(operand_a) >= $signed(operand_b));
+                // 3'b110: BLTU (Branch if Less Than, Unsigned)
+                BRANCH_LTU: branch_taken = (operand_a < operand_b);
+                // 3'b111: BGEU (Branch if Greater Than or Equal, Unsigned)
+                BRANCH_GEU: branch_taken = (operand_a >= operand_b);
+                default: branch_taken = 1'b0;
+            endcase
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/compressed_decoder.sv b/rv64cpu/rtl/core/execute/compressed_decoder.sv
new file mode 100644
index 0000000..ccbb043
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/compressed_decoder.sv
@@ -0,0 +1,234 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+// File path: rtl/core/compressed_decoder.sv
+// Description: RV64C/Zca decompressor. Produces an equivalent 32-bit instruction
+//              for the existing RV64 pipeline and flags reserved encodings.
+
+module compressed_decoder
+    import rv64_pkg::*;
+(
+    input  logic [15:0] compressed_instr,
+    output logic [31:0] expanded_instr,
+    output logic        illegal_instr
+);
+
+    logic [1:0] quadrant;
+    logic [2:0] funct3;
+    logic [4:0] rd_rs1;
+    logic [4:0] rs2;
+    logic [4:0] rd_prime;
+    logic [4:0] rs1_prime;
+    logic [4:0] rs2_prime;
+
+    assign quadrant  = compressed_instr[1:0];
+    assign funct3    = compressed_instr[15:13];
+    assign rd_rs1    = compressed_instr[11:7];
+    assign rs2       = compressed_instr[6:2];
+    assign rd_prime  = {2'b01, compressed_instr[4:2]};
+    assign rs1_prime = {2'b01, compressed_instr[9:7]};
+    assign rs2_prime = {2'b01, compressed_instr[4:2]};
+
+    function automatic logic [31:0] r_instr(
+        input logic [6:0] funct7,
+        input logic [4:0] rs2_i,
+        input logic [4:0] rs1_i,
+        input logic [2:0] funct3_i,
+        input logic [4:0] rd_i,
+        input logic [6:0] opcode_i
+    );
+        return {funct7, rs2_i, rs1_i, funct3_i, rd_i, opcode_i};
+    endfunction
+
+    function automatic logic [31:0] i_instr(
+        input logic [11:0] imm_i,
+        input logic [4:0]  rs1_i,
+        input logic [2:0]  funct3_i,
+        input logic [4:0]  rd_i,
+        input logic [6:0]  opcode_i
+    );
+        return {imm_i, rs1_i, funct3_i, rd_i, opcode_i};
+    endfunction
+
+    function automatic logic [31:0] s_instr(
+        input logic [11:0] imm_i,
+        input logic [4:0]  rs2_i,
+        input logic [4:0]  rs1_i,
+        input logic [2:0]  funct3_i,
+        input logic [6:0]  opcode_i
+    );
+        return {imm_i[11:5], rs2_i, rs1_i, funct3_i, imm_i[4:0], opcode_i};
+    endfunction
+
+    function automatic logic [31:0] b_instr(
+        input logic [12:0] imm_i,
+        input logic [4:0]  rs2_i,
+        input logic [4:0]  rs1_i,
+        input logic [2:0]  funct3_i
+    );
+        return {imm_i[12], imm_i[10:5], rs2_i, rs1_i, funct3_i,
+                imm_i[4:1], imm_i[11], OPCODE_BRANCH};
+    endfunction
+
+    function automatic logic [31:0] u_instr(
+        input logic [19:0] imm_i,
+        input logic [4:0]  rd_i,
+        input logic [6:0]  opcode_i
+    );
+        return {imm_i, rd_i, opcode_i};
+    endfunction
+
+    function automatic logic [31:0] j_instr(
+        input logic [20:0] imm_i,
+        input logic [4:0]  rd_i
+    );
+        return {imm_i[20], imm_i[10:1], imm_i[11], imm_i[19:12], rd_i, OPCODE_JAL};
+    endfunction
+
+    always_comb begin
+        logic [11:0] ci_imm;
+        logic [11:0] addi4spn_imm;
+        logic [11:0] lw_imm;
+        logic [11:0] ld_imm;
+        logic [11:0] lwsp_imm;
+        logic [11:0] ldsp_imm;
+        logic [11:0] swsp_imm;
+        logic [11:0] sdsp_imm;
+        logic [12:0] branch_imm;
+        logic [20:0] jump_imm;
+        logic [5:0]  shamt;
+        logic [19:0] lui_imm;
+
+        ci_imm       = {{6{compressed_instr[12]}}, compressed_instr[12], compressed_instr[6:2]};
+        addi4spn_imm = {2'b00, compressed_instr[10:7], compressed_instr[12:11],
+                        compressed_instr[5], compressed_instr[6], 2'b00};
+        lw_imm       = {5'b00000, compressed_instr[5], compressed_instr[12:10],
+                        compressed_instr[6], 2'b00};
+        ld_imm       = {4'b0000, compressed_instr[6:5], compressed_instr[12:10], 3'b000};
+        lwsp_imm     = {4'b0000, compressed_instr[3:2], compressed_instr[12],
+                        compressed_instr[6:4], 2'b00};
+        ldsp_imm     = {3'b000, compressed_instr[4:2], compressed_instr[12],
+                        compressed_instr[6:5], 3'b000};
+        swsp_imm     = {4'b0000, compressed_instr[8:7], compressed_instr[12:9], 2'b00};
+        sdsp_imm     = {3'b000, compressed_instr[9:7], compressed_instr[12:10], 3'b000};
+        branch_imm   = {{4{compressed_instr[12]}}, compressed_instr[12],
+                        compressed_instr[6:5], compressed_instr[2],
+                        compressed_instr[11:10], compressed_instr[4:3], 1'b0};
+        jump_imm     = {{9{compressed_instr[12]}}, compressed_instr[12],
+                        compressed_instr[8], compressed_instr[10:9], compressed_instr[6],
+                        compressed_instr[7], compressed_instr[2], compressed_instr[11],
+                        compressed_instr[5:3], 1'b0};
+        shamt        = {compressed_instr[12], compressed_instr[6:2]};
+        lui_imm      = {{14{compressed_instr[12]}}, compressed_instr[12], compressed_instr[6:2]};
+
+        expanded_instr = 32'h0000_0013;
+        illegal_instr  = 1'b0;
+
+        unique case (quadrant)
+            2'b00: begin
+                unique case (funct3)
+                    3'b000: begin // C.ADDI4SPN
+                        expanded_instr = i_instr(addi4spn_imm, 5'd2, 3'b000, rd_prime, OPCODE_OP_IMM);
+                        illegal_instr  = (addi4spn_imm == 12'b0);
+                    end
+                    3'b001: expanded_instr = i_instr(ld_imm, rs1_prime, 3'b011, rd_prime, OPCODE_LOAD_FP);
+                    3'b010: expanded_instr = i_instr(lw_imm, rs1_prime, 3'b010, rd_prime, OPCODE_LOAD);
+                    3'b011: expanded_instr = i_instr(ld_imm, rs1_prime, 3'b011, rd_prime, OPCODE_LOAD);
+                    3'b101: expanded_instr = s_instr(ld_imm, rs2_prime, rs1_prime, 3'b011, OPCODE_STORE_FP);
+                    3'b110: expanded_instr = s_instr(lw_imm, rs2_prime, rs1_prime, 3'b010, OPCODE_STORE);
+                    3'b111: expanded_instr = s_instr(ld_imm, rs2_prime, rs1_prime, 3'b011, OPCODE_STORE);
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            2'b01: begin
+                unique case (funct3)
+                    3'b000: expanded_instr = i_instr(ci_imm, rd_rs1, 3'b000, rd_rs1, OPCODE_OP_IMM);
+                    3'b001: expanded_instr = i_instr(ci_imm, rd_rs1, 3'b000, rd_rs1, OPCODE_OP_IMM_32);
+                    3'b010: expanded_instr = i_instr(ci_imm, 5'd0, 3'b000, rd_rs1, OPCODE_OP_IMM);
+                    3'b011: begin
+                        if (rd_rs1 == 5'd2) begin
+                            expanded_instr = i_instr({{2{compressed_instr[12]}}, compressed_instr[12],
+                                                       compressed_instr[4:3], compressed_instr[5],
+                                                       compressed_instr[2], compressed_instr[6], 4'b0000},
+                                                      5'd2, 3'b000, 5'd2, OPCODE_OP_IMM);
+                            illegal_instr = (compressed_instr[12:2] == 11'b0);
+                        end else begin
+                            expanded_instr = u_instr(lui_imm, rd_rs1, OPCODE_LUI);
+                            illegal_instr = (rd_rs1 == 5'd0) || (lui_imm == 20'b0);
+                        end
+                    end
+                    3'b100: begin
+                        unique case (compressed_instr[11:10])
+                            2'b00: begin
+                                expanded_instr = i_instr({6'b000000, shamt}, rs1_prime, 3'b101, rs1_prime, OPCODE_OP_IMM);
+                                illegal_instr = (shamt == 6'b0);
+                            end
+                            2'b01: begin
+                                expanded_instr = i_instr({6'b010000, shamt}, rs1_prime, 3'b101, rs1_prime, OPCODE_OP_IMM);
+                                illegal_instr = (shamt == 6'b0);
+                            end
+                            2'b10: expanded_instr = i_instr(ci_imm, rs1_prime, 3'b111, rs1_prime, OPCODE_OP_IMM);
+                            2'b11: begin
+                                unique case ({compressed_instr[12], compressed_instr[6:5]})
+                                    3'b000: expanded_instr = r_instr(7'b0100000, rs2_prime, rs1_prime, 3'b000, rs1_prime, OPCODE_OP);
+                                    3'b001: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b100, rs1_prime, OPCODE_OP);
+                                    3'b010: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b110, rs1_prime, OPCODE_OP);
+                                    3'b011: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b111, rs1_prime, OPCODE_OP);
+                                    3'b100: expanded_instr = r_instr(7'b0100000, rs2_prime, rs1_prime, 3'b000, rs1_prime, OPCODE_OP_32);
+                                    3'b101: expanded_instr = r_instr(7'b0000000, rs2_prime, rs1_prime, 3'b000, rs1_prime, OPCODE_OP_32);
+                                    default: illegal_instr = 1'b1;
+                                endcase
+                            end
+                        endcase
+                    end
+                    3'b101: expanded_instr = j_instr(jump_imm, 5'd0);
+                    3'b110: expanded_instr = b_instr(branch_imm, 5'd0, rs1_prime, 3'b000);
+                    3'b111: expanded_instr = b_instr(branch_imm, 5'd0, rs1_prime, 3'b001);
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            2'b10: begin
+                unique case (funct3)
+                    3'b000: begin
+                        expanded_instr = i_instr({6'b000000, shamt}, rd_rs1, 3'b001, rd_rs1, OPCODE_OP_IMM);
+                        illegal_instr = (shamt == 6'b0);
+                    end
+                    3'b001: begin
+                        expanded_instr = i_instr(ldsp_imm, 5'd2, 3'b011, rd_rs1, OPCODE_LOAD_FP);
+                    end
+                    3'b010: begin
+                        expanded_instr = i_instr(lwsp_imm, 5'd2, 3'b010, rd_rs1, OPCODE_LOAD);
+                        illegal_instr = (rd_rs1 == 5'd0);
+                    end
+                    3'b011: begin
+                        expanded_instr = i_instr(ldsp_imm, 5'd2, 3'b011, rd_rs1, OPCODE_LOAD);
+                        illegal_instr = (rd_rs1 == 5'd0);
+                    end
+                    3'b100: begin
+                        if (!compressed_instr[12] && (rs2 == 5'd0)) begin
+                            expanded_instr = i_instr(12'b0, rd_rs1, 3'b000, 5'd0, OPCODE_JALR);
+                            illegal_instr = (rd_rs1 == 5'd0);
+                        end else if (!compressed_instr[12]) begin
+                            expanded_instr = r_instr(7'b0000000, rs2, 5'd0, 3'b000, rd_rs1, OPCODE_OP);
+                        end else if (compressed_instr[12] && (rd_rs1 == 5'd0) && (rs2 == 5'd0)) begin
+                            expanded_instr = 32'h0010_0073; // EBREAK
+                        end else if (compressed_instr[12] && (rs2 == 5'd0)) begin
+                            expanded_instr = i_instr(12'b0, rd_rs1, 3'b000, 5'd1, OPCODE_JALR);
+                        end else begin
+                            expanded_instr = r_instr(7'b0000000, rs2, rd_rs1, 3'b000, rd_rs1, OPCODE_OP);
+                        end
+                    end
+                    3'b101: expanded_instr = s_instr(sdsp_imm, rs2, 5'd2, 3'b011, OPCODE_STORE_FP);
+                    3'b110: expanded_instr = s_instr(swsp_imm, rs2, 5'd2, 3'b010, OPCODE_STORE);
+                    3'b111: expanded_instr = s_instr(sdsp_imm, rs2, 5'd2, 3'b011, OPCODE_STORE);
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            default: illegal_instr = 1'b1;
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/hazard_unit.sv b/rv64cpu/rtl/core/execute/hazard_unit.sv
new file mode 100644
index 0000000..ee90356
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/hazard_unit.sv
@@ -0,0 +1,200 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/hazard_unit.sv
+// Description: Hazard Control Unit
+// Handles data hazards (forwarding, Load-Use stalls), control hazards
+// (branch misprediction flushes), and memory stalls (D-Cache misses).
+
+module hazard_unit
+    import rv64_pkg::*;
+(
+    // -----------------------
+    // Inputs from ID Stage (for Load-Use detection)
+    // -----------------------
+    input  logic [4:0]  id_rs1_addr,
+    input  logic [4:0]  id_rs2_addr,
+
+    // -----------------------
+    // Inputs from EX Stage (ID/EX register outputs)
+    // -----------------------
+    input  logic [4:0]  ex_rs1_addr,
+    input  logic [4:0]  ex_rs2_addr,
+    input  logic [4:0]  ex_rd_addr,
+    input  logic        ex_reg_write_en,
+    input  logic        ex_is_load,
+    input  logic        ex_is_csr,
+    input  logic        ex_branch_taken,
+    input  logic [XLEN-1:0] ex_branch_target,
+
+    // -----------------------
+    // Inputs from MEM Stage (EX/MEM register outputs)
+    // -----------------------
+    input  logic [4:0]  mem_rd_addr,
+    input  logic        mem_reg_write_en,
+    input  logic        mem_result_late,
+    input  logic        mem_is_csr,
+    input  logic        mem_stall_req,       // D-Cache miss or memory busy
+
+    // -----------------------
+    // Trap Controller Inputs
+    // -----------------------
+    input  logic        trap_flush_req,
+    input  logic        trap_pc_sel,
+    input  logic [XLEN-1:0] trap_target_pc,
+
+    // -----------------------
+    // Inputs from WB Stage (MEM/WB register outputs)
+    // -----------------------
+    input  logic [4:0]  wb_rd_addr,
+    input  logic        wb_reg_write_en,
+
+    // -----------------------
+    // Outputs: EX-stage operand forwarding mux selects
+    // -----------------------
+    // 00: no forward (use regfile), 01: forward from MEM, 10: forward from WB
+    output logic [1:0]  forward_a_sel,
+    output logic [1:0]  forward_b_sel,
+
+    // -----------------------
+    // Outputs: Pipeline stall and flush controls
+    // -----------------------
+    output logic        if_stall_en,
+    output logic        if_id_stall_en,
+    output logic        id_ex_stall_en,
+    output logic        ex_mem_stall_en,
+    output logic        if_id_flush_en,
+    output logic        id_ex_flush_en,
+
+    // -----------------------
+    // Outputs: IF-stage PC redirect controls
+    // -----------------------
+    output logic        pc_redirect_sel,
+    output logic [XLEN-1:0] pc_redirect_target,
+    output logic        pc_redirect_is_trap
+);
+
+    // -----------------------
+    // Data Forwarding (EX-stage operand bypass)
+    // -----------------------
+    // Forwarding resolves RAW hazards by routing results from
+    // MEM or WB back to the EX ALU inputs. We compare EX-stage
+    // source register addresses (the instruction currently in EX)
+    // against the destination registers of instructions in MEM/WB.
+    //
+    // Priority: MEM > WB (MEM result is more recent).
+
+    // Operand A (ex_rs1_addr)
+    always_comb begin
+        if (mem_reg_write_en && !mem_result_late && (mem_rd_addr != 5'b0) && (mem_rd_addr == ex_rs1_addr)) begin
+            forward_a_sel = 2'b01; // Forward from MEM stage
+        end else if (wb_reg_write_en && (wb_rd_addr != 5'b0) && (wb_rd_addr == ex_rs1_addr)) begin
+            forward_a_sel = 2'b10; // Forward from WB stage
+        end else begin
+            forward_a_sel = 2'b00; // No forwarding
+        end
+    end
+
+    // Operand B (ex_rs2_addr)
+    always_comb begin
+        if (mem_reg_write_en && !mem_result_late && (mem_rd_addr != 5'b0) && (mem_rd_addr == ex_rs2_addr)) begin
+            forward_b_sel = 2'b01; // Forward from MEM stage
+        end else if (wb_reg_write_en && (wb_rd_addr != 5'b0) && (wb_rd_addr == ex_rs2_addr)) begin
+            forward_b_sel = 2'b10; // Forward from WB stage
+        end else begin
+            forward_b_sel = 2'b00; // No forwarding
+        end
+    end
+
+    // -----------------------
+    // Load-Use Hazard Detection
+    // -----------------------
+    // If the instruction in EX is a load, and the instruction in
+    // ID reads from the load's destination, we must stall for 1
+    // cycle because the load data is not available until MEM.
+    logic load_use_stall;
+    assign load_use_stall = ex_is_load && (ex_rd_addr != 5'b0) &&
+                            ((ex_rd_addr == id_rs1_addr) || (ex_rd_addr == id_rs2_addr));
+
+    logic csr_use_stall;
+    assign csr_use_stall = ((ex_is_csr && ex_reg_write_en && (ex_rd_addr != 5'b0) &&
+                             ((ex_rd_addr == id_rs1_addr) || (ex_rd_addr == id_rs2_addr))) ||
+                            (mem_is_csr && mem_reg_write_en && (mem_rd_addr != 5'b0) &&
+                             ((mem_rd_addr == id_rs1_addr) || (mem_rd_addr == id_rs2_addr))));
+
+    // -----------------------
+    // PC Redirect Selection (Trap has top priority)
+    // -----------------------
+    always_comb begin
+        pc_redirect_sel      = 1'b0;
+        pc_redirect_target   = '0;
+        pc_redirect_is_trap  = 1'b0;
+
+        if (trap_pc_sel) begin
+            pc_redirect_sel     = 1'b1;
+            pc_redirect_target  = trap_target_pc;
+            pc_redirect_is_trap = 1'b1;
+        end else if (ex_branch_taken) begin
+            pc_redirect_sel     = 1'b1;
+            pc_redirect_target  = ex_branch_target;
+        end
+    end
+
+    // -----------------------
+    // Stall and Flush Signal Generation
+    // -----------------------
+    always_comb begin
+        // Defaults: no stalls, no flushes
+        if_stall_en     = 1'b0;
+        if_id_stall_en  = 1'b0;
+        id_ex_stall_en  = 1'b0;
+        ex_mem_stall_en = 1'b0;
+        if_id_flush_en  = 1'b0;
+        id_ex_flush_en  = 1'b0;
+
+        if (trap_flush_req) begin
+            if_id_flush_en = 1'b1;
+            id_ex_flush_en = 1'b1;
+        end else begin
+            // (1) Memory stall (D-Cache miss): freeze the entire pipeline
+            if (mem_stall_req) begin
+                if_stall_en     = 1'b1;
+                if_id_stall_en  = 1'b1;
+                id_ex_stall_en  = 1'b1;
+                ex_mem_stall_en = 1'b1;
+            end
+
+            // (2) Value-use stall: freeze IF and IF/ID, insert bubble into ID/EX.
+            // Loads and CSR reads both produce their register result after EX.
+            if ((load_use_stall || csr_use_stall) && !mem_stall_req) begin
+                if_stall_en    = 1'b1;
+                if_id_stall_en = 1'b1;
+                id_ex_flush_en = 1'b1;
+            end
+
+            // (3) Branch/jump misprediction: flush IF/ID and ID/EX
+            // Branch flush overrides load-use stall (the stalled instruction is invalid anyway)
+            if (ex_branch_taken) begin
+                if_id_flush_en = 1'b1;
+                id_ex_flush_en = 1'b1;
+                // Cancel load-use stall if branch is also taken
+                if_stall_en    = 1'b0;
+                if_id_stall_en = 1'b0;
+            end
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/instruction_decoder.sv b/rv64cpu/rtl/core/execute/instruction_decoder.sv
new file mode 100644
index 0000000..14d9559
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/instruction_decoder.sv
@@ -0,0 +1,672 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/decoder.sv
+// Description: Instruction decoder for RV64IMAFD base ISA + Zicsr + Zifencei
+//              Translates 32-bit instructions into control signals for the pipeline.
+//              C is handled by the IF-stage decompressor before this decoder.
+
+module instruction_decoder
+    import rv64_pkg::*;
+(
+    input  logic [31:0]       instr,          // 32-bit instruction
+    input  logic [1:0]        priv_mode,      // Current privilege mode from CSR
+    input  logic              mstatus_tsr,    // Trap SRET in S-mode
+    input  logic              mstatus_tw,     // Trap WFI in S/U-mode
+    input  logic              mstatus_tvm,    // Trap supervisor memory-management ops in S-mode
+
+    // Register File
+    output logic [4:0]        rs1_addr,       // Source register 1 address
+    output logic [4:0]        rs2_addr,       // Source register 2 address
+    output logic [4:0]        rd_addr,        // Destination register address
+    output logic              reg_write_en,      // Register file write enable
+
+    // ALU Control
+    output alu_op_t           alu_op,         // ALU operation select
+    output logic              alu_src1_sel,   // 0 = rs1,  1 = PC
+    output logic              alu_src2_sel,   // 0 = rs2,  1 = immediate
+
+    // Immediate
+    output logic [XLEN-1:0]   imm,            // Sign-extended immediate
+
+    // Memory Access
+    output logic              is_load,        // MEM stage: read from memory
+    output logic              is_store,       // MEM stage: write to memory
+    output logic [2:0]        mem_size,       // funct3 → byte/half/word/double + sign
+    output logic              is_amo,         // A extension atomic memory operation
+    output amo_op_t           amo_op,         // LR/SC/AMO operation selector
+    output logic              is_fp,
+    output logic              fp_reg_write_en,
+    output logic              fp_store_data_sel,
+    output fp_op_t            fp_op,
+    output fp_fmt_t           fp_fmt,
+    output logic [2:0]        fp_rm,
+
+    // Branch / Jump
+    output logic              is_branch,      // EX: conditional branch
+    output logic              is_jump,        // EX: unconditional jump (JAL/JALR)
+    output logic [2:0]        branch_op,      // funct3 → BEQ/BNE/BLT/BGE/BLTU/BGEU
+
+    // Writeback Source
+    output logic [1:0]        wb_sel,         // 0 = ALU result, 1 = MEM data, 2 = PC+4
+
+    // RV64 Word Operations
+    output logic              is_word_op,     // EX: operate on lower 32 bits, sext result
+
+    // System / Privilege
+    output logic              is_csr,         // CSR instruction
+    output logic [2:0]        csr_op,         // funct3 → CSRRW/CSRRS/CSRRC/CSRRWI/CSRRSI/CSRRCI
+    output logic              is_ecall,       // ECALL
+    output logic              is_ebreak,      // EBREAK
+    output logic              is_mret,        // MRET
+    output logic              is_sret,        // SRET
+    output logic              is_wfi,         // WFI
+    output logic              is_sfence_vma,  // SFENCE.VMA
+    output logic              is_fence,       // FENCE
+    output logic              is_fence_i,     // FENCE.I (Zifencei)
+
+    // Error
+    output logic              illegal_instr   // Unrecognised or malformed instruction
+);
+
+    // ============================
+    // Instruction Field Extraction
+    // ============================
+    typedef enum logic [1:0] {
+        PRIV_MODE_U = 2'b00,
+        PRIV_MODE_S = 2'b01,
+        PRIV_MODE_M = 2'b11
+    } priv_mode_t;
+
+    logic [6:0] opcode;
+    logic [2:0] funct3;
+    logic [6:0] funct7;
+    logic [4:0] amo_funct5;
+
+    assign opcode   = instr[6:0];
+    assign rd_addr  = instr[11:7];
+    assign funct3   = instr[14:12];
+    assign rs1_addr = (opcode == OPCODE_LUI) ? 5'b00000 : instr[19:15];
+    assign rs2_addr = instr[24:20];
+    assign funct7   = instr[31:25];
+    assign amo_funct5 = instr[31:27];
+
+    // ============================
+    // Immediate Generation
+    // ============================
+    logic [XLEN-1:0] imm_i, imm_s, imm_b, imm_u, imm_j;
+    logic [11:0]     csr_addr_field;
+
+    // I-type:  instr[31:20]
+    assign imm_i = {{52{instr[31]}}, instr[31:20]};
+
+    // S-type:  {instr[31:25], instr[11:7]}
+    assign imm_s = {{52{instr[31]}}, instr[31:25], instr[11:7]};
+
+    // B-type:  {instr[31], instr[7], instr[30:25], instr[11:8], 1'b0}
+    assign imm_b = {{51{instr[31]}}, instr[31], instr[7], instr[30:25], instr[11:8], 1'b0};
+
+    // U-type:  {instr[31:12], 12'b0}
+    assign imm_u = {{32{instr[31]}}, instr[31:12], 12'b0};
+
+    // J-type:  {instr[31], instr[19:12], instr[20], instr[30:21], 1'b0}
+    assign imm_j = {{43{instr[31]}}, instr[31], instr[19:12], instr[20], instr[30:21], 1'b0};
+    assign csr_addr_field = instr[31:20];
+
+    function automatic int priv_rank(logic [1:0] mode);
+        case (mode)
+            PRIV_MODE_U: return 0;
+            PRIV_MODE_S: return 1;
+            2'b10:       return 2; // Reserved/H-mode, treat as higher than S
+            default:     return 3; // Machine mode (or anything else) highest
+        endcase
+    endfunction
+
+    function automatic logic priv_ge(logic [1:0] cur, logic [1:0] req);
+        return priv_rank(cur) >= priv_rank(req);
+    endfunction
+
+    function automatic logic csr_access_allowed(logic [1:0] cur, logic [11:0] addr);
+        logic [1:0] req_priv;
+        req_priv = addr[9:8];
+        return priv_ge(cur, req_priv);
+    endfunction
+
+    // ============================
+    // Main Decode Logic
+    // ============================
+    always_comb begin
+        // Default values (all signals inactive)
+        alu_op        = ALU_ADD;
+        alu_src1_sel  = 1'b0;   // rs1
+        alu_src2_sel  = 1'b0;   // rs2
+        reg_write_en  = 1'b0;
+        imm           = '0;
+        is_load       = 1'b0;
+        is_store      = 1'b0;
+        mem_size      = 3'b0;
+        is_amo        = 1'b0;
+        amo_op        = AMO_NONE;
+        is_fp         = 1'b0;
+        fp_reg_write_en  = 1'b0;
+        fp_store_data_sel = 1'b0;
+        fp_op         = FP_OP_NONE;
+        fp_fmt        = FP_FMT_S;
+        fp_rm         = funct3;
+        is_branch     = 1'b0;
+        is_jump       = 1'b0;
+        branch_op     = 3'b0;
+        wb_sel        = 2'b00;  // ALU result
+        is_word_op    = 1'b0;
+        is_csr        = 1'b0;
+        csr_op        = 3'b0;
+        is_ecall      = 1'b0;
+        is_ebreak     = 1'b0;
+        is_mret       = 1'b0;
+        is_sret       = 1'b0;
+        is_wfi        = 1'b0;
+        is_sfence_vma = 1'b0;
+        is_fence      = 1'b0;
+        is_fence_i    = 1'b0;
+        illegal_instr = 1'b0;
+
+        case (opcode)
+
+            // LUI — rd = imm_u
+
+            OPCODE_LUI: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b0;    // doesn't matter,  ALU_ADD(0, imm_u)
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_u;
+                alu_op       = ALU_ADD; // result = 0 + imm_u  (rs1 forced to x0 externally or use alu_src1_sel trick)
+                // NOTE: EX stage should feed 0 to ALU operand 1 when alu_src1_sel==0 and opcode==LUI.
+                // Alternative: add a dedicated alu_src1_sel=2 for "zero". For now, the pipeline
+                // can detect LUI by opcode and zero out the operand.
+            end
+
+            // AUIPC — rd = PC + imm_u
+
+
+            OPCODE_AUIPC: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b1;    // PC
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_u;
+                alu_op       = ALU_ADD; // result = PC + imm_u
+            end
+
+            // -----------------
+            // JAL — rd = PC+4;  PC = PC + imm_j
+            OPCODE_JAL: begin
+                reg_write_en = 1'b1;
+                is_jump      = 1'b1;
+                alu_src1_sel = 1'b1;    // PC
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_j;
+                alu_op       = ALU_ADD; // target = PC + imm_j
+                wb_sel       = 2'b10;   // writeback PC+4
+            end
+
+            // ------------------
+            // JALR — rd = PC+4;  PC = (rs1 + imm_i) & ~1
+
+            OPCODE_JALR: begin
+                reg_write_en = 1'b1;
+                is_jump      = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+                alu_op       = ALU_ADD; // target = rs1 + imm_i  (LSB cleared by EX stage)
+                wb_sel       = 2'b10;   // writeback PC+4
+            end
+
+            // -----------------
+            // Branch — compare rs1, rs2;  if taken, PC = PC + imm_b
+            OPCODE_BRANCH: begin
+                reg_write_en = 1'b0;    // branches never write rd
+                is_branch    = 1'b1;
+                alu_src1_sel = 1'b1;    // PC  (ALU computes branch target)
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_b;
+                alu_op       = ALU_ADD; // branch target = PC + imm_b
+                branch_op    = funct3;  // BEQ/BNE/BLT/BGE/BLTU/BGEU
+            end
+
+            // -----------------
+            // Load — rd = Mem[rs1 + imm_i]
+            OPCODE_LOAD: begin
+                reg_write_en = 1'b1;
+                is_load      = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+                alu_op       = ALU_ADD; // effective address = rs1 + imm_i
+                mem_size     = funct3;  // LB/LH/LW/LD/LBU/LHU/LWU
+                wb_sel       = 2'b01;   // writeback from memory
+            end
+
+            // -----------------
+            // Store — Mem[rs1 + imm_s] = rs2
+            OPCODE_STORE: begin
+                reg_write_en = 1'b0;
+                is_store     = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate  (NOT rs2 — rs2 is store data)
+                imm          = imm_s;
+                alu_op       = ALU_ADD; // effective address = rs1 + imm_s
+                mem_size     = funct3;  // SB/SH/SW/SD
+            end
+
+            // -----------------
+            // OP-IMM (I-type ALU) — rd = rs1 OP imm
+            // ADDI, SLTI, SLTIU, XORI, ORI, ANDI, SLLI, SRLI, SRAI
+            OPCODE_OP_IMM: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+
+                case (funct3)
+                    3'b000:  alu_op = ALU_ADD;                              // ADDI
+                    3'b010:  alu_op = ALU_SLT;                              // SLTI
+                    3'b011:  alu_op = ALU_SLTU;                             // SLTIU
+                    3'b100:  alu_op = ALU_XOR;                              // XORI
+                    3'b110:  alu_op = ALU_OR;                               // ORI
+                    3'b111:  alu_op = ALU_AND;                              // ANDI
+                    3'b001:  alu_op = ALU_SLL;                              // SLLI
+                    3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;      // SRAI / SRLI
+                    default: alu_op = ALU_ADD;
+                endcase
+            end
+
+            // -----------------
+            // OP-IMM-32 (I-type ALU, 32-bit word variants for RV64)
+            // ADDIW, SLLIW, SRLIW, SRAIW
+            OPCODE_OP_IMM_32: begin
+                reg_write_en = 1'b1;
+                is_word_op   = 1'b1;    // EX stage: operate on lower 32 bits, sext result
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b1;    // immediate
+                imm          = imm_i;
+
+                case (funct3)
+                    3'b000:  alu_op = ALU_ADD;                              // ADDIW
+                    3'b001:  alu_op = ALU_SLL;                              // SLLIW
+                    3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;      // SRAIW / SRLIW
+                    default: begin
+                        alu_op = ALU_ADD;
+                        illegal_instr = 1'b1;
+                    end
+                endcase
+            end
+
+            // -----------------
+            // OP (R-type ALU) — rd = rs1 OP rs2
+            // ADD, SUB, SLL, SLT, SLTU, XOR, SRL, SRA, OR, AND
+            OPCODE_OP: begin
+                reg_write_en = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b0;    // rs2
+
+                if (funct7 == 7'b0000001) begin
+                    case (funct3)
+                        3'b000:  alu_op = ALU_MUL;
+                        3'b001:  alu_op = ALU_MULH;
+                        3'b010:  alu_op = ALU_MULHSU;
+                        3'b011:  alu_op = ALU_MULHU;
+                        3'b100:  alu_op = ALU_DIV;
+                        3'b101:  alu_op = ALU_DIVU;
+                        3'b110:  alu_op = ALU_REM;
+                        3'b111:  alu_op = ALU_REMU;
+                        default: illegal_instr = 1'b1;
+                    endcase
+                end else begin
+                    case (funct3)
+                        3'b000:  alu_op = (funct7[5]) ? ALU_SUB : ALU_ADD;  // SUB / ADD
+                        3'b001:  alu_op = ALU_SLL;                          // SLL
+                        3'b010:  alu_op = ALU_SLT;                          // SLT
+                        3'b011:  alu_op = ALU_SLTU;                         // SLTU
+                        3'b100:  alu_op = ALU_XOR;                          // XOR
+                        3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;  // SRA / SRL
+                        3'b110:  alu_op = ALU_OR;                           // OR
+                        3'b111:  alu_op = ALU_AND;                          // AND
+                        default: alu_op = ALU_ADD;
+                    endcase
+                end
+            end
+
+            // -----------------
+            // OP-32 (R-type ALU, 32-bit word variants for RV64)
+            // ADDW, SUBW, SLLW, SRLW, SRAW
+            OPCODE_OP_32: begin
+                reg_write_en = 1'b1;
+                is_word_op   = 1'b1;
+                alu_src1_sel = 1'b0;    // rs1
+                alu_src2_sel = 1'b0;    // rs2
+
+                if (funct7 == 7'b0000001) begin
+                    case (funct3)
+                        3'b000:  alu_op = ALU_MUL;
+                        3'b100:  alu_op = ALU_DIV;
+                        3'b101:  alu_op = ALU_DIVU;
+                        3'b110:  alu_op = ALU_REM;
+                        3'b111:  alu_op = ALU_REMU;
+                        default: illegal_instr = 1'b1;
+                    endcase
+                end else begin
+                    case (funct3)
+                        3'b000:  alu_op = (funct7[5]) ? ALU_SUB : ALU_ADD;  // SUBW / ADDW
+                        3'b001:  alu_op = ALU_SLL;                          // SLLW
+                        3'b101:  alu_op = (funct7[5]) ? ALU_SRA : ALU_SRL;  // SRAW / SRLW
+                        default: begin
+                            alu_op = ALU_ADD;
+                            illegal_instr = 1'b1;
+                        end
+                    endcase
+                end
+            end
+
+            // -----------------
+            // MISC-MEM — FENCE / FENCE.I
+            OPCODE_MISC_MEM: begin
+                case (funct3)
+                    3'b000:  is_fence   = 1'b1;  // FENCE  (NOP in single-core in-order)
+                    3'b001:  is_fence_i = 1'b1;  // FENCE.I — flush I-Cache
+                    default: illegal_instr = 1'b1;
+                endcase
+            end
+
+            // -----------------
+            // SYSTEM — ECALL / EBREAK / xRET / WFI / SFENCE.VMA / CSR*
+            OPCODE_SYSTEM: begin
+                if (funct3 == 3'b000) begin
+                    // Privileged instructions (non-CSR)
+                    reg_write_en = 1'b0;
+                    case ({funct7, instr[24:20]})    // {funct7, rs2}
+                        12'b0000000_00000: is_ecall      = 1'b1;   // ECALL
+                        12'b0000000_00001: is_ebreak     = 1'b1;   // EBREAK
+                        12'b0011000_00010: begin                  // MRET
+                            if (priv_mode == PRIV_MODE_M)
+                                is_mret = 1'b1;
+                            else
+                                illegal_instr = 1'b1;
+                        end
+                        12'b0001000_00010: begin                  // SRET
+                            if (!priv_ge(priv_mode, PRIV_MODE_S))
+                                illegal_instr = 1'b1;
+                            else if ((priv_mode == PRIV_MODE_S) && mstatus_tsr)
+                                illegal_instr = 1'b1;
+                            else
+                                is_sret = 1'b1;
+                        end
+                        12'b0001000_00101: begin                  // WFI
+                            if ((priv_mode != PRIV_MODE_M) && mstatus_tw)
+                                illegal_instr = 1'b1;
+                            else
+                                is_wfi = 1'b1;
+                        end
+                        default: begin
+                            if (funct7 == 7'b0001001) begin
+                                if (!priv_ge(priv_mode, PRIV_MODE_S))
+                                    illegal_instr = 1'b1;
+                                else if ((priv_mode == PRIV_MODE_S) && mstatus_tvm)
+                                    illegal_instr = 1'b1;
+                                else
+                                    is_sfence_vma = 1'b1;           // SFENCE.VMA
+                            end else begin
+                                illegal_instr = 1'b1;
+                            end
+                        end
+                    endcase
+                end else begin
+                    // CSR instructions
+                    // funct3: 001=CSRRW, 010=CSRRS, 011=CSRRC,
+                    //         101=CSRRWI, 110=CSRRSI, 111=CSRRCI
+                    reg_write_en = 1'b1;
+                    is_csr       = 1'b1;
+                    csr_op       = funct3;
+                    imm          = imm_i;   // CSR address = instr[31:20] (reuse imm_i path)
+                    if (!csr_access_allowed(priv_mode, csr_addr_field)) begin
+                        reg_write_en  = 1'b0;
+                        is_csr        = 1'b0;
+                        illegal_instr = 1'b1;
+                    end
+                end
+            end
+
+            // -----------------
+            // AMO — A extension (LR/SC/AMO*)
+            OPCODE_AMO: begin
+                reg_write_en = 1'b1;
+                is_amo       = 1'b1;
+                alu_src1_sel = 1'b0;
+                alu_src2_sel = 1'b1;
+                imm          = '0;
+                alu_op       = ALU_ADD;
+                wb_sel       = 2'b01;
+
+                unique case (funct3)
+                    3'b010:  mem_size = 3'b010; // AMO*.W
+                    3'b011:  mem_size = 3'b011; // AMO*.D
+                    default: illegal_instr = 1'b1;
+                endcase
+
+                unique case (amo_funct5)
+                    5'b00010: begin
+                        amo_op = AMO_LR;
+                        if (rs2_addr != 5'd0) begin
+                            illegal_instr = 1'b1;
+                        end
+                    end
+                    5'b00011: amo_op = AMO_SC;
+                    5'b00001: amo_op = AMO_SWAP;
+                    5'b00000: amo_op = AMO_ADD;
+                    5'b00100: amo_op = AMO_XOR;
+                    5'b01100: amo_op = AMO_AND;
+                    5'b01000: amo_op = AMO_OR;
+                    5'b10000: amo_op = AMO_MIN;
+                    5'b10100: amo_op = AMO_MAX;
+                    5'b11000: amo_op = AMO_MINU;
+                    5'b11100: amo_op = AMO_MAXU;
+                    default: begin
+                        amo_op        = AMO_NONE;
+                        illegal_instr = 1'b1;
+                    end
+                endcase
+
+                if (illegal_instr) begin
+                    reg_write_en = 1'b0;
+                    is_amo       = 1'b0;
+                    amo_op       = AMO_NONE;
+                end
+            end
+
+            // -----------------
+            // Floating-Point — F/D extensions
+            OPCODE_LOAD_FP: begin
+                is_fp           = 1'b1;
+                is_load         = 1'b1;
+                fp_reg_write_en = 1'b1;
+                alu_src1_sel    = 1'b0;
+                alu_src2_sel    = 1'b1;
+                imm             = imm_i;
+                alu_op          = ALU_ADD;
+                wb_sel          = 2'b01;
+                unique case (funct3)
+                    3'b010: begin
+                        mem_size = 3'b010; // FLW
+                        fp_fmt   = FP_FMT_S;
+                    end
+                    3'b011: begin
+                        mem_size = 3'b011; // FLD
+                        fp_fmt   = FP_FMT_D;
+                    end
+                    default: begin
+                        is_fp           = 1'b0;
+                        fp_reg_write_en = 1'b0;
+                        illegal_instr   = 1'b1;
+                    end
+                endcase
+            end
+
+            OPCODE_STORE_FP: begin
+                is_fp             = 1'b1;
+                is_store          = 1'b1;
+                fp_store_data_sel = 1'b1;
+                alu_src1_sel      = 1'b0;
+                alu_src2_sel      = 1'b1;
+                imm               = imm_s;
+                alu_op            = ALU_ADD;
+                unique case (funct3)
+                    3'b010: begin
+                        mem_size = 3'b010; // FSW
+                        fp_fmt   = FP_FMT_S;
+                    end
+                    3'b011: begin
+                        mem_size = 3'b011; // FSD
+                        fp_fmt   = FP_FMT_D;
+                    end
+                    default: begin
+                        is_fp             = 1'b0;
+                        fp_store_data_sel = 1'b0;
+                        illegal_instr     = 1'b1;
+                    end
+                endcase
+            end
+
+            OPCODE_MADD,
+            OPCODE_MSUB,
+            OPCODE_NMSUB,
+            OPCODE_NMADD: begin
+                is_fp           = 1'b1;
+                fp_reg_write_en = 1'b1;
+                fp_rm           = funct3;
+                unique case (instr[26:25])
+                    2'b00: fp_fmt = FP_FMT_S;
+                    2'b01: fp_fmt = FP_FMT_D;
+                    default: illegal_instr = 1'b1;
+                endcase
+                unique case (opcode)
+                    OPCODE_MADD:  fp_op = FP_OP_MADD;
+                    OPCODE_MSUB:  fp_op = FP_OP_MSUB;
+                    OPCODE_NMSUB: fp_op = FP_OP_NMSUB;
+                    default:      fp_op = FP_OP_NMADD;
+                endcase
+                if (illegal_instr) begin
+                    is_fp           = 1'b0;
+                    fp_reg_write_en = 1'b0;
+                    fp_op           = FP_OP_NONE;
+                end
+            end
+
+            OPCODE_OP_FP: begin
+                is_fp = 1'b1;
+                fp_rm = funct3;
+                unique case (instr[26:25])
+                    2'b00: fp_fmt = FP_FMT_S;
+                    2'b01: fp_fmt = FP_FMT_D;
+                    default: illegal_instr = 1'b1;
+                endcase
+
+                unique case (funct7)
+                    7'b0000000, 7'b0000001: begin fp_op = FP_OP_ADD; fp_reg_write_en = 1'b1; end
+                    7'b0000100, 7'b0000101: begin fp_op = FP_OP_SUB; fp_reg_write_en = 1'b1; end
+                    7'b0001000, 7'b0001001: begin fp_op = FP_OP_MUL; fp_reg_write_en = 1'b1; end
+                    7'b0001100, 7'b0001101: begin fp_op = FP_OP_DIV; fp_reg_write_en = 1'b1; end
+                    7'b0101100, 7'b0101101: begin
+                        fp_op = FP_OP_SQRT;
+                        fp_reg_write_en = 1'b1;
+                        if (rs2_addr != 5'd0) illegal_instr = 1'b1;
+                    end
+                    7'b0010000, 7'b0010001: begin
+                        fp_reg_write_en = 1'b1;
+                        unique case (funct3)
+                            3'b000: fp_op = FP_OP_SGNJ;
+                            3'b001: fp_op = FP_OP_SGNJN;
+                            3'b010: fp_op = FP_OP_SGNJX;
+                            default: illegal_instr = 1'b1;
+                        endcase
+                    end
+                    7'b0010100, 7'b0010101: begin
+                        fp_reg_write_en = 1'b1;
+                        unique case (funct3)
+                            3'b000: fp_op = FP_OP_MIN;
+                            3'b001: fp_op = FP_OP_MAX;
+                            default: illegal_instr = 1'b1;
+                        endcase
+                    end
+                    7'b0100000, 7'b0100001: begin
+                        fp_op = FP_OP_CVT_F2F;
+                        fp_reg_write_en = 1'b1;
+                    end
+                    7'b1010000, 7'b1010001: begin
+                        reg_write_en = 1'b1;
+                        unique case (funct3)
+                            3'b010: fp_op = FP_OP_EQ;
+                            3'b001: fp_op = FP_OP_LT;
+                            3'b000: fp_op = FP_OP_LE;
+                            default: illegal_instr = 1'b1;
+                        endcase
+                    end
+                    7'b1100000, 7'b1100001: begin
+                        fp_op = FP_OP_CVT_F2I;
+                        reg_write_en = 1'b1;
+                    end
+                    7'b1101000, 7'b1101001: begin
+                        fp_op = FP_OP_CVT_I2F;
+                        fp_reg_write_en = 1'b1;
+                    end
+                    7'b1110000, 7'b1110001: begin
+                        if (rs2_addr == 5'd0 && funct3 == 3'b000) begin
+                            fp_op = FP_OP_MV_F2X;
+                            reg_write_en = 1'b1;
+                        end else if (rs2_addr == 5'd0 && funct3 == 3'b001) begin
+                            fp_op = FP_OP_CLASS;
+                            reg_write_en = 1'b1;
+                        end else begin
+                            illegal_instr = 1'b1;
+                        end
+                    end
+                    7'b1111000, 7'b1111001: begin
+                        if (rs2_addr == 5'd0 && funct3 == 3'b000) begin
+                            fp_op = FP_OP_MV_X2F;
+                            fp_reg_write_en = 1'b1;
+                        end else begin
+                            illegal_instr = 1'b1;
+                        end
+                    end
+                    default: illegal_instr = 1'b1;
+                endcase
+
+                if (illegal_instr) begin
+                    is_fp           = 1'b0;
+                    fp_reg_write_en = 1'b0;
+                    reg_write_en    = 1'b0;
+                    fp_op           = FP_OP_NONE;
+                end
+            end
+
+            // -----------------
+            // Unrecognised opcode
+            default: begin
+                illegal_instr = 1'b1;
+            end
+
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/execute/integer_alu.sv b/rv64cpu/rtl/core/execute/integer_alu.sv
new file mode 100644
index 0000000..833f902
--- /dev/null
+++ b/rv64cpu/rtl/core/execute/integer_alu.sv
@@ -0,0 +1,123 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/alu.sv
+// Description: 64-bit Arithmetic Logic Unit (ALU)
+
+module integer_alu
+    import rv64_pkg::*;
+(
+    input  alu_op_t          alu_op,     // ALU operation select (from decoder)
+    input  logic [XLEN-1:0]  operand_a,    // Operand 1 (register rs1 value or PC)
+    input  logic [XLEN-1:0]  operand_b,    // Operand 2 (register rs2 value or immediate)
+
+    output logic [XLEN-1:0]  alu_result     // ALU result
+);
+
+    // Shift amount: RV64I uses lower 6 bits of rs2 (shamt[5:0])
+    logic [5:0] shamt;
+    assign shamt = operand_b[5:0];
+
+    logic signed [XLEN-1:0] operand_a_signed;
+    logic signed [XLEN-1:0] operand_b_signed;
+    logic signed [127:0]    mul_signed_signed;
+    logic signed [127:0]    mul_signed_unsigned;
+    logic [127:0]           mul_unsigned_unsigned;
+
+    assign operand_a_signed     = operand_a;
+    assign operand_b_signed     = operand_b;
+    assign mul_signed_signed    = operand_a_signed * operand_b_signed;
+    assign mul_signed_unsigned  = operand_a_signed * $signed({1'b0, operand_b});
+    assign mul_unsigned_unsigned = operand_a * operand_b;
+
+    always_comb begin
+        alu_result = '0;
+
+        case (alu_op)
+            ALU_ADD:  alu_result = operand_a + operand_b;
+            ALU_SUB:  alu_result = operand_a - operand_b;
+
+            // Logical left shift: the shift amount is the lower 6 bits of rs2
+            ALU_SLL:  alu_result = operand_a << shamt;
+
+            // Set less than (signed)
+            ALU_SLT:  alu_result = {63'b0, $signed(operand_a) < $signed(operand_b)};
+
+            // Set less than (unsigned)
+            ALU_SLTU: alu_result = {63'b0, operand_a < operand_b};
+
+            ALU_XOR:  alu_result = operand_a ^ operand_b;
+
+            // Logical right shift: the shift amount is the lower 6 bits of rs2
+            ALU_SRL:  alu_result = operand_a >> shamt;
+
+            // Arithmetic right shift: the shift amount is the lower 6 bits of rs2
+            ALU_SRA:  alu_result = $unsigned($signed(operand_a) >>> shamt);
+
+            ALU_OR:   alu_result = operand_a | operand_b;
+            ALU_AND:  alu_result = operand_a & operand_b;
+
+            ALU_MUL: begin
+                alu_result = mul_signed_signed[XLEN-1:0];
+            end
+
+            ALU_MULH: begin
+                alu_result = mul_signed_signed[127:64];
+            end
+
+            ALU_MULHSU: begin
+                alu_result = mul_signed_unsigned[127:64];
+            end
+
+            ALU_MULHU: begin
+                alu_result = mul_unsigned_unsigned[127:64];
+            end
+
+            ALU_DIV: begin
+                if (operand_b == '0) begin
+                    alu_result = {XLEN{1'b1}};
+                end else if ((operand_a == 64'h8000_0000_0000_0000) &&
+                             (operand_b == 64'hFFFF_FFFF_FFFF_FFFF)) begin
+                    alu_result = operand_a;
+                end else begin
+                    alu_result = $unsigned(operand_a_signed / operand_b_signed);
+                end
+            end
+
+            ALU_DIVU: begin
+                alu_result = (operand_b == '0) ? {XLEN{1'b1}} : (operand_a / operand_b);
+            end
+
+            ALU_REM: begin
+                if (operand_b == '0) begin
+                    alu_result = operand_a;
+                end else if ((operand_a == 64'h8000_0000_0000_0000) &&
+                             (operand_b == 64'hFFFF_FFFF_FFFF_FFFF)) begin
+                    alu_result = '0;
+                end else begin
+                    alu_result = $unsigned(operand_a_signed % operand_b_signed);
+                end
+            end
+
+            ALU_REMU: begin
+                alu_result = (operand_b == '0) ? operand_a : (operand_a % operand_b);
+            end
+
+            default:  alu_result = '0;
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/fpu/fpu.sv b/rv64cpu/rtl/core/fpu/fpu.sv
new file mode 100644
index 0000000..9e9494d
--- /dev/null
+++ b/rv64cpu/rtl/core/fpu/fpu.sv
@@ -0,0 +1,2392 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+// File path: rtl/core/fpu/fpu_unit.sv
+// Description: RV64F/RV64D floating-point execution unit.
+
+module fpu
+    import rv64_pkg::*;
+(
+    input  fp_op_t            fp_op,
+    input  fp_fmt_t           fp_fmt,
+    input  logic [2:0]        rm,
+    input  logic [2:0]        funct3,
+    input  logic [4:0]        rs2_addr,
+    input  logic [XLEN-1:0]   fs1_data,
+    input  logic [XLEN-1:0]   fs2_data,
+    input  logic [XLEN-1:0]   fs3_data,
+    input  logic [XLEN-1:0]   int_rs1_data,
+
+    output logic [XLEN-1:0]   fp_result,
+    output logic [XLEN-1:0]   int_result,
+    output logic [4:0]        fflags,
+    output logic              illegal_instr
+);
+
+    localparam logic [31:0] CANONICAL_NAN_S = 32'h7FC0_0000;
+    localparam logic [63:0] CANONICAL_NAN_D = 64'h7FF8_0000_0000_0000;
+
+    logic [31:0] s1_raw;
+    logic [31:0] s2_raw;
+    logic [31:0] s3_raw;
+    logic [31:0] s1;
+    logic [31:0] s2;
+    logic [31:0] s3;
+    logic [63:0] d1;
+    logic [63:0] d2;
+    logic [63:0] d3;
+
+    assign s1_raw = fs1_data[31:0];
+    assign s2_raw = fs2_data[31:0];
+    assign s3_raw = fs3_data[31:0];
+    assign s1 = (fs1_data[63:32] == 32'hFFFF_FFFF) ? s1_raw : CANONICAL_NAN_S;
+    assign s2 = (fs2_data[63:32] == 32'hFFFF_FFFF) ? s2_raw : CANONICAL_NAN_S;
+    assign s3 = (fs3_data[63:32] == 32'hFFFF_FFFF) ? s3_raw : CANONICAL_NAN_S;
+    assign d1 = fs1_data;
+    assign d2 = fs2_data;
+    assign d3 = fs3_data;
+
+    function automatic logic is_nan_s(input logic [31:0] value);
+        return (&value[30:23]) && (|value[22:0]);
+    endfunction
+
+    function automatic logic is_snan_s(input logic [31:0] value);
+        return is_nan_s(value) && (value[22] == 1'b0);
+    endfunction
+
+    function automatic logic is_nan_d(input logic [63:0] value);
+        return (&value[62:52]) && (|value[51:0]);
+    endfunction
+
+    function automatic logic is_snan_d(input logic [63:0] value);
+        return is_nan_d(value) && (value[51] == 1'b0);
+    endfunction
+
+    function automatic logic [9:0] class_s(input logic [31:0] value);
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        sign = value[31];
+        exp = value[30:23];
+        frac = value[22:0];
+        class_s = '0;
+        if (&exp && (frac != '0)) begin
+            class_s[is_snan_s(value) ? 8 : 9] = 1'b1;
+        end else if (sign && (&exp) && (frac == '0)) begin
+            class_s[0] = 1'b1;
+        end else if (sign && (exp != '0)) begin
+            class_s[1] = 1'b1;
+        end else if (sign && (exp == '0) && (frac != '0)) begin
+            class_s[2] = 1'b1;
+        end else if (sign && (exp == '0) && (frac == '0)) begin
+            class_s[3] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac == '0)) begin
+            class_s[4] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac != '0)) begin
+            class_s[5] = 1'b1;
+        end else if (!sign && (exp != '0) && !(&exp)) begin
+            class_s[6] = 1'b1;
+        end else if (!sign && (&exp) && (frac == '0)) begin
+            class_s[7] = 1'b1;
+        end
+    endfunction
+
+    function automatic logic [9:0] class_d(input logic [63:0] value);
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        sign = value[63];
+        exp = value[62:52];
+        frac = value[51:0];
+        class_d = '0;
+        if (&exp && (frac != '0)) begin
+            class_d[is_snan_d(value) ? 8 : 9] = 1'b1;
+        end else if (sign && (&exp) && (frac == '0)) begin
+            class_d[0] = 1'b1;
+        end else if (sign && (exp != '0)) begin
+            class_d[1] = 1'b1;
+        end else if (sign && (exp == '0) && (frac != '0)) begin
+            class_d[2] = 1'b1;
+        end else if (sign && (exp == '0) && (frac == '0)) begin
+            class_d[3] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac == '0)) begin
+            class_d[4] = 1'b1;
+        end else if (!sign && (exp == '0) && (frac != '0)) begin
+            class_d[5] = 1'b1;
+        end else if (!sign && (exp != '0) && !(&exp)) begin
+            class_d[6] = 1'b1;
+        end else if (!sign && (&exp) && (frac == '0)) begin
+            class_d[7] = 1'b1;
+        end
+    endfunction
+
+    function automatic logic less_s(input logic [31:0] lhs, input logic [31:0] rhs);
+        logic lhs_neg;
+        logic rhs_neg;
+        lhs_neg = lhs[31];
+        rhs_neg = rhs[31];
+        if ((lhs[30:0] == '0) && (rhs[30:0] == '0)) begin
+            return 1'b0;
+        end
+        if (lhs_neg != rhs_neg) begin
+            return lhs_neg;
+        end
+        return lhs_neg ? (lhs[30:0] > rhs[30:0]) : (lhs[30:0] < rhs[30:0]);
+    endfunction
+
+    function automatic logic less_d(input logic [63:0] lhs, input logic [63:0] rhs);
+        logic lhs_neg;
+        logic rhs_neg;
+        lhs_neg = lhs[63];
+        rhs_neg = rhs[63];
+        if ((lhs[62:0] == '0) && (rhs[62:0] == '0)) begin
+            return 1'b0;
+        end
+        if (lhs_neg != rhs_neg) begin
+            return lhs_neg;
+        end
+        return lhs_neg ? (lhs[62:0] > rhs[62:0]) : (lhs[62:0] < rhs[62:0]);
+    endfunction
+
+    function automatic logic equal_s(input logic [31:0] lhs, input logic [31:0] rhs);
+        if ((lhs[30:0] == '0) && (rhs[30:0] == '0)) begin
+            return 1'b1;
+        end
+        return lhs == rhs;
+    endfunction
+
+    function automatic logic equal_d(input logic [63:0] lhs, input logic [63:0] rhs);
+        if ((lhs[62:0] == '0) && (rhs[62:0] == '0)) begin
+            return 1'b1;
+        end
+        return lhs == rhs;
+    endfunction
+
+    function automatic logic [2:0] effective_rm(input logic [2:0] rm_value);
+        effective_rm = (rm_value <= 3'b100) ? rm_value : 3'b000;
+    endfunction
+
+    function automatic logic round_increment(
+        input logic [2:0]      round_mode,
+        input logic            sign,
+        input logic [127:0]    quotient,
+        input logic [127:0]    remainder,
+        input logic [127:0]    half
+    );
+        logic greater_than_half;
+        logic equal_to_half;
+
+        greater_than_half = remainder > half;
+        equal_to_half     = remainder == half;
+
+        unique case (effective_rm(round_mode))
+            3'b000: round_increment = greater_than_half || (equal_to_half && quotient[0]); // RNE
+            3'b001: round_increment = 1'b0;                                                // RTZ
+            3'b010: round_increment = sign && (remainder != '0);                           // RDN
+            3'b011: round_increment = !sign && (remainder != '0);                          // RUP
+            3'b100: round_increment = greater_than_half || equal_to_half;                  // RMM
+            default: round_increment = 1'b0;
+        endcase
+    endfunction
+
+    function automatic logic [63:0] invalid_integer_result(
+        input logic is_signed,
+        input logic is_long,
+        input logic sign,
+        input logic is_nan
+    );
+        if (is_signed) begin
+            if (sign && !is_nan) begin
+                invalid_integer_result = is_long ? 64'h8000_0000_0000_0000 : 64'hFFFF_FFFF_8000_0000;
+            end else begin
+                invalid_integer_result = is_long ? 64'h7FFF_FFFF_FFFF_FFFF : 64'h0000_0000_7FFF_FFFF;
+            end
+        end else begin
+            if (sign && !is_nan) begin
+                invalid_integer_result = '0;
+            end else begin
+                invalid_integer_result = 64'hFFFF_FFFF_FFFF_FFFF;
+            end
+        end
+    endfunction
+
+    function automatic logic [63:0] cvt_s_to_int(
+        input logic [31:0] value,
+        input logic [1:0]  int_fmt,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        logic [23:0] significand;
+        logic signed [12:0] unbiased_exp;
+        logic is_signed;
+        logic is_long;
+        logic invalid;
+        logic inexact;
+        logic [127:0] magnitude;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [127:0] rounded_magnitude;
+        logic [63:0] positive_limit;
+        logic [63:0] negative_limit;
+        logic [63:0] unsigned_limit;
+        logic [63:0] integer_value;
+        int shift;
+
+        sign          = value[31];
+        exp           = value[30:23];
+        frac          = value[22:0];
+        significand   = (exp == '0) ? {1'b0, frac} : {1'b1, frac};
+        unbiased_exp  = $signed({5'b0, exp}) - 13'sd127;
+        is_signed     = (int_fmt == 2'd0) || (int_fmt == 2'd2);
+        is_long       = (int_fmt == 2'd2) || (int_fmt == 2'd3);
+        invalid       = 1'b0;
+        inexact       = 1'b0;
+        magnitude     = '0;
+        quotient      = '0;
+        remainder     = '0;
+        divisor       = '0;
+        half          = '0;
+        rounded_magnitude = '0;
+        positive_limit = is_long ? 64'h7FFF_FFFF_FFFF_FFFF : 64'h0000_0000_7FFF_FFFF;
+        negative_limit = is_long ? 64'h8000_0000_0000_0000 : 64'h0000_0000_8000_0000;
+        unsigned_limit = is_long ? 64'hFFFF_FFFF_FFFF_FFFF : 64'h0000_0000_FFFF_FFFF;
+        integer_value  = '0;
+        flags          = '0;
+
+        if (is_nan_s(value) || (&exp)) begin
+            invalid = 1'b1;
+        end else if (significand == '0) begin
+            rounded_magnitude = '0;
+        end else begin
+            if (unbiased_exp >= 13'sd23) begin
+                shift = int'(unbiased_exp) - 23;
+                if (shift >= 104) begin
+                    invalid = 1'b1;
+                end else begin
+                    rounded_magnitude = {104'b0, significand} << shift;
+                end
+            end else begin
+                shift = 23 - int'(unbiased_exp);
+                if (shift > 24) begin
+                    quotient  = '0;
+                    remainder = {104'b0, significand};
+                    divisor   = '0;
+                    half      = '0;
+                    inexact   = remainder != '0;
+                    unique case (effective_rm(round_mode))
+                        3'b010:  rounded_magnitude = (sign && inexact) ? 128'd1 : 128'd0;
+                        3'b011:  rounded_magnitude = (!sign && inexact) ? 128'd1 : 128'd0;
+                        default: rounded_magnitude = '0;
+                    endcase
+                end else begin
+                    divisor   = 128'd1 << shift;
+                    quotient  = {104'b0, significand} >> shift;
+                    remainder = {104'b0, significand} & (divisor - 128'd1);
+                    half      = divisor >> 1;
+                    inexact   = (remainder != '0);
+                    rounded_magnitude = quotient +
+                                        (round_increment(round_mode, sign, quotient, remainder, half) ? 128'd1 : 128'd0);
+                end
+            end
+        end
+
+        if (!invalid) begin
+            if (is_signed) begin
+                if ((!sign && (rounded_magnitude > {64'b0, positive_limit})) ||
+                    (sign && (rounded_magnitude > {64'b0, negative_limit}))) begin
+                    invalid = 1'b1;
+                end
+            end else begin
+                if ((sign && (rounded_magnitude != '0)) ||
+                    (rounded_magnitude > {64'b0, unsigned_limit})) begin
+                    invalid = 1'b1;
+                end
+            end
+        end
+
+        if (invalid) begin
+            flags[4] = 1'b1;
+            return invalid_integer_result(is_signed, is_long, sign, is_nan_s(value));
+        end
+
+        if (inexact) begin
+            flags[0] = 1'b1;
+        end
+
+        integer_value = rounded_magnitude[63:0];
+        if (sign) begin
+            integer_value = (~integer_value) + 64'd1;
+        end
+
+        if (!is_long) begin
+            return {{32{integer_value[31]}}, integer_value[31:0]};
+        end
+
+        return integer_value;
+    endfunction
+
+    function automatic logic [63:0] cvt_d_to_int(
+        input logic [63:0] value,
+        input logic [1:0]  int_fmt,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        logic [52:0] significand;
+        logic signed [12:0] unbiased_exp;
+        logic is_signed;
+        logic is_long;
+        logic invalid;
+        logic inexact;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [127:0] rounded_magnitude;
+        logic [63:0] positive_limit;
+        logic [63:0] negative_limit;
+        logic [63:0] unsigned_limit;
+        logic [63:0] integer_value;
+        int shift;
+
+        sign          = value[63];
+        exp           = value[62:52];
+        frac          = value[51:0];
+        significand   = (exp == '0) ? {1'b0, frac} : {1'b1, frac};
+        unbiased_exp  = $signed({2'b0, exp}) - 13'sd1023;
+        is_signed     = (int_fmt == 2'd0) || (int_fmt == 2'd2);
+        is_long       = (int_fmt == 2'd2) || (int_fmt == 2'd3);
+        invalid       = 1'b0;
+        inexact       = 1'b0;
+        quotient      = '0;
+        remainder     = '0;
+        divisor       = '0;
+        half          = '0;
+        rounded_magnitude = '0;
+        positive_limit = is_long ? 64'h7FFF_FFFF_FFFF_FFFF : 64'h0000_0000_7FFF_FFFF;
+        negative_limit = is_long ? 64'h8000_0000_0000_0000 : 64'h0000_0000_8000_0000;
+        unsigned_limit = is_long ? 64'hFFFF_FFFF_FFFF_FFFF : 64'h0000_0000_FFFF_FFFF;
+        integer_value  = '0;
+        flags          = '0;
+
+        if (is_nan_d(value) || (&exp)) begin
+            invalid = 1'b1;
+        end else if (significand == '0) begin
+            rounded_magnitude = '0;
+        end else begin
+            if (unbiased_exp >= 13'sd52) begin
+                shift = int'(unbiased_exp) - 52;
+                if (shift >= 75) begin
+                    invalid = 1'b1;
+                end else begin
+                    rounded_magnitude = {75'b0, significand} << shift;
+                end
+            end else begin
+                shift = 52 - int'(unbiased_exp);
+                if (shift > 53) begin
+                    quotient  = '0;
+                    remainder = {75'b0, significand};
+                    inexact   = remainder != '0;
+                    unique case (effective_rm(round_mode))
+                        3'b010:  rounded_magnitude = (sign && inexact) ? 128'd1 : 128'd0;
+                        3'b011:  rounded_magnitude = (!sign && inexact) ? 128'd1 : 128'd0;
+                        default: rounded_magnitude = '0;
+                    endcase
+                end else begin
+                    divisor   = 128'd1 << shift;
+                    quotient  = {75'b0, significand} >> shift;
+                    remainder = {75'b0, significand} & (divisor - 128'd1);
+                    half      = divisor >> 1;
+                    inexact   = (remainder != '0);
+                    rounded_magnitude = quotient +
+                                        (round_increment(round_mode, sign, quotient, remainder, half) ? 128'd1 : 128'd0);
+                end
+            end
+        end
+
+        if (!invalid) begin
+            if (is_signed) begin
+                if ((!sign && (rounded_magnitude > {64'b0, positive_limit})) ||
+                    (sign && (rounded_magnitude > {64'b0, negative_limit}))) begin
+                    invalid = 1'b1;
+                end
+            end else begin
+                if ((sign && (rounded_magnitude != '0)) ||
+                    (rounded_magnitude > {64'b0, unsigned_limit})) begin
+                    invalid = 1'b1;
+                end
+            end
+        end
+
+        if (invalid) begin
+            flags[4] = 1'b1;
+            return invalid_integer_result(is_signed, is_long, sign, is_nan_d(value));
+        end
+
+        if (inexact) begin
+            flags[0] = 1'b1;
+        end
+
+        integer_value = rounded_magnitude[63:0];
+        if (sign) begin
+            integer_value = (~integer_value) + 64'd1;
+        end
+
+        if (!is_long) begin
+            return {{32{integer_value[31]}}, integer_value[31:0]};
+        end
+
+        return integer_value;
+    endfunction
+
+    function automatic int highest_bit_64(input logic [63:0] value);
+        highest_bit_64 = -1;
+        for (int bit_idx = 0; bit_idx < 64; bit_idx++) begin
+            if (value[bit_idx]) begin
+                highest_bit_64 = bit_idx;
+            end
+        end
+    endfunction
+
+    function automatic int highest_bit_53(input logic [52:0] value);
+        highest_bit_53 = -1;
+        for (int bit_idx = 0; bit_idx < 53; bit_idx++) begin
+            if (value[bit_idx]) begin
+                highest_bit_53 = bit_idx;
+            end
+        end
+    endfunction
+
+    function automatic logic [31:0] overflow_s_result(input logic sign, input logic [2:0] round_mode);
+        unique case (effective_rm(round_mode))
+            3'b001:  overflow_s_result = {sign, 8'hFE, 23'h7F_FFFF};
+            3'b010:  overflow_s_result = sign ? 32'hFF80_0000 : 32'h7F7F_FFFF;
+            3'b011:  overflow_s_result = sign ? 32'hFF7F_FFFF : 32'h7F80_0000;
+            default: overflow_s_result = {sign, 8'hFF, 23'b0};
+        endcase
+    endfunction
+
+    function automatic logic [63:0] overflow_d_result(input logic sign, input logic [2:0] round_mode);
+        unique case (effective_rm(round_mode))
+            3'b001:  overflow_d_result = {sign, 11'h7FE, 52'hF_FFFF_FFFF_FFFF};
+            3'b010:  overflow_d_result = sign ? 64'hFFF0_0000_0000_0000 : 64'h7FEF_FFFF_FFFF_FFFF;
+            3'b011:  overflow_d_result = sign ? 64'hFFEF_FFFF_FFFF_FFFF : 64'h7FF0_0000_0000_0000;
+            default: overflow_d_result = {sign, 11'h7FF, 52'b0};
+        endcase
+    endfunction
+
+    function automatic logic [27:0] shift_right_jam_28(
+        input logic [27:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 28) begin
+            return (value != '0) ? 28'd1 : 28'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 28; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 28'd1 : 28'd0);
+    endfunction
+
+    function automatic logic shifted_out_28(
+        input logic [27:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return 1'b0;
+        end
+        if (shift >= 28) begin
+            return value != '0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 28; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return sticky;
+    endfunction
+
+    function automatic logic [27:0] shift_right_jam_48(
+        input logic [47:0] value,
+        input int          shift
+    );
+        logic sticky;
+        logic [27:0] shifted;
+
+        if (shift <= 0) begin
+            return value[27:0];
+        end
+        if (shift >= 48) begin
+            return (value != '0) ? 28'd1 : 28'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 48; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        shifted = 28'((value >> shift));
+        return shifted | (sticky ? 28'd1 : 28'd0);
+    endfunction
+
+    function automatic logic [56:0] shift_right_jam_57(
+        input logic [56:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 57) begin
+            return (value != '0) ? 57'd1 : 57'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 57; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 57'd1 : 57'd0);
+    endfunction
+
+    function automatic logic [64:0] shift_right_jam_65(
+        input logic [64:0] value,
+        input int          shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 65) begin
+            return (value != '0) ? 65'd1 : 65'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 65; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 65'd1 : 65'd0);
+    endfunction
+
+    function automatic logic [127:0] shift_right_jam_128(
+        input logic [127:0] value,
+        input int           shift
+    );
+        logic sticky;
+
+        if (shift <= 0) begin
+            return value;
+        end
+        if (shift >= 128) begin
+            return (value != '0) ? 128'd1 : 128'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 128; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        return (value >> shift) | (sticky ? 128'd1 : 128'd0);
+    endfunction
+
+    function automatic logic [56:0] shift_right_jam_128_to_57(
+        input logic [127:0] value,
+        input int           shift
+    );
+        logic sticky;
+        logic [56:0] shifted;
+
+        if (shift <= 0) begin
+            return value[56:0];
+        end
+        if (shift >= 128) begin
+            return (value != '0) ? 57'd1 : 57'd0;
+        end
+
+        sticky = 1'b0;
+        for (int bit_idx = 0; bit_idx < 128; bit_idx++) begin
+            if (bit_idx < shift) begin
+                sticky = sticky | value[bit_idx];
+            end
+        end
+        shifted = 57'(value >> shift);
+        return shifted | (sticky ? 57'd1 : 57'd0);
+    endfunction
+
+    function automatic logic [31:0] round_pack_s(
+        input logic                   sign,
+        input logic signed [10:0]     unbiased_exp,
+        input logic [27:0]            significand,
+        input logic                   inexact_before_round,
+        input logic [2:0]             round_mode,
+        output logic [4:0]            flags
+    );
+        logic signed [10:0] result_unbiased_exp;
+        logic [27:0] result_sig;
+        logic round_any;
+        logic round_up;
+        logic [24:0] rounded_sig;
+        logic [7:0] result_exp;
+        logic [22:0] result_frac;
+        logic tiny_before_round;
+        int subnormal_shift;
+
+        flags = '0;
+        result_unbiased_exp = unbiased_exp;
+        result_sig = significand;
+        result_sig[0] = result_sig[0] | inexact_before_round;
+        tiny_before_round = 1'b0;
+
+        if (result_unbiased_exp < -11'sd126) begin
+            tiny_before_round = 1'b1;
+            subnormal_shift = -126 - int'(result_unbiased_exp);
+            result_sig = shift_right_jam_28(result_sig, subnormal_shift);
+            result_unbiased_exp = -11'sd126;
+        end
+
+        round_any = |result_sig[2:0];
+        unique case (effective_rm(round_mode))
+            3'b000:  round_up = result_sig[2] && (result_sig[1] || result_sig[0] || result_sig[3]);
+            3'b001:  round_up = 1'b0;
+            3'b010:  round_up = sign && round_any;
+            3'b011:  round_up = !sign && round_any;
+            3'b100:  round_up = result_sig[2];
+            default: round_up = 1'b0;
+        endcase
+
+        rounded_sig = {1'b0, result_sig[26:3]} + (round_up ? 25'd1 : 25'd0);
+        if (round_any) begin
+            flags[0] = 1'b1;
+        end
+        if (tiny_before_round && flags[0]) begin
+            flags[1] = 1'b1;
+        end
+
+        if (rounded_sig[24]) begin
+            rounded_sig = rounded_sig >> 1;
+            result_unbiased_exp = result_unbiased_exp + 11'sd1;
+        end
+
+        if (result_unbiased_exp > 11'sd127) begin
+            flags[2] = 1'b1;
+            flags[0] = 1'b1;
+            return overflow_s_result(sign, round_mode);
+        end
+
+        if ((result_unbiased_exp == -11'sd126) && !rounded_sig[23]) begin
+            result_exp = 8'b0;
+            result_frac = rounded_sig[22:0];
+            if (flags[0]) begin
+                flags[1] = 1'b1;
+            end
+        end else begin
+            result_exp = 8'(result_unbiased_exp + 11'sd127);
+            result_frac = rounded_sig[22:0];
+        end
+
+        return {sign, result_exp, result_frac};
+    endfunction
+
+    function automatic logic [63:0] round_pack_d(
+        input logic                   sign,
+        input logic signed [12:0]     unbiased_exp,
+        input logic [56:0]            significand,
+        input logic                   inexact_before_round,
+        input logic [2:0]             round_mode,
+        output logic [4:0]            flags
+    );
+        logic signed [12:0] result_unbiased_exp;
+        logic [56:0] result_sig;
+        logic round_any;
+        logic round_up;
+        logic [53:0] rounded_sig;
+        logic [10:0] result_exp;
+        logic [51:0] result_frac;
+        logic tiny_before_round;
+        int subnormal_shift;
+
+        flags = '0;
+        result_unbiased_exp = unbiased_exp;
+        result_sig = significand;
+        result_sig[0] = result_sig[0] | inexact_before_round;
+        tiny_before_round = 1'b0;
+
+        if (result_unbiased_exp < -13'sd1022) begin
+            tiny_before_round = 1'b1;
+            subnormal_shift = -1022 - int'(result_unbiased_exp);
+            result_sig = shift_right_jam_57(result_sig, subnormal_shift);
+            result_unbiased_exp = -13'sd1022;
+        end
+
+        round_any = |result_sig[2:0];
+        unique case (effective_rm(round_mode))
+            3'b000:  round_up = result_sig[2] && (result_sig[1] || result_sig[0] || result_sig[3]);
+            3'b001:  round_up = 1'b0;
+            3'b010:  round_up = sign && round_any;
+            3'b011:  round_up = !sign && round_any;
+            3'b100:  round_up = result_sig[2];
+            default: round_up = 1'b0;
+        endcase
+
+        rounded_sig = {1'b0, result_sig[55:3]} + (round_up ? 54'd1 : 54'd0);
+        if (round_any) begin
+            flags[0] = 1'b1;
+        end
+        if (tiny_before_round && flags[0]) begin
+            flags[1] = 1'b1;
+        end
+
+        if (rounded_sig[53]) begin
+            rounded_sig = rounded_sig >> 1;
+            result_unbiased_exp = result_unbiased_exp + 13'sd1;
+        end
+
+        if (result_unbiased_exp > 13'sd1023) begin
+            flags[2] = 1'b1;
+            flags[0] = 1'b1;
+            return overflow_d_result(sign, round_mode);
+        end
+
+        if ((result_unbiased_exp == -13'sd1022) && !rounded_sig[52]) begin
+            result_exp = 11'b0;
+            result_frac = rounded_sig[51:0];
+            if (flags[0]) begin
+                flags[1] = 1'b1;
+            end
+        end else begin
+            result_exp = 11'(result_unbiased_exp + 13'sd1023);
+            result_frac = rounded_sig[51:0];
+        end
+
+        return {sign, result_exp, result_frac};
+    endfunction
+
+    function automatic logic [31:0] fp_addsub_s(
+        input logic [31:0] lhs,
+        input logic [31:0] rhs,
+        input logic        subtract,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [27:0] lhs_sig;
+        logic [27:0] rhs_sig;
+        logic [27:0] result_sig;
+        logic [27:0] larger_sig;
+        logic [27:0] smaller_sig;
+        logic signed [10:0] larger_exp;
+        logic signs_match;
+        logic alignment_inexact;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = lhs[31];
+        rhs_sign = rhs[31] ^ subtract;
+        lhs_exp = lhs[30:23];
+        rhs_exp = rhs[30:23];
+        lhs_frac = lhs[22:0];
+        rhs_frac = rhs[22:0];
+        lhs_is_zero = (lhs[30:0] == '0);
+        rhs_is_zero = (rhs[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        signs_match = lhs_sign == rhs_sign;
+        alignment_inexact = 1'b0;
+
+        if (is_nan_s(lhs) || is_nan_s(rhs)) begin
+            flags[4] = is_snan_s(lhs) || is_snan_s(rhs);
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (lhs_is_inf && rhs_is_inf && !signs_match) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_S;
+            end
+            return lhs_is_inf ? {lhs_sign, 8'hFF, 23'b0} : {rhs_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero && rhs_is_zero) begin
+            result_sign = signs_match ? lhs_sign : (effective_rm(round_mode) == 3'b010);
+            return {result_sign, 31'b0};
+        end
+        if (lhs_is_zero) begin
+            return {rhs_sign, rhs[30:0]};
+        end
+        if (rhs_is_zero) begin
+            return lhs;
+        end
+
+        lhs_unbiased_exp = (lhs_exp == '0) ? -11'sd126 : ($signed({3'b0, lhs_exp}) - 11'sd127);
+        rhs_unbiased_exp = (rhs_exp == '0) ? -11'sd126 : ($signed({3'b0, rhs_exp}) - 11'sd127);
+        lhs_sig = {1'b0, (lhs_exp == '0) ? 1'b0 : 1'b1, lhs_frac, 3'b000};
+        rhs_sig = {1'b0, (rhs_exp == '0) ? 1'b0 : 1'b1, rhs_frac, 3'b000};
+
+        if ((lhs_unbiased_exp > rhs_unbiased_exp) ||
+            ((lhs_unbiased_exp == rhs_unbiased_exp) && (lhs_sig >= rhs_sig))) begin
+            exp_diff = int'(lhs_unbiased_exp) - int'(rhs_unbiased_exp);
+            larger_sig = lhs_sig;
+            alignment_inexact = shifted_out_28(rhs_sig, exp_diff);
+            smaller_sig = shift_right_jam_28(rhs_sig, exp_diff);
+            larger_exp = lhs_unbiased_exp;
+            result_sign = lhs_sign;
+        end else begin
+            exp_diff = int'(rhs_unbiased_exp) - int'(lhs_unbiased_exp);
+            larger_sig = rhs_sig;
+            alignment_inexact = shifted_out_28(lhs_sig, exp_diff);
+            smaller_sig = shift_right_jam_28(lhs_sig, exp_diff);
+            larger_exp = rhs_unbiased_exp;
+            result_sign = rhs_sign;
+        end
+
+        result_unbiased_exp = larger_exp;
+        if (signs_match) begin
+            result_sig = larger_sig + smaller_sig;
+            if (result_sig[27]) begin
+                result_sig = shift_right_jam_28(result_sig, 1);
+                result_unbiased_exp = result_unbiased_exp + 11'sd1;
+            end
+        end else begin
+            result_sig = larger_sig - smaller_sig;
+            if (result_sig == '0) begin
+                result_sign = effective_rm(round_mode) == 3'b010;
+                return {result_sign, 31'b0};
+            end
+            while (!result_sig[26] && (result_unbiased_exp > -11'sd126)) begin
+                result_sig = result_sig << 1;
+                result_unbiased_exp = result_unbiased_exp - 11'sd1;
+            end
+        end
+
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, alignment_inexact, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_addsub_d(
+        input logic [63:0] lhs,
+        input logic [63:0] rhs,
+        input logic        subtract,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [56:0] lhs_sig;
+        logic [56:0] rhs_sig;
+        logic [56:0] result_sig;
+        logic [56:0] larger_sig;
+        logic [56:0] smaller_sig;
+        logic signed [12:0] larger_exp;
+        logic signs_match;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = lhs[63];
+        rhs_sign = rhs[63] ^ subtract;
+        lhs_exp = lhs[62:52];
+        rhs_exp = rhs[62:52];
+        lhs_frac = lhs[51:0];
+        rhs_frac = rhs[51:0];
+        lhs_is_zero = (lhs[62:0] == '0);
+        rhs_is_zero = (rhs[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        signs_match = lhs_sign == rhs_sign;
+
+        if (is_nan_d(lhs) || is_nan_d(rhs)) begin
+            flags[4] = is_snan_d(lhs) || is_snan_d(rhs);
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (lhs_is_inf && rhs_is_inf && !signs_match) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_D;
+            end
+            return lhs_is_inf ? {lhs_sign, 11'h7FF, 52'b0} : {rhs_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero && rhs_is_zero) begin
+            result_sign = signs_match ? lhs_sign : (effective_rm(round_mode) == 3'b010);
+            return {result_sign, 63'b0};
+        end
+        if (lhs_is_zero) begin
+            return {rhs_sign, rhs[62:0]};
+        end
+        if (rhs_is_zero) begin
+            return lhs;
+        end
+
+        lhs_unbiased_exp = (lhs_exp == '0) ? -13'sd1022 : ($signed({2'b0, lhs_exp}) - 13'sd1023);
+        rhs_unbiased_exp = (rhs_exp == '0) ? -13'sd1022 : ($signed({2'b0, rhs_exp}) - 13'sd1023);
+        lhs_sig = {1'b0, (lhs_exp == '0) ? 1'b0 : 1'b1, lhs_frac, 3'b000};
+        rhs_sig = {1'b0, (rhs_exp == '0) ? 1'b0 : 1'b1, rhs_frac, 3'b000};
+
+        if ((lhs_unbiased_exp > rhs_unbiased_exp) ||
+            ((lhs_unbiased_exp == rhs_unbiased_exp) && (lhs_sig >= rhs_sig))) begin
+            exp_diff = int'(lhs_unbiased_exp) - int'(rhs_unbiased_exp);
+            larger_sig = lhs_sig;
+            smaller_sig = shift_right_jam_57(rhs_sig, exp_diff);
+            larger_exp = lhs_unbiased_exp;
+            result_sign = lhs_sign;
+        end else begin
+            exp_diff = int'(rhs_unbiased_exp) - int'(lhs_unbiased_exp);
+            larger_sig = rhs_sig;
+            smaller_sig = shift_right_jam_57(lhs_sig, exp_diff);
+            larger_exp = rhs_unbiased_exp;
+            result_sign = rhs_sign;
+        end
+
+        result_unbiased_exp = larger_exp;
+        if (signs_match) begin
+            result_sig = larger_sig + smaller_sig;
+            if (result_sig[56]) begin
+                result_sig = shift_right_jam_57(result_sig, 1);
+                result_unbiased_exp = result_unbiased_exp + 13'sd1;
+            end
+        end else begin
+            result_sig = larger_sig - smaller_sig;
+            if (result_sig == '0) begin
+                result_sign = effective_rm(round_mode) == 3'b010;
+                return {result_sign, 63'b0};
+            end
+            while (!result_sig[55] && (result_unbiased_exp > -13'sd1022)) begin
+                result_sig = result_sig << 1;
+                result_unbiased_exp = result_unbiased_exp - 13'sd1;
+            end
+        end
+
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_mul_d(
+        input logic [63:0] lhs,
+        input logic [63:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] lhs_sig;
+        logic [52:0] rhs_sig;
+        logic [105:0] product;
+        logic [56:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[63] ^ rhs[63];
+        lhs_exp = lhs[62:52];
+        rhs_exp = rhs[62:52];
+        lhs_frac = lhs[51:0];
+        rhs_frac = rhs[51:0];
+        lhs_is_zero = (lhs[62:0] == '0);
+        rhs_is_zero = (rhs[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_d(lhs) || is_nan_d(rhs)) begin
+            flags[4] = is_snan_d(lhs) || is_snan_d(rhs);
+            return CANONICAL_NAN_D;
+        end
+
+        if ((lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            return {result_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            return {result_sign, 63'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_53({1'b0, lhs_frac});
+            lhs_unbiased_exp = 13'(lhs_msb - 1074);
+            lhs_sig = {1'b0, lhs_frac} << (52 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({2'b0, lhs_exp}) - 13'sd1023;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_53({1'b0, rhs_frac});
+            rhs_unbiased_exp = 13'(rhs_msb - 1074);
+            rhs_sig = {1'b0, rhs_frac} << (52 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({2'b0, rhs_exp}) - 13'sd1023;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[105]) begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 13'sd1;
+            result_sig = shift_right_jam_128_to_57({22'b0, product}, 50);
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            result_sig = shift_right_jam_128_to_57({22'b0, product}, 49);
+        end
+
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_mul_s(
+        input logic [31:0] lhs,
+        input logic [31:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] lhs_sig;
+        logic [23:0] rhs_sig;
+        logic [47:0] product;
+        logic [27:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[31] ^ rhs[31];
+        lhs_exp = lhs[30:23];
+        rhs_exp = rhs[30:23];
+        lhs_frac = lhs[22:0];
+        rhs_frac = rhs[22:0];
+        lhs_is_zero = (lhs[30:0] == '0);
+        rhs_is_zero = (rhs[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_s(lhs) || is_nan_s(rhs)) begin
+            flags[4] = is_snan_s(lhs) || is_snan_s(rhs);
+            return CANONICAL_NAN_S;
+        end
+
+        if ((lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            return {result_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            return {result_sign, 31'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_64({41'b0, lhs_frac});
+            lhs_unbiased_exp = 11'(lhs_msb - 149);
+            lhs_sig = {1'b0, lhs_frac} << (23 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({3'b0, lhs_exp}) - 11'sd127;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_64({41'b0, rhs_frac});
+            rhs_unbiased_exp = 11'(rhs_msb - 149);
+            rhs_sig = {1'b0, rhs_frac} << (23 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({3'b0, rhs_exp}) - 11'sd127;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[47]) begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 11'sd1;
+            result_sig = shift_right_jam_48(product, 21);
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            result_sig = shift_right_jam_48(product, 20);
+        end
+
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_div_s(
+        input logic [31:0] lhs,
+        input logic [31:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] lhs_sig;
+        logic [23:0] rhs_sig;
+        logic [63:0] dividend;
+        logic [63:0] quotient;
+        logic [23:0] remainder;
+        logic [27:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[31] ^ rhs[31];
+        lhs_exp = lhs[30:23];
+        rhs_exp = rhs[30:23];
+        lhs_frac = lhs[22:0];
+        rhs_frac = rhs[22:0];
+        lhs_is_zero = (lhs[30:0] == '0);
+        rhs_is_zero = (rhs[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_s(lhs) || is_nan_s(rhs)) begin
+            flags[4] = is_snan_s(lhs) || is_snan_s(rhs);
+            return CANONICAL_NAN_S;
+        end
+
+        if ((lhs_is_zero && rhs_is_zero) || (lhs_is_inf && rhs_is_inf)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_zero) begin
+            if (rhs_is_zero && !lhs_is_zero && !lhs_is_inf) begin
+                flags[3] = 1'b1;
+            end
+            return {result_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_inf) begin
+            return {result_sign, 31'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_64({41'b0, lhs_frac});
+            lhs_unbiased_exp = 11'(lhs_msb - 149);
+            lhs_sig = {1'b0, lhs_frac} << (23 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({3'b0, lhs_exp}) - 11'sd127;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_64({41'b0, rhs_frac});
+            rhs_unbiased_exp = 11'(rhs_msb - 149);
+            rhs_sig = {1'b0, rhs_frac} << (23 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({3'b0, rhs_exp}) - 11'sd127;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (lhs_sig >= rhs_sig) begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp;
+            dividend = {40'b0, lhs_sig} << 26;
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp - 11'sd1;
+            dividend = {40'b0, lhs_sig} << 27;
+        end
+
+        quotient = dividend / {40'b0, rhs_sig};
+        remainder = 24'(dividend % {40'b0, rhs_sig});
+        result_sig = 28'(quotient);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_div_d(
+        input logic [63:0] lhs,
+        input logic [63:0] rhs,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] lhs_sig;
+        logic [52:0] rhs_sig;
+        logic [127:0] dividend;
+        logic [127:0] quotient;
+        logic [52:0] remainder;
+        logic [56:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+
+        flags = '0;
+        result_sign = lhs[63] ^ rhs[63];
+        lhs_exp = lhs[62:52];
+        rhs_exp = rhs[62:52];
+        lhs_frac = lhs[51:0];
+        rhs_frac = rhs[51:0];
+        lhs_is_zero = (lhs[62:0] == '0);
+        rhs_is_zero = (rhs[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+
+        if (is_nan_d(lhs) || is_nan_d(rhs)) begin
+            flags[4] = is_snan_d(lhs) || is_snan_d(rhs);
+            return CANONICAL_NAN_D;
+        end
+
+        if ((lhs_is_zero && rhs_is_zero) || (lhs_is_inf && rhs_is_inf)) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_zero) begin
+            if (rhs_is_zero && !lhs_is_zero && !lhs_is_inf) begin
+                flags[3] = 1'b1;
+            end
+            return {result_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_inf) begin
+            return {result_sign, 63'b0};
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_53({1'b0, lhs_frac});
+            lhs_unbiased_exp = 13'(lhs_msb - 1074);
+            lhs_sig = {1'b0, lhs_frac} << (52 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({2'b0, lhs_exp}) - 13'sd1023;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_53({1'b0, rhs_frac});
+            rhs_unbiased_exp = 13'(rhs_msb - 1074);
+            rhs_sig = {1'b0, rhs_frac} << (52 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({2'b0, rhs_exp}) - 13'sd1023;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (lhs_sig >= rhs_sig) begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp;
+            dividend = {75'b0, lhs_sig} << 55;
+        end else begin
+            result_unbiased_exp = lhs_unbiased_exp - rhs_unbiased_exp - 13'sd1;
+            dividend = {75'b0, lhs_sig} << 56;
+        end
+
+        quotient = dividend / {75'b0, rhs_sig};
+        remainder = 53'(dividend % {75'b0, rhs_sig});
+        result_sig = 57'(quotient);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_sqrt_s(
+        input logic [31:0] value,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        logic signed [10:0] unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] sig;
+        logic [63:0] radicand;
+        logic [63:0] trial;
+        logic [63:0] root;
+        logic [63:0] remainder;
+        logic [27:0] result_sig;
+        int sig_msb;
+
+        flags = '0;
+        sign = value[31];
+        exp = value[30:23];
+        frac = value[22:0];
+
+        if (is_nan_s(value)) begin
+            flags[4] = is_snan_s(value);
+            return CANONICAL_NAN_S;
+        end
+
+        if ((&exp) && (frac == '0)) begin
+            if (sign) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_S;
+            end
+            return value;
+        end
+
+        if (value[30:0] == '0) begin
+            return value;
+        end
+
+        if (sign) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_S;
+        end
+
+        if (exp == '0) begin
+            sig_msb = highest_bit_64({41'b0, frac});
+            unbiased_exp = 11'(sig_msb - 149);
+            sig = {1'b0, frac} << (23 - sig_msb);
+        end else begin
+            unbiased_exp = $signed({3'b0, exp}) - 11'sd127;
+            sig = {1'b1, frac};
+        end
+
+        if (unbiased_exp[0]) begin
+            radicand = {39'b0, sig, 1'b0} << 29;
+            result_unbiased_exp = (unbiased_exp - 11'sd1) >>> 1;
+        end else begin
+            radicand = {40'b0, sig} << 29;
+            result_unbiased_exp = unbiased_exp >>> 1;
+        end
+
+        root = '0;
+        for (int bit_idx = 26; bit_idx >= 0; bit_idx--) begin
+            trial = root | (64'd1 << bit_idx);
+            if ((trial * trial) <= radicand) begin
+                root = trial;
+            end
+        end
+
+        remainder = radicand - (root * root);
+        result_sig = 28'(root);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_s(1'b0, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_sqrt_d(
+        input logic [63:0] value,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        logic signed [12:0] unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] sig;
+        logic [127:0] radicand;
+        logic [127:0] trial;
+        logic [127:0] root;
+        logic [127:0] remainder;
+        logic [56:0] result_sig;
+        int sig_msb;
+
+        flags = '0;
+        sign = value[63];
+        exp = value[62:52];
+        frac = value[51:0];
+
+        if (is_nan_d(value)) begin
+            flags[4] = is_snan_d(value);
+            return CANONICAL_NAN_D;
+        end
+
+        if ((&exp) && (frac == '0)) begin
+            if (sign) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_D;
+            end
+            return value;
+        end
+
+        if (value[62:0] == '0) begin
+            return value;
+        end
+
+        if (sign) begin
+            flags[4] = 1'b1;
+            return CANONICAL_NAN_D;
+        end
+
+        if (exp == '0) begin
+            sig_msb = highest_bit_53({1'b0, frac});
+            unbiased_exp = 13'(sig_msb - 1074);
+            sig = {1'b0, frac} << (52 - sig_msb);
+        end else begin
+            unbiased_exp = $signed({2'b0, exp}) - 13'sd1023;
+            sig = {1'b1, frac};
+        end
+
+        if (unbiased_exp[0]) begin
+            radicand = {74'b0, sig, 1'b0} << 58;
+            result_unbiased_exp = (unbiased_exp - 13'sd1) >>> 1;
+        end else begin
+            radicand = {75'b0, sig} << 58;
+            result_unbiased_exp = unbiased_exp >>> 1;
+        end
+
+        root = '0;
+        for (int bit_idx = 55; bit_idx >= 0; bit_idx--) begin
+            trial = root | (128'd1 << bit_idx);
+            if ((trial * trial) <= radicand) begin
+                root = trial;
+            end
+        end
+
+        remainder = radicand - (root * root);
+        result_sig = 57'(root);
+        if (remainder != '0) begin
+            result_sig[0] = 1'b1;
+        end
+
+        return round_pack_d(1'b0, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [31:0] fp_fma_s(
+        input logic [31:0] mul_lhs,
+        input logic [31:0] mul_rhs,
+        input logic [31:0] addend,
+        input logic        negate_product,
+        input logic        negate_addend,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic addend_sign;
+        logic product_sign;
+        logic result_sign;
+        logic [7:0] lhs_exp;
+        logic [7:0] rhs_exp;
+        logic [7:0] addend_exp;
+        logic [22:0] lhs_frac;
+        logic [22:0] rhs_frac;
+        logic [22:0] addend_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic addend_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic addend_is_inf;
+        logic invalid_multiply;
+        logic signed [10:0] lhs_unbiased_exp;
+        logic signed [10:0] rhs_unbiased_exp;
+        logic signed [10:0] addend_unbiased_exp;
+        logic signed [10:0] product_unbiased_exp;
+        logic signed [10:0] common_unbiased_exp;
+        logic signed [10:0] result_unbiased_exp;
+        logic [23:0] lhs_sig;
+        logic [23:0] rhs_sig;
+        logic [23:0] addend_sig;
+        logic [47:0] product;
+        logic [47:0] normalized_product;
+        logic [64:0] product_ext;
+        logic [64:0] addend_ext;
+        logic [64:0] aligned_product;
+        logic [64:0] aligned_addend;
+        logic [64:0] result_ext;
+        logic [64:0] rounded_source;
+        logic [27:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+        int addend_msb;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = mul_lhs[31];
+        rhs_sign = mul_rhs[31];
+        addend_sign = addend[31] ^ negate_addend;
+        product_sign = lhs_sign ^ rhs_sign ^ negate_product;
+        lhs_exp = mul_lhs[30:23];
+        rhs_exp = mul_rhs[30:23];
+        addend_exp = addend[30:23];
+        lhs_frac = mul_lhs[22:0];
+        rhs_frac = mul_rhs[22:0];
+        addend_frac = addend[22:0];
+        lhs_is_zero = (mul_lhs[30:0] == '0);
+        rhs_is_zero = (mul_rhs[30:0] == '0);
+        addend_is_zero = (addend[30:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        addend_is_inf = (&addend_exp) && (addend_frac == '0);
+        invalid_multiply = (lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero);
+
+        if (is_nan_s(mul_lhs) || is_nan_s(mul_rhs) || is_nan_s(addend) || invalid_multiply) begin
+            flags[4] = invalid_multiply || is_snan_s(mul_lhs) || is_snan_s(mul_rhs) || is_snan_s(addend);
+            return CANONICAL_NAN_S;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (addend_is_inf && (product_sign != addend_sign)) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_S;
+            end
+            return {product_sign, 8'hFF, 23'b0};
+        end
+
+        if (addend_is_inf) begin
+            return {addend_sign, 8'hFF, 23'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            if (addend_is_zero) begin
+                result_sign = (product_sign == addend_sign) ? product_sign : (effective_rm(round_mode) == 3'b010);
+                return {result_sign, 31'b0};
+            end
+            return {addend_sign, addend[30:0]};
+        end
+
+        if (addend_is_zero) begin
+            return fp_mul_s({mul_lhs[31] ^ negate_product, mul_lhs[30:0]}, mul_rhs, round_mode, flags);
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_64({41'b0, lhs_frac});
+            lhs_unbiased_exp = 11'(lhs_msb - 149);
+            lhs_sig = {1'b0, lhs_frac} << (23 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({3'b0, lhs_exp}) - 11'sd127;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_64({41'b0, rhs_frac});
+            rhs_unbiased_exp = 11'(rhs_msb - 149);
+            rhs_sig = {1'b0, rhs_frac} << (23 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({3'b0, rhs_exp}) - 11'sd127;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (addend_exp == '0) begin
+            addend_msb = highest_bit_64({41'b0, addend_frac});
+            addend_unbiased_exp = 11'(addend_msb - 149);
+            addend_sig = {1'b0, addend_frac} << (23 - addend_msb);
+        end else begin
+            addend_unbiased_exp = $signed({3'b0, addend_exp}) - 11'sd127;
+            addend_sig = {1'b1, addend_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[47]) begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 11'sd1;
+            normalized_product = product;
+        end else begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            normalized_product = product << 1;
+        end
+
+        product_ext = {4'b0, normalized_product, 13'b0};
+        addend_ext = {4'b0, addend_sig, 37'b0};
+
+        if (product_unbiased_exp >= addend_unbiased_exp) begin
+            common_unbiased_exp = product_unbiased_exp;
+            exp_diff = int'(product_unbiased_exp) - int'(addend_unbiased_exp);
+            aligned_product = product_ext;
+            aligned_addend = shift_right_jam_65(addend_ext, exp_diff);
+        end else begin
+            common_unbiased_exp = addend_unbiased_exp;
+            exp_diff = int'(addend_unbiased_exp) - int'(product_unbiased_exp);
+            aligned_product = shift_right_jam_65(product_ext, exp_diff);
+            aligned_addend = addend_ext;
+        end
+
+        result_unbiased_exp = common_unbiased_exp;
+        if (product_sign == addend_sign) begin
+            result_ext = aligned_product + aligned_addend;
+            result_sign = product_sign;
+            if (result_ext[61]) begin
+                result_ext = shift_right_jam_65(result_ext, 1);
+                result_unbiased_exp = result_unbiased_exp + 11'sd1;
+            end
+        end else if (aligned_product >= aligned_addend) begin
+            result_ext = aligned_product - aligned_addend;
+            result_sign = product_sign;
+        end else begin
+            result_ext = aligned_addend - aligned_product;
+            result_sign = addend_sign;
+        end
+
+        if (result_ext == '0) begin
+            result_sign = effective_rm(round_mode) == 3'b010;
+            return {result_sign, 31'b0};
+        end
+
+        while (!result_ext[60] && (result_unbiased_exp > -11'sd126)) begin
+            result_ext = result_ext << 1;
+            result_unbiased_exp = result_unbiased_exp - 11'sd1;
+        end
+
+        rounded_source = shift_right_jam_65(result_ext, 34);
+        result_sig = rounded_source[27:0];
+        return round_pack_s(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] fp_fma_d(
+        input logic [63:0] mul_lhs,
+        input logic [63:0] mul_rhs,
+        input logic [63:0] addend,
+        input logic        negate_product,
+        input logic        negate_addend,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic lhs_sign;
+        logic rhs_sign;
+        logic addend_sign;
+        logic product_sign;
+        logic result_sign;
+        logic [10:0] lhs_exp;
+        logic [10:0] rhs_exp;
+        logic [10:0] addend_exp;
+        logic [51:0] lhs_frac;
+        logic [51:0] rhs_frac;
+        logic [51:0] addend_frac;
+        logic lhs_is_zero;
+        logic rhs_is_zero;
+        logic addend_is_zero;
+        logic lhs_is_inf;
+        logic rhs_is_inf;
+        logic addend_is_inf;
+        logic invalid_multiply;
+        logic signed [12:0] lhs_unbiased_exp;
+        logic signed [12:0] rhs_unbiased_exp;
+        logic signed [12:0] addend_unbiased_exp;
+        logic signed [12:0] product_unbiased_exp;
+        logic signed [12:0] common_unbiased_exp;
+        logic signed [12:0] result_unbiased_exp;
+        logic [52:0] lhs_sig;
+        logic [52:0] rhs_sig;
+        logic [52:0] addend_sig;
+        logic [105:0] product;
+        logic [105:0] normalized_product;
+        logic [127:0] product_ext;
+        logic [127:0] addend_ext;
+        logic [127:0] aligned_product;
+        logic [127:0] aligned_addend;
+        logic [127:0] result_ext;
+        logic [127:0] rounded_source;
+        logic [56:0] result_sig;
+        int lhs_msb;
+        int rhs_msb;
+        int addend_msb;
+        int exp_diff;
+
+        flags = '0;
+        lhs_sign = mul_lhs[63];
+        rhs_sign = mul_rhs[63];
+        addend_sign = addend[63] ^ negate_addend;
+        product_sign = lhs_sign ^ rhs_sign ^ negate_product;
+        lhs_exp = mul_lhs[62:52];
+        rhs_exp = mul_rhs[62:52];
+        addend_exp = addend[62:52];
+        lhs_frac = mul_lhs[51:0];
+        rhs_frac = mul_rhs[51:0];
+        addend_frac = addend[51:0];
+        lhs_is_zero = (mul_lhs[62:0] == '0);
+        rhs_is_zero = (mul_rhs[62:0] == '0);
+        addend_is_zero = (addend[62:0] == '0);
+        lhs_is_inf = (&lhs_exp) && (lhs_frac == '0);
+        rhs_is_inf = (&rhs_exp) && (rhs_frac == '0);
+        addend_is_inf = (&addend_exp) && (addend_frac == '0);
+        invalid_multiply = (lhs_is_inf && rhs_is_zero) || (rhs_is_inf && lhs_is_zero);
+
+        if (is_nan_d(mul_lhs) || is_nan_d(mul_rhs) || is_nan_d(addend) || invalid_multiply) begin
+            flags[4] = invalid_multiply || is_snan_d(mul_lhs) || is_snan_d(mul_rhs) || is_snan_d(addend);
+            return CANONICAL_NAN_D;
+        end
+
+        if (lhs_is_inf || rhs_is_inf) begin
+            if (addend_is_inf && (product_sign != addend_sign)) begin
+                flags[4] = 1'b1;
+                return CANONICAL_NAN_D;
+            end
+            return {product_sign, 11'h7FF, 52'b0};
+        end
+
+        if (addend_is_inf) begin
+            return {addend_sign, 11'h7FF, 52'b0};
+        end
+
+        if (lhs_is_zero || rhs_is_zero) begin
+            if (addend_is_zero) begin
+                result_sign = (product_sign == addend_sign) ? product_sign : (effective_rm(round_mode) == 3'b010);
+                return {result_sign, 63'b0};
+            end
+            return {addend_sign, addend[62:0]};
+        end
+
+        if (addend_is_zero) begin
+            return fp_mul_d({mul_lhs[63] ^ negate_product, mul_lhs[62:0]}, mul_rhs, round_mode, flags);
+        end
+
+        if (lhs_exp == '0) begin
+            lhs_msb = highest_bit_53({1'b0, lhs_frac});
+            lhs_unbiased_exp = 13'(lhs_msb - 1074);
+            lhs_sig = {1'b0, lhs_frac} << (52 - lhs_msb);
+        end else begin
+            lhs_unbiased_exp = $signed({2'b0, lhs_exp}) - 13'sd1023;
+            lhs_sig = {1'b1, lhs_frac};
+        end
+
+        if (rhs_exp == '0) begin
+            rhs_msb = highest_bit_53({1'b0, rhs_frac});
+            rhs_unbiased_exp = 13'(rhs_msb - 1074);
+            rhs_sig = {1'b0, rhs_frac} << (52 - rhs_msb);
+        end else begin
+            rhs_unbiased_exp = $signed({2'b0, rhs_exp}) - 13'sd1023;
+            rhs_sig = {1'b1, rhs_frac};
+        end
+
+        if (addend_exp == '0) begin
+            addend_msb = highest_bit_53({1'b0, addend_frac});
+            addend_unbiased_exp = 13'(addend_msb - 1074);
+            addend_sig = {1'b0, addend_frac} << (52 - addend_msb);
+        end else begin
+            addend_unbiased_exp = $signed({2'b0, addend_exp}) - 13'sd1023;
+            addend_sig = {1'b1, addend_frac};
+        end
+
+        product = lhs_sig * rhs_sig;
+        if (product[105]) begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp + 13'sd1;
+            normalized_product = product;
+        end else begin
+            product_unbiased_exp = lhs_unbiased_exp + rhs_unbiased_exp;
+            normalized_product = product << 1;
+        end
+
+        product_ext = {9'b0, normalized_product, 13'b0};
+        addend_ext = {9'b0, addend_sig, 66'b0};
+
+        if (product_unbiased_exp >= addend_unbiased_exp) begin
+            common_unbiased_exp = product_unbiased_exp;
+            exp_diff = int'(product_unbiased_exp) - int'(addend_unbiased_exp);
+            aligned_product = product_ext;
+            aligned_addend = shift_right_jam_128(addend_ext, exp_diff);
+        end else begin
+            common_unbiased_exp = addend_unbiased_exp;
+            exp_diff = int'(addend_unbiased_exp) - int'(product_unbiased_exp);
+            aligned_product = shift_right_jam_128(product_ext, exp_diff);
+            aligned_addend = addend_ext;
+        end
+
+        result_unbiased_exp = common_unbiased_exp;
+        if (product_sign == addend_sign) begin
+            result_ext = aligned_product + aligned_addend;
+            result_sign = product_sign;
+            if (result_ext[119]) begin
+                result_ext = shift_right_jam_128(result_ext, 1);
+                result_unbiased_exp = result_unbiased_exp + 13'sd1;
+            end
+        end else if (aligned_product >= aligned_addend) begin
+            result_ext = aligned_product - aligned_addend;
+            result_sign = product_sign;
+        end else begin
+            result_ext = aligned_addend - aligned_product;
+            result_sign = addend_sign;
+        end
+
+        if (result_ext == '0) begin
+            result_sign = effective_rm(round_mode) == 3'b010;
+            return {result_sign, 63'b0};
+        end
+
+        while (!result_ext[118] && (result_unbiased_exp > -13'sd1022)) begin
+            result_ext = result_ext << 1;
+            result_unbiased_exp = result_unbiased_exp - 13'sd1;
+        end
+
+        rounded_source = shift_right_jam_128(result_ext, 63);
+        result_sig = rounded_source[56:0];
+        return round_pack_d(result_sign, result_unbiased_exp, result_sig, 1'b0, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] pack_uint_to_s(
+        input logic        sign,
+        input logic [63:0] magnitude,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        int msb_index;
+        int shift;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [24:0] rounded_sig;
+        logic [7:0] exp_field;
+        logic [31:0] result_s;
+
+        flags = '0;
+        if (magnitude == '0) begin
+            return {32'hFFFF_FFFF, sign, 31'b0};
+        end
+
+        msb_index = highest_bit_64(magnitude);
+        exp_field = 8'(msb_index + 127);
+        if (msb_index <= 23) begin
+            rounded_sig = {1'b0, magnitude[23:0] << (23 - msb_index)};
+        end else begin
+            shift     = msb_index - 23;
+            divisor   = 128'd1 << shift;
+            quotient  = {64'b0, magnitude} >> shift;
+            remainder = {64'b0, magnitude} & (divisor - 128'd1);
+            half      = divisor >> 1;
+            rounded_sig = {1'b0, quotient[23:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 25'd1 : 25'd0);
+            if (remainder != '0) begin
+                flags[0] = 1'b1;
+            end
+            if (rounded_sig[24]) begin
+                rounded_sig = rounded_sig >> 1;
+                exp_field = exp_field + 8'd1;
+            end
+        end
+
+        result_s = {sign, exp_field, rounded_sig[22:0]};
+        return {32'hFFFF_FFFF, result_s};
+    endfunction
+
+    function automatic logic [63:0] pack_uint_to_d(
+        input logic        sign,
+        input logic [63:0] magnitude,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        int msb_index;
+        int shift;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [53:0] rounded_sig;
+        logic [10:0] exp_field;
+
+        flags = '0;
+        if (magnitude == '0) begin
+            return {sign, 63'b0};
+        end
+
+        msb_index = highest_bit_64(magnitude);
+        exp_field = 11'(msb_index + 1023);
+        if (msb_index <= 52) begin
+            rounded_sig = {1'b0, magnitude[52:0] << (52 - msb_index)};
+        end else begin
+            shift     = msb_index - 52;
+            divisor   = 128'd1 << shift;
+            quotient  = {64'b0, magnitude} >> shift;
+            remainder = {64'b0, magnitude} & (divisor - 128'd1);
+            half      = divisor >> 1;
+            rounded_sig = {1'b0, quotient[52:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 54'd1 : 54'd0);
+            if (remainder != '0) begin
+                flags[0] = 1'b1;
+            end
+            if (rounded_sig[53]) begin
+                rounded_sig = rounded_sig >> 1;
+                exp_field = exp_field + 11'd1;
+            end
+        end
+
+        return {sign, exp_field, rounded_sig[51:0]};
+    endfunction
+
+    function automatic logic [63:0] cvt_int_to_fp(
+        input logic [63:0] int_value,
+        input logic [1:0]  int_fmt,
+        input logic        to_double,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic is_signed;
+        logic is_long;
+        logic sign;
+        logic [63:0] source_value;
+        logic [63:0] magnitude;
+
+        is_signed    = (int_fmt == 2'd0) || (int_fmt == 2'd2);
+        is_long      = (int_fmt == 2'd2) || (int_fmt == 2'd3);
+        source_value = is_long ? int_value :
+                       (is_signed ? {{32{int_value[31]}}, int_value[31:0]} : {32'b0, int_value[31:0]});
+        sign         = is_signed && source_value[63];
+        magnitude    = sign ? ((~source_value) + 64'd1) : source_value;
+
+        if (to_double) begin
+            return pack_uint_to_d(sign, magnitude, round_mode, flags);
+        end
+
+        return pack_uint_to_s(sign, magnitude, round_mode, flags);
+    endfunction
+
+    function automatic logic [63:0] cvt_s_to_d(
+        input logic [31:0] value,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [7:0] exp;
+        logic [22:0] frac;
+        int frac_msb;
+        logic [10:0] exp_d;
+        logic [51:0] frac_d;
+        logic [22:0] subnormal_tail_s;
+
+        flags = '0;
+        sign  = value[31];
+        exp   = value[30:23];
+        frac  = value[22:0];
+
+        if (is_nan_s(value)) begin
+            flags[4] = is_snan_s(value);
+            return CANONICAL_NAN_D;
+        end
+        if (&exp) begin
+            return {sign, 11'h7FF, 52'b0};
+        end
+        if ((exp == '0) && (frac == '0)) begin
+            return {sign, 63'b0};
+        end
+        if (exp == '0) begin
+            frac_msb = -1;
+            for (int bit_idx = 0; bit_idx < 23; bit_idx++) begin
+                if (frac[bit_idx]) begin
+                    frac_msb = bit_idx;
+                end
+            end
+            exp_d = 11'(frac_msb - 149 + 1023);
+            subnormal_tail_s = frac ^ (23'(1) << frac_msb);
+            frac_d = {29'b0, subnormal_tail_s} << (52 - frac_msb);
+            return {sign, exp_d, frac_d};
+        end
+
+        exp_d = 11'(int'(exp) - 127 + 1023);
+        frac_d = {frac, 29'b0};
+        return {sign, exp_d, frac_d};
+    endfunction
+
+    function automatic logic [63:0] cvt_d_to_s(
+        input logic [63:0] value,
+        input logic [2:0]  round_mode,
+        output logic [4:0] flags
+    );
+        logic sign;
+        logic [10:0] exp;
+        logic [51:0] frac;
+        logic [52:0] significand;
+        logic signed [13:0] unbiased_exp;
+        int frac_msb;
+        int shift;
+        logic [127:0] quotient;
+        logic [127:0] remainder;
+        logic [127:0] divisor;
+        logic [127:0] half;
+        logic [24:0] rounded_sig;
+        logic [7:0] exp_s;
+        logic [31:0] result_s;
+
+        flags = '0;
+        sign  = value[63];
+        exp   = value[62:52];
+        frac  = value[51:0];
+
+        if (is_nan_d(value)) begin
+            flags[4] = is_snan_d(value);
+            return {32'hFFFF_FFFF, CANONICAL_NAN_S};
+        end
+        if (&exp) begin
+            return {32'hFFFF_FFFF, sign, 8'hFF, 23'b0};
+        end
+        if ((exp == '0) && (frac == '0)) begin
+            return {32'hFFFF_FFFF, sign, 31'b0};
+        end
+
+        if (exp == '0) begin
+            frac_msb = highest_bit_53({1'b0, frac});
+            unbiased_exp = 14'(frac_msb - 1074);
+            significand = {1'b0, frac} << (52 - frac_msb);
+        end else begin
+            unbiased_exp = $signed({3'b0, exp}) - 14'sd1023;
+            significand = {1'b1, frac};
+        end
+
+        if (unbiased_exp > 14'sd127) begin
+            flags[2] = 1'b1;
+            flags[0] = 1'b1;
+            return {32'hFFFF_FFFF, overflow_s_result(sign, round_mode)};
+        end
+
+        if (unbiased_exp >= -14'sd126) begin
+            exp_s = 8'(unbiased_exp + 14'sd127);
+            shift = 29;
+            divisor = 128'd1 << shift;
+            quotient = {75'b0, significand} >> shift;
+            remainder = {75'b0, significand} & (divisor - 128'd1);
+            half = divisor >> 1;
+            rounded_sig = {1'b0, quotient[23:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 25'd1 : 25'd0);
+            if (remainder != '0) begin
+                flags[0] = 1'b1;
+            end
+            if (rounded_sig[24]) begin
+                rounded_sig = rounded_sig >> 1;
+                exp_s = exp_s + 8'd1;
+                if (exp_s == 8'hFF) begin
+                    flags[2] = 1'b1;
+                    flags[0] = 1'b1;
+                    return {32'hFFFF_FFFF, overflow_s_result(sign, round_mode)};
+                end
+            end
+            result_s = {sign, exp_s, rounded_sig[22:0]};
+            return {32'hFFFF_FFFF, result_s};
+        end
+
+        shift = 29 + ((-126) - int'(unbiased_exp));
+        if (shift >= 128) begin
+            quotient = '0;
+            remainder = {75'b0, significand};
+            rounded_sig = '0;
+            if (remainder != '0) begin
+                flags[1] = 1'b1;
+                flags[0] = 1'b1;
+                unique case (effective_rm(round_mode))
+                    3'b010:  rounded_sig = sign ? 25'd1 : 25'd0;
+                    3'b011:  rounded_sig = !sign ? 25'd1 : 25'd0;
+                    default: rounded_sig = '0;
+                endcase
+            end
+        end else begin
+            divisor = 128'd1 << shift;
+            quotient = {75'b0, significand} >> shift;
+            remainder = {75'b0, significand} & (divisor - 128'd1);
+            half = divisor >> 1;
+            rounded_sig = {1'b0, quotient[23:0]} +
+                          (round_increment(round_mode, sign, quotient, remainder, half) ? 25'd1 : 25'd0);
+            if (remainder != '0) begin
+                flags[1] = 1'b1;
+                flags[0] = 1'b1;
+            end
+        end
+
+        if (rounded_sig[23]) begin
+            result_s = {sign, 8'd1, 23'b0};
+        end else begin
+            result_s = {sign, 8'b0, rounded_sig[22:0]};
+        end
+        return {32'hFFFF_FFFF, result_s};
+    endfunction
+
+    always_comb begin
+        logic invalid_s;
+        logic invalid_d;
+        logic cmp_value;
+        logic [31:0] result_s;
+        logic [63:0] result_d;
+        logic [63:0] result_int;
+        logic [4:0] result_flags;
+
+        fp_result     = '0;
+        int_result    = '0;
+        fflags        = '0;
+        illegal_instr = 1'b0;
+        invalid_s     = is_nan_s(s1) || is_nan_s(s2);
+        invalid_d     = is_nan_d(d1) || is_nan_d(d2);
+        cmp_value     = 1'b0;
+        result_s      = '0;
+        result_d      = '0;
+        result_int    = '0;
+        result_flags  = '0;
+
+        unique case (fp_op)
+            FP_OP_NONE: begin
+                fp_result  = '0;
+                int_result = '0;
+                fflags     = '0;
+            end
+
+            FP_OP_ADD, FP_OP_SUB: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_addsub_s(s1, s2, fp_op == FP_OP_SUB, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_addsub_d(d1, d2, fp_op == FP_OP_SUB, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_MUL: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_mul_s(s1, s2, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_mul_d(d1, d2, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_DIV: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_div_s(s1, s2, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_div_d(d1, d2, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_SQRT: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_s = fp_sqrt_s(s1, rm, result_flags);
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    result_d = fp_sqrt_d(d1, rm, result_flags);
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_MADD, FP_OP_MSUB, FP_OP_NMSUB, FP_OP_NMADD: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    unique case (fp_op)
+                        FP_OP_MADD:  result_s = fp_fma_s(s1, s2, s3, 1'b0, 1'b0, rm, result_flags);
+                        FP_OP_MSUB:  result_s = fp_fma_s(s1, s2, s3, 1'b0, 1'b1, rm, result_flags);
+                        FP_OP_NMSUB: result_s = fp_fma_s(s1, s2, s3, 1'b1, 1'b0, rm, result_flags);
+                        default:     result_s = fp_fma_s(s1, s2, s3, 1'b1, 1'b1, rm, result_flags);
+                    endcase
+                    fp_result = {32'hFFFF_FFFF, result_s};
+                    fflags = result_flags;
+                end else begin
+                    unique case (fp_op)
+                        FP_OP_MADD:  result_d = fp_fma_d(d1, d2, d3, 1'b0, 1'b0, rm, result_flags);
+                        FP_OP_MSUB:  result_d = fp_fma_d(d1, d2, d3, 1'b0, 1'b1, rm, result_flags);
+                        FP_OP_NMSUB: result_d = fp_fma_d(d1, d2, d3, 1'b1, 1'b0, rm, result_flags);
+                        default:     result_d = fp_fma_d(d1, d2, d3, 1'b1, 1'b1, rm, result_flags);
+                    endcase
+                    fp_result = result_d;
+                    fflags = result_flags;
+                end
+            end
+
+            FP_OP_SGNJ, FP_OP_SGNJN, FP_OP_SGNJX: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    unique case (fp_op)
+                        FP_OP_SGNJ:  fp_result = {32'hFFFF_FFFF, s2[31],  s1[30:0]};
+                        FP_OP_SGNJN: fp_result = {32'hFFFF_FFFF, ~s2[31], s1[30:0]};
+                        default:     fp_result = {32'hFFFF_FFFF, s1[31] ^ s2[31], s1[30:0]};
+                    endcase
+                end else begin
+                    unique case (fp_op)
+                        FP_OP_SGNJ:  fp_result = {d2[63],  d1[62:0]};
+                        FP_OP_SGNJN: fp_result = {~d2[63], d1[62:0]};
+                        default:     fp_result = {d1[63] ^ d2[63], d1[62:0]};
+                    endcase
+                end
+            end
+
+            FP_OP_MV_X2F: begin
+                fp_result = (fp_fmt == FP_FMT_S) ? {32'hFFFF_FFFF, int_rs1_data[31:0]} : int_rs1_data;
+            end
+
+            FP_OP_MV_F2X: begin
+                int_result = (fp_fmt == FP_FMT_S) ? {{32{s1_raw[31]}}, s1_raw} : d1;
+            end
+
+            FP_OP_CLASS: begin
+                int_result = (fp_fmt == FP_FMT_S) ? {54'b0, class_s(s1)} : {54'b0, class_d(d1)};
+            end
+
+            FP_OP_EQ, FP_OP_LT, FP_OP_LE: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    if (invalid_s) begin
+                        fflags[4] = is_snan_s(s1) || is_snan_s(s2) || (fp_op != FP_OP_EQ);
+                        cmp_value = 1'b0;
+                    end else begin
+                        cmp_value = (fp_op == FP_OP_EQ) ? equal_s(s1, s2) :
+                                    (fp_op == FP_OP_LT) ? less_s(s1, s2) :
+                                                          (less_s(s1, s2) || equal_s(s1, s2));
+                    end
+                end else begin
+                    if (invalid_d) begin
+                        fflags[4] = is_snan_d(d1) || is_snan_d(d2) || (fp_op != FP_OP_EQ);
+                        cmp_value = 1'b0;
+                    end else begin
+                        cmp_value = (fp_op == FP_OP_EQ) ? equal_d(d1, d2) :
+                                    (fp_op == FP_OP_LT) ? less_d(d1, d2) :
+                                                          (less_d(d1, d2) || equal_d(d1, d2));
+                    end
+                end
+                int_result = {63'b0, cmp_value};
+            end
+
+            FP_OP_MIN, FP_OP_MAX: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    if (is_nan_s(s1) && is_nan_s(s2)) begin
+                        fp_result = {32'hFFFF_FFFF, CANONICAL_NAN_S};
+                        fflags[4] = is_snan_s(s1) || is_snan_s(s2);
+                    end else if (is_nan_s(s1)) begin
+                        fp_result = {32'hFFFF_FFFF, s2};
+                        fflags[4] = is_snan_s(s1);
+                    end else if (is_nan_s(s2)) begin
+                        fp_result = {32'hFFFF_FFFF, s1};
+                        fflags[4] = is_snan_s(s2);
+                    end else if ((s1[30:0] == '0) && (s2[30:0] == '0)) begin
+                        fp_result = {32'hFFFF_FFFF, (fp_op == FP_OP_MIN) ? {s1[31] | s2[31], 31'b0} :
+                                                                     {s1[31] & s2[31], 31'b0}};
+                    end else if (fp_op == FP_OP_MIN) begin
+                        fp_result = {32'hFFFF_FFFF, less_s(s1, s2) ? s1 : s2};
+                    end else begin
+                        fp_result = {32'hFFFF_FFFF, less_s(s1, s2) ? s2 : s1};
+                    end
+                end else begin
+                    if (is_nan_d(d1) && is_nan_d(d2)) begin
+                        fp_result = CANONICAL_NAN_D;
+                        fflags[4] = is_snan_d(d1) || is_snan_d(d2);
+                    end else if (is_nan_d(d1)) begin
+                        fp_result = d2;
+                        fflags[4] = is_snan_d(d1);
+                    end else if (is_nan_d(d2)) begin
+                        fp_result = d1;
+                        fflags[4] = is_snan_d(d2);
+                    end else if ((d1[62:0] == '0) && (d2[62:0] == '0)) begin
+                        fp_result = (fp_op == FP_OP_MIN) ? {d1[63] | d2[63], 63'b0} :
+                                                           {d1[63] & d2[63], 63'b0};
+                    end else if (fp_op == FP_OP_MIN) begin
+                        fp_result = less_d(d1, d2) ? d1 : d2;
+                    end else begin
+                        fp_result = less_d(d1, d2) ? d2 : d1;
+                    end
+                end
+            end
+
+            FP_OP_CVT_F2I: begin
+                if (fp_fmt == FP_FMT_S) begin
+                    result_int = cvt_s_to_int(s1, rs2_addr[1:0], rm, result_flags);
+                end else begin
+                    result_int = cvt_d_to_int(d1, rs2_addr[1:0], rm, result_flags);
+                end
+                int_result = result_int;
+                fflags = result_flags;
+            end
+
+            FP_OP_CVT_I2F: begin
+                result_d = cvt_int_to_fp(int_rs1_data, rs2_addr[1:0], fp_fmt == FP_FMT_D, rm, result_flags);
+                fp_result = result_d;
+                fflags = result_flags;
+            end
+
+            FP_OP_CVT_F2F: begin
+                if (fp_fmt == FP_FMT_D) begin
+                    result_d = cvt_s_to_d(s1, result_flags);
+                    fp_result = result_d;
+                    if (is_nan_s(s1)) begin
+                        result_flags[4] = is_snan_s(s1);
+                    end
+                end else begin
+                    result_d = cvt_d_to_s(d1, rm, result_flags);
+                    fp_result = result_d;
+                    if (is_nan_d(d1)) begin
+                        result_flags[4] = is_snan_d(d1);
+                    end
+                end
+                fflags = result_flags;
+            end
+
+            default: begin
+                fp_result = (fp_fmt == FP_FMT_S) ? {32'hFFFF_FFFF, CANONICAL_NAN_S} : CANONICAL_NAN_D;
+                int_result = '0;
+                fflags[4] = 1'b1;
+            end
+        endcase
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/mmu/mmu.sv b/rv64cpu/rtl/core/mmu/mmu.sv
new file mode 100644
index 0000000..60c4afa
--- /dev/null
+++ b/rv64cpu/rtl/core/mmu/mmu.sv
@@ -0,0 +1,359 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/mmu/mmu_top.sv
+// Description: MMU top-level integrating iTLB + dTLB + shared PTW
+
+module mmu
+	import rv64_pkg::*;
+#(
+	parameter int ITLB_ENTRIES = 16,
+	parameter int DTLB_ENTRIES = 32,
+	parameter int ASID_WIDTH = 16
+)(
+	input  logic                  clk,
+	input  logic                  rst,
+
+	// CSR/context inputs
+	input  logic [XLEN-1:0]       satp_value,
+	input  logic [1:0]            priv_mode,
+	input  logic                  sum,
+	input  logic                  mxr,
+
+	// i-side translation request
+	input  logic                  i_req_valid,
+	output logic                  i_req_ready,
+	input  logic [XLEN-1:0]       i_req_vaddr,
+	output logic                  i_resp_valid,
+	output logic                  i_resp_hit,
+	output logic                  i_resp_page_fault,
+	output logic [XLEN-1:0]       i_resp_vaddr,
+	output logic [XLEN-1:0]       i_resp_paddr,
+
+	// d-side translation request
+	input  logic                  d_req_valid,
+	output logic                  d_req_ready,
+	input  logic [XLEN-1:0]       d_req_vaddr,
+	input  logic                  d_req_is_store,
+	output logic                  d_resp_valid,
+	output logic                  d_resp_hit,
+	output logic                  d_resp_page_fault,
+	output logic [XLEN-1:0]       d_resp_paddr,
+
+	// sfence / invalidation
+	input  logic                  flush_all,
+	input  logic                  flush_asid_valid,
+	input  logic [ASID_WIDTH-1:0] flush_asid,
+	input  logic                  flush_vpn_valid,
+	input  logic [44:0]           flush_vpn,
+
+	// PTW memory interface
+	output logic                  ptw_mem_req_valid,
+	input  logic                  ptw_mem_req_ready,
+	output logic [XLEN-1:0]       ptw_mem_req_addr,
+	input  logic                  ptw_mem_resp_valid,
+	input  logic [XLEN-1:0]       ptw_mem_resp_data
+);
+
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+
+	typedef enum logic [0:0] {
+		PTW_SRC_I = 1'b0,
+		PTW_SRC_D = 1'b1
+	} ptw_src_t;
+
+	logic [ASID_WIDTH-1:0] req_asid;
+	logic [3:0]            req_mode;
+
+	logic                  i_tlb_resp_valid;
+	logic                  i_tlb_resp_hit;
+	logic [XLEN-1:0]       i_tlb_resp_paddr;
+	logic                  i_tlb_resp_page_fault;
+	logic                  i_tlb_resp_need_ptw;
+
+	logic                  d_tlb_resp_valid;
+	logic                  d_tlb_resp_hit;
+	logic [XLEN-1:0]       d_tlb_resp_paddr;
+	logic                  d_tlb_resp_page_fault;
+	logic                  d_tlb_resp_need_ptw;
+
+	logic                  i_pending_q;
+	logic                  d_pending_q;
+	logic [XLEN-1:0]       i_pending_vaddr_q;
+	logic [XLEN-1:0]       d_pending_vaddr_q;
+	logic [1:0]            i_pending_priv_q;
+	logic [1:0]            d_pending_priv_q;
+	logic                  i_pending_sum_q;
+	logic                  d_pending_sum_q;
+	logic                  i_pending_mxr_q;
+	logic                  d_pending_mxr_q;
+	logic [ASID_WIDTH-1:0] i_pending_asid_q;
+	logic [ASID_WIDTH-1:0] d_pending_asid_q;
+	logic [3:0]            i_pending_mode_q;
+	logic [3:0]            d_pending_mode_q;
+	logic                  d_pending_is_store_q;
+
+	logic                  ptw_req_valid;
+	logic                  ptw_req_ready;
+	logic                  ptw_resp_valid;
+	logic                  ptw_resp_fault;
+	logic [XLEN-1:0]       ptw_resp_paddr;
+
+	logic                  ptw_refill_valid;
+	logic [44:0]           ptw_refill_vpn;
+	logic [43:0]           ptw_refill_ppn;
+	logic [2:0]            ptw_refill_level;
+	logic [3:0]            ptw_refill_mode;
+	logic [ASID_WIDTH-1:0] ptw_refill_asid;
+	logic                  ptw_refill_g;
+	logic                  ptw_refill_u;
+	logic                  ptw_refill_r;
+	logic                  ptw_refill_w;
+	logic                  ptw_refill_x;
+	logic                  ptw_refill_a;
+	logic                  ptw_refill_d;
+
+	logic                  ptw_busy_q;
+	ptw_src_t              ptw_owner_q;
+	ptw_src_t              ptw_owner_next;
+
+	logic                  i_accept;
+	logic                  d_accept;
+	logic                  i_fast_resp_valid;
+	logic                  d_fast_resp_valid;
+	logic                  i_ptw_resp_sel;
+	logic                  d_ptw_resp_sel;
+
+	logic                  i_refill_valid;
+	logic                  d_refill_valid;
+
+	assign req_mode = satp_value[63:60];
+	assign req_asid = satp_value[59 -: ASID_WIDTH];
+
+	assign i_req_ready = !i_pending_q;
+	assign d_req_ready = !d_pending_q;
+	assign i_accept = i_req_valid && i_req_ready;
+	assign d_accept = d_req_valid && d_req_ready;
+
+	assign i_fast_resp_valid = i_accept && (i_tlb_resp_hit || i_tlb_resp_page_fault);
+	assign d_fast_resp_valid = d_accept && (d_tlb_resp_hit || d_tlb_resp_page_fault);
+
+	assign i_ptw_resp_sel = ptw_resp_valid && (ptw_owner_q == PTW_SRC_I);
+	assign d_ptw_resp_sel = ptw_resp_valid && (ptw_owner_q == PTW_SRC_D);
+
+	assign i_resp_valid = i_fast_resp_valid || i_ptw_resp_sel;
+	assign i_resp_hit = i_fast_resp_valid ? i_tlb_resp_hit : (i_ptw_resp_sel && !ptw_resp_fault);
+	assign i_resp_page_fault = i_fast_resp_valid ? i_tlb_resp_page_fault : (i_ptw_resp_sel && ptw_resp_fault);
+	assign i_resp_vaddr = i_fast_resp_valid ? i_req_vaddr : i_pending_vaddr_q;
+	assign i_resp_paddr = i_fast_resp_valid ? i_tlb_resp_paddr : ptw_resp_paddr;
+
+	assign d_resp_valid = d_fast_resp_valid || d_ptw_resp_sel;
+	assign d_resp_hit = d_fast_resp_valid ? d_tlb_resp_hit : (d_ptw_resp_sel && !ptw_resp_fault);
+	assign d_resp_page_fault = d_fast_resp_valid ? d_tlb_resp_page_fault : (d_ptw_resp_sel && ptw_resp_fault);
+	assign d_resp_paddr = d_fast_resp_valid ? d_tlb_resp_paddr : ptw_resp_paddr;
+
+	always_comb begin
+		if (d_pending_q) begin
+			ptw_owner_next = PTW_SRC_D;
+		end else begin
+			ptw_owner_next = PTW_SRC_I;
+		end
+	end
+
+	assign ptw_req_valid = !ptw_busy_q && (d_pending_q || i_pending_q);
+
+	assign i_refill_valid = ptw_refill_valid && (ptw_owner_q == PTW_SRC_I);
+	assign d_refill_valid = ptw_refill_valid && (ptw_owner_q == PTW_SRC_D);
+
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			i_pending_q <= 1'b0;
+			d_pending_q <= 1'b0;
+			ptw_busy_q <= 1'b0;
+			ptw_owner_q <= PTW_SRC_I;
+			i_pending_vaddr_q <= '0;
+			d_pending_vaddr_q <= '0;
+			i_pending_priv_q <= '0;
+			d_pending_priv_q <= '0;
+			i_pending_sum_q <= 1'b0;
+			d_pending_sum_q <= 1'b0;
+			i_pending_mxr_q <= 1'b0;
+			d_pending_mxr_q <= 1'b0;
+			i_pending_asid_q <= '0;
+			d_pending_asid_q <= '0;
+			i_pending_mode_q <= SATP_MODE_BARE;
+			d_pending_mode_q <= SATP_MODE_BARE;
+			d_pending_is_store_q <= 1'b0;
+		end else begin
+			if (i_accept && i_tlb_resp_need_ptw) begin
+				i_pending_q <= 1'b1;
+				i_pending_vaddr_q <= i_req_vaddr;
+				i_pending_priv_q <= priv_mode;
+				i_pending_sum_q <= sum;
+				i_pending_mxr_q <= mxr;
+				i_pending_asid_q <= req_asid;
+				i_pending_mode_q <= req_mode;
+			end
+
+			if (d_accept && d_tlb_resp_need_ptw) begin
+				d_pending_q <= 1'b1;
+				d_pending_vaddr_q <= d_req_vaddr;
+				d_pending_priv_q <= priv_mode;
+				d_pending_sum_q <= sum;
+				d_pending_mxr_q <= mxr;
+				d_pending_asid_q <= req_asid;
+				d_pending_mode_q <= req_mode;
+				d_pending_is_store_q <= d_req_is_store;
+			end
+
+			if (ptw_req_valid && ptw_req_ready) begin
+				ptw_busy_q <= 1'b1;
+				ptw_owner_q <= ptw_owner_next;
+			end
+
+			if (ptw_resp_valid) begin
+				ptw_busy_q <= 1'b0;
+				if (ptw_owner_q == PTW_SRC_I) begin
+					i_pending_q <= 1'b0;
+				end else begin
+					d_pending_q <= 1'b0;
+				end
+			end
+		end
+	end
+
+	tlb #(
+		.ENTRIES(ITLB_ENTRIES),
+		.ASID_WIDTH(ASID_WIDTH)
+	) u_itlb (
+		.clk              (clk),
+		.rst              (rst),
+		.req_valid        (i_accept),
+		.req_vaddr        (i_req_vaddr),
+		.req_asid         (req_asid),
+		.req_mode         (req_mode),
+		.req_priv         (priv_mode),
+		.req_is_fetch     (1'b1),
+		.req_is_store     (1'b0),
+		.req_sum          (sum),
+		.req_mxr          (mxr),
+		.resp_valid       (i_tlb_resp_valid),
+		.resp_hit         (i_tlb_resp_hit),
+		.resp_paddr       (i_tlb_resp_paddr),
+		.resp_page_fault  (i_tlb_resp_page_fault),
+		.resp_need_ptw    (i_tlb_resp_need_ptw),
+		.refill_valid     (i_refill_valid),
+		.refill_vpn       (ptw_refill_vpn),
+		.refill_ppn       (ptw_refill_ppn),
+		.refill_level     (ptw_refill_level),
+		.refill_mode      (ptw_refill_mode),
+		.refill_asid      (ptw_refill_asid),
+		.refill_g         (ptw_refill_g),
+		.refill_u         (ptw_refill_u),
+		.refill_r         (ptw_refill_r),
+		.refill_w         (ptw_refill_w),
+		.refill_x         (ptw_refill_x),
+		.refill_a         (ptw_refill_a),
+		.refill_d         (ptw_refill_d),
+		.flush_all        (flush_all),
+		.flush_asid_valid (flush_asid_valid),
+		.flush_asid       (flush_asid),
+		.flush_vpn_valid  (flush_vpn_valid),
+		.flush_vpn        (flush_vpn),
+		.flush_mode       (req_mode)
+	);
+
+	tlb #(
+		.ENTRIES(DTLB_ENTRIES),
+		.ASID_WIDTH(ASID_WIDTH)
+	) u_dtlb (
+		.clk              (clk),
+		.rst              (rst),
+		.req_valid        (d_accept),
+		.req_vaddr        (d_req_vaddr),
+		.req_asid         (req_asid),
+		.req_mode         (req_mode),
+		.req_priv         (priv_mode),
+		.req_is_fetch     (1'b0),
+		.req_is_store     (d_req_is_store),
+		.req_sum          (sum),
+		.req_mxr          (mxr),
+		.resp_valid       (d_tlb_resp_valid),
+		.resp_hit         (d_tlb_resp_hit),
+		.resp_paddr       (d_tlb_resp_paddr),
+		.resp_page_fault  (d_tlb_resp_page_fault),
+		.resp_need_ptw    (d_tlb_resp_need_ptw),
+		.refill_valid     (d_refill_valid),
+		.refill_vpn       (ptw_refill_vpn),
+		.refill_ppn       (ptw_refill_ppn),
+		.refill_level     (ptw_refill_level),
+		.refill_mode      (ptw_refill_mode),
+		.refill_asid      (ptw_refill_asid),
+		.refill_g         (ptw_refill_g),
+		.refill_u         (ptw_refill_u),
+		.refill_r         (ptw_refill_r),
+		.refill_w         (ptw_refill_w),
+		.refill_x         (ptw_refill_x),
+		.refill_a         (ptw_refill_a),
+		.refill_d         (ptw_refill_d),
+		.flush_all        (flush_all),
+		.flush_asid_valid (flush_asid_valid),
+		.flush_asid       (flush_asid),
+		.flush_vpn_valid  (flush_vpn_valid),
+		.flush_vpn        (flush_vpn),
+		.flush_mode       (req_mode)
+	);
+
+	ptw #(
+		.ASID_WIDTH(ASID_WIDTH)
+	) u_ptw (
+		.clk           (clk),
+		.rst           (rst),
+		.req_valid     (ptw_req_valid),
+		.req_ready     (ptw_req_ready),
+		.req_vaddr     ((ptw_owner_next == PTW_SRC_D) ? d_pending_vaddr_q : i_pending_vaddr_q),
+		.req_asid      ((ptw_owner_next == PTW_SRC_D) ? d_pending_asid_q : i_pending_asid_q),
+		.req_mode      ((ptw_owner_next == PTW_SRC_D) ? d_pending_mode_q : i_pending_mode_q),
+		.req_priv      ((ptw_owner_next == PTW_SRC_D) ? d_pending_priv_q : i_pending_priv_q),
+		.req_is_fetch  (ptw_owner_next == PTW_SRC_I),
+		.req_is_store  ((ptw_owner_next == PTW_SRC_D) ? d_pending_is_store_q : 1'b0),
+		.req_sum       ((ptw_owner_next == PTW_SRC_D) ? d_pending_sum_q : i_pending_sum_q),
+		.req_mxr       ((ptw_owner_next == PTW_SRC_D) ? d_pending_mxr_q : i_pending_mxr_q),
+		.satp_value    (satp_value),
+		.mem_req_valid (ptw_mem_req_valid),
+		.mem_req_ready (ptw_mem_req_ready),
+		.mem_req_addr  (ptw_mem_req_addr),
+		.mem_resp_valid(ptw_mem_resp_valid),
+		.mem_resp_data (ptw_mem_resp_data),
+		.resp_valid    (ptw_resp_valid),
+		.resp_fault    (ptw_resp_fault),
+		.resp_paddr    (ptw_resp_paddr),
+		.refill_valid  (ptw_refill_valid),
+		.refill_vpn    (ptw_refill_vpn),
+		.refill_ppn    (ptw_refill_ppn),
+		.refill_level  (ptw_refill_level),
+		.refill_mode   (ptw_refill_mode),
+		.refill_asid   (ptw_refill_asid),
+		.refill_g      (ptw_refill_g),
+		.refill_u      (ptw_refill_u),
+		.refill_r      (ptw_refill_r),
+		.refill_w      (ptw_refill_w),
+		.refill_x      (ptw_refill_x),
+		.refill_a      (ptw_refill_a),
+		.refill_d      (ptw_refill_d)
+	);
+
+endmodule
diff --git a/rv64cpu/rtl/core/mmu/ptw.sv b/rv64cpu/rtl/core/mmu/ptw.sv
new file mode 100644
index 0000000..6447b5a
--- /dev/null
+++ b/rv64cpu/rtl/core/mmu/ptw.sv
@@ -0,0 +1,416 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/mmu/ptw.sv
+// Description: Page-table walker for Sv39. Sv48/Sv57/Sv64 are explicit stubs.
+
+module ptw
+	import rv64_pkg::*;
+#(
+	parameter int ASID_WIDTH = 16
+)(
+	input  logic                   clk,
+	input  logic                   rst,
+
+	input  logic                   req_valid,
+	output logic                   req_ready,
+	input  logic [XLEN-1:0]        req_vaddr,
+	input  logic [ASID_WIDTH-1:0]  req_asid,
+	input  logic [3:0]             req_mode,
+	input  logic [1:0]             req_priv,
+	input  logic                   req_is_fetch,
+	input  logic                   req_is_store,
+	input  logic                   req_sum,
+	input  logic                   req_mxr,
+	input  logic [XLEN-1:0]        satp_value,
+
+	output logic                   mem_req_valid,
+	input  logic                   mem_req_ready,
+	output logic [XLEN-1:0]        mem_req_addr,
+	input  logic                   mem_resp_valid,
+	input  logic [XLEN-1:0]        mem_resp_data,
+
+	output logic                   resp_valid,
+	output logic                   resp_fault,
+	output logic [XLEN-1:0]        resp_paddr,
+
+	output logic                   refill_valid,
+	output logic [44:0]            refill_vpn,
+	output logic [43:0]            refill_ppn,
+	output logic [2:0]             refill_level,
+	output logic [3:0]             refill_mode,
+	output logic [ASID_WIDTH-1:0]  refill_asid,
+	output logic                   refill_g,
+	output logic                   refill_u,
+	output logic                   refill_r,
+	output logic                   refill_w,
+	output logic                   refill_x,
+	output logic                   refill_a,
+	output logic                   refill_d
+);
+
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+	localparam logic [3:0] SATP_MODE_SV39 = 4'd8;
+	localparam logic [3:0] SATP_MODE_SV48 = 4'd9;
+	localparam logic [3:0] SATP_MODE_SV57 = 4'd10;
+	localparam logic [3:0] SATP_MODE_SV64 = 4'd11; // Reserved/future stub.
+
+	typedef enum logic [2:0] {
+		PTW_IDLE,
+		PTW_ISSUE,
+		PTW_WAIT,
+		PTW_DONE
+	} ptw_state_t;
+
+	ptw_state_t state_q;
+
+	logic [XLEN-1:0]        req_vaddr_q;
+	logic [ASID_WIDTH-1:0]  req_asid_q;
+	logic [3:0]             req_mode_q;
+	logic [1:0]             req_priv_q;
+	logic                   req_is_fetch_q;
+	logic                   req_is_store_q;
+	logic                   req_sum_q;
+	logic                   req_mxr_q;
+
+	logic [43:0]            table_ppn_q;
+	logic [2:0]             level_q;
+
+	logic                   pte_v;
+	logic                   pte_r;
+	logic                   pte_w;
+	logic                   pte_x;
+	logic                   pte_u;
+	logic                   pte_g;
+	logic                   pte_a;
+	logic                   pte_d;
+	logic [43:0]            pte_ppn;
+
+	logic                   leaf_pte;
+	logic                   bad_pte;
+	logic                   superpage_misaligned;
+
+	logic [44:0]            req_vpn_q;
+	logic [XLEN-1:0]        next_paddr;
+
+	logic                   resp_fault_q;
+	logic [XLEN-1:0]        resp_paddr_q;
+	logic                   refill_valid_q;
+
+	logic [44:0]            refill_vpn_q;
+	logic [43:0]            refill_ppn_q;
+	logic [2:0]             refill_level_q;
+	logic [3:0]             refill_mode_q;
+	logic [ASID_WIDTH-1:0]  refill_asid_q;
+	logic                   refill_g_q;
+	logic                   refill_u_q;
+	logic                   refill_r_q;
+	logic                   refill_w_q;
+	logic                   refill_x_q;
+	logic                   refill_a_q;
+	logic                   refill_d_q;
+
+	function automatic logic is_supported_mode(input logic [3:0] mode);
+		return (mode == SATP_MODE_SV39);
+	endfunction
+
+	function automatic int vpn_stride(input logic [3:0] mode);
+		case (mode)
+			SATP_MODE_SV39: vpn_stride = 9;
+			default:        vpn_stride = 0;
+		endcase
+	endfunction
+
+	function automatic logic [44:0] vaddr_to_vpn(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		logic [44:0] vpn;
+		begin
+			vpn = '0;
+			case (mode)
+				SATP_MODE_SV39: vpn[26:0] = vaddr[38:12];
+				default:         vpn = '0;
+			endcase
+			return vpn;
+		end
+	endfunction
+
+	function automatic logic [9:0] vpn_index(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		logic [9:0] idx;
+		begin
+			idx = '0;
+			case (mode)
+				SATP_MODE_SV39: idx = {1'b0, vaddr[12 + level * 9 +: 9]};
+				default: idx = '0;
+			endcase
+			return idx;
+		end
+	endfunction
+
+	function automatic logic vaddr_is_canonical(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		case (mode)
+			SATP_MODE_SV39: vaddr_is_canonical = (vaddr[63:39] == {25{vaddr[38]}});
+			default:        vaddr_is_canonical = 1'b1;
+		endcase
+	endfunction
+
+	function automatic logic [5:0] page_shift(
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		int stride;
+		begin
+			stride = vpn_stride(mode);
+			return 6'(12 + level * stride);
+		end
+	endfunction
+
+	function automatic logic allow_access(
+		input logic is_fetch,
+		input logic is_store,
+		input logic [1:0] priv,
+		input logic sum,
+		input logic mxr,
+		input logic u,
+		input logic r,
+		input logic w,
+		input logic x,
+		input logic a,
+		input logic d
+	);
+		logic is_load;
+		logic readable;
+		logic priv_u;
+		logic priv_ok;
+		begin
+			is_load = ~is_fetch && ~is_store;
+			readable = r || (mxr && x);
+			priv_u = (priv == 2'b00);
+
+			if (u) begin
+				if (priv_u) begin
+					priv_ok = 1'b1;
+				end else begin
+					priv_ok = ~is_fetch && sum;
+				end
+			end else begin
+				priv_ok = ~priv_u;
+			end
+
+			return a && priv_ok && (
+				(is_fetch && x) ||
+				(is_store && w && d) ||
+				(is_load && readable)
+			);
+		end
+	endfunction
+
+	assign pte_v = mem_resp_data[0];
+	assign pte_r = mem_resp_data[1];
+	assign pte_w = mem_resp_data[2];
+	assign pte_x = mem_resp_data[3];
+	assign pte_u = mem_resp_data[4];
+	assign pte_g = mem_resp_data[5];
+	assign pte_a = mem_resp_data[6];
+	assign pte_d = mem_resp_data[7];
+	assign pte_ppn = mem_resp_data[53:10];
+
+	assign leaf_pte = pte_r || pte_x;
+	assign bad_pte = (!pte_v) || (pte_w && !pte_r);
+
+	always_comb begin
+		int stride;
+		int lower_bits;
+		logic [63:0] ppn_mask;
+		stride = vpn_stride(req_mode_q);
+		lower_bits = level_q * stride;
+		ppn_mask = '0;
+		superpage_misaligned = 1'b0;
+		if (leaf_pte && (lower_bits > 0)) begin
+			if (lower_bits >= 44) begin
+				ppn_mask = 64'h0000_0FFF_FFFF_FFFF;
+			end else begin
+				ppn_mask = (64'd1 << lower_bits) - 1;
+			end
+			superpage_misaligned = |({20'b0, pte_ppn} & ppn_mask);
+		end
+	end
+
+	always_comb begin
+		logic [5:0] shift;
+		logic [XLEN-1:0] low_mask;
+		int shift_index;
+		shift = page_shift(req_mode_q, level_q);
+		shift_index = int'(shift);
+		if (shift_index >= XLEN) begin
+			low_mask = {XLEN{1'b1}};
+		end else begin
+			low_mask = ({{(XLEN-1){1'b0}}, 1'b1} << shift_index) - 1;
+		end
+		next_paddr = (({{(XLEN-56){1'b0}}, pte_ppn, 12'b0}) & ~low_mask) | (req_vaddr_q & low_mask);
+	end
+
+	assign req_ready = (state_q == PTW_IDLE);
+	assign mem_req_valid = (state_q == PTW_ISSUE);
+	assign mem_req_addr = {{(XLEN-56){1'b0}}, table_ppn_q, 12'b0} +
+						 ({{(XLEN-10){1'b0}}, vpn_index(req_vaddr_q, req_mode_q, level_q)} << 3);
+
+	assign resp_valid = (state_q == PTW_DONE);
+	assign resp_fault = resp_fault_q;
+	assign resp_paddr = resp_paddr_q;
+
+	assign refill_valid = refill_valid_q && (state_q == PTW_DONE);
+	assign refill_vpn = refill_vpn_q;
+	assign refill_ppn = refill_ppn_q;
+	assign refill_level = refill_level_q;
+	assign refill_mode = refill_mode_q;
+	assign refill_asid = refill_asid_q;
+	assign refill_g = refill_g_q;
+	assign refill_u = refill_u_q;
+	assign refill_r = refill_r_q;
+	assign refill_w = refill_w_q;
+	assign refill_x = refill_x_q;
+	assign refill_a = refill_a_q;
+	assign refill_d = refill_d_q;
+
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			state_q <= PTW_IDLE;
+			req_vaddr_q <= '0;
+			req_asid_q <= '0;
+			req_mode_q <= SATP_MODE_BARE;
+			req_priv_q <= '0;
+			req_is_fetch_q <= 1'b0;
+			req_is_store_q <= 1'b0;
+			req_sum_q <= 1'b0;
+			req_mxr_q <= 1'b0;
+			table_ppn_q <= '0;
+			level_q <= '0;
+			req_vpn_q <= '0;
+			resp_fault_q <= 1'b0;
+			resp_paddr_q <= '0;
+			refill_valid_q <= 1'b0;
+			refill_vpn_q <= '0;
+			refill_ppn_q <= '0;
+			refill_level_q <= '0;
+			refill_mode_q <= SATP_MODE_BARE;
+			refill_asid_q <= '0;
+			refill_g_q <= 1'b0;
+			refill_u_q <= 1'b0;
+			refill_r_q <= 1'b0;
+			refill_w_q <= 1'b0;
+			refill_x_q <= 1'b0;
+			refill_a_q <= 1'b0;
+			refill_d_q <= 1'b0;
+		end else begin
+			case (state_q)
+				PTW_IDLE: begin
+					refill_valid_q <= 1'b0;
+					if (req_valid) begin
+						req_vaddr_q <= req_vaddr;
+						req_asid_q <= req_asid;
+						req_mode_q <= req_mode;
+						req_priv_q <= req_priv;
+						req_is_fetch_q <= req_is_fetch;
+						req_is_store_q <= req_is_store;
+						req_sum_q <= req_sum;
+						req_mxr_q <= req_mxr;
+						req_vpn_q <= vaddr_to_vpn(req_vaddr, req_mode);
+
+						if (!is_supported_mode(req_mode) || !vaddr_is_canonical(req_vaddr, req_mode)) begin
+							resp_fault_q <= 1'b1;
+							resp_paddr_q <= '0;
+							state_q <= PTW_DONE;
+						end else begin
+							table_ppn_q <= satp_value[43:0];
+							case (req_mode)
+								SATP_MODE_SV39: level_q <= 3'd2;
+								default:        level_q <= '0;
+							endcase
+							state_q <= PTW_ISSUE;
+						end
+					end
+				end
+
+				PTW_ISSUE: begin
+					if (mem_req_ready) begin
+						state_q <= PTW_WAIT;
+					end
+				end
+
+				PTW_WAIT: begin
+					if (mem_resp_valid) begin
+						if (bad_pte) begin
+							resp_fault_q <= 1'b1;
+							resp_paddr_q <= '0;
+							refill_valid_q <= 1'b0;
+							state_q <= PTW_DONE;
+						end else if (leaf_pte) begin
+							if (superpage_misaligned ||
+								!allow_access(req_is_fetch_q, req_is_store_q, req_priv_q, req_sum_q, req_mxr_q,
+												  pte_u, pte_r, pte_w, pte_x, pte_a, pte_d)) begin
+								resp_fault_q <= 1'b1;
+								resp_paddr_q <= '0;
+								refill_valid_q <= 1'b0;
+							end else begin
+								resp_fault_q <= 1'b0;
+								resp_paddr_q <= next_paddr;
+								refill_valid_q <= 1'b1;
+								refill_vpn_q <= req_vpn_q;
+								refill_ppn_q <= pte_ppn;
+								refill_level_q <= level_q;
+								refill_mode_q <= req_mode_q;
+								refill_asid_q <= req_asid_q;
+								refill_g_q <= pte_g;
+								refill_u_q <= pte_u;
+								refill_r_q <= pte_r;
+								refill_w_q <= pte_w;
+								refill_x_q <= pte_x;
+								refill_a_q <= pte_a;
+								refill_d_q <= pte_d;
+							end
+							state_q <= PTW_DONE;
+						end else if (level_q == 0) begin
+							resp_fault_q <= 1'b1;
+							resp_paddr_q <= '0;
+							refill_valid_q <= 1'b0;
+							state_q <= PTW_DONE;
+						end else begin
+							table_ppn_q <= pte_ppn;
+							level_q <= level_q - 1'b1;
+							state_q <= PTW_ISSUE;
+						end
+					end
+				end
+
+				PTW_DONE: begin
+					state_q <= PTW_IDLE;
+				end
+
+				default: state_q <= PTW_IDLE;
+			endcase
+		end
+	end
+
+endmodule
diff --git a/rv64cpu/rtl/core/mmu/tlb.sv b/rv64cpu/rtl/core/mmu/tlb.sv
new file mode 100644
index 0000000..06eb947
--- /dev/null
+++ b/rv64cpu/rtl/core/mmu/tlb.sv
@@ -0,0 +1,382 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/mmu/tlb.sv
+// Description: Parameterized L1 TLB (Bare + Sv39, higher modes stubbed)
+
+module tlb
+	import rv64_pkg::*;
+#(
+	parameter int ENTRIES = 32,
+	parameter int ASID_WIDTH = 16
+)(
+	input  logic                   clk,
+	input  logic                   rst,
+
+	// Translation request
+	input  logic                   req_valid,
+	input  logic [XLEN-1:0]        req_vaddr,
+	input  logic [ASID_WIDTH-1:0]  req_asid,
+	input  logic [3:0]             req_mode,       // satp.MODE
+	input  logic [1:0]             req_priv,       // 0=U,1=S,3=M
+	input  logic                   req_is_fetch,
+	input  logic                   req_is_store,
+	input  logic                   req_sum,
+	input  logic                   req_mxr,
+
+	// Translation response
+	output logic                   resp_valid,
+	output logic                   resp_hit,
+	output logic [XLEN-1:0]        resp_paddr,
+	output logic                   resp_page_fault,
+	output logic                   resp_need_ptw,
+
+	// Refill from PTW
+	input  logic                   refill_valid,
+	input  logic [44:0]            refill_vpn,     // normalized VPN, max Sv57 width
+	input  logic [43:0]            refill_ppn,
+	input  logic [2:0]             refill_level,   // 0=4KB leaf, 1=next superpage ...
+	input  logic [3:0]             refill_mode,
+	input  logic [ASID_WIDTH-1:0]  refill_asid,
+	input  logic                   refill_g,
+	input  logic                   refill_u,
+	input  logic                   refill_r,
+	input  logic                   refill_w,
+	input  logic                   refill_x,
+	input  logic                   refill_a,
+	input  logic                   refill_d,
+
+	// sfence / invalidation hooks
+	input  logic                   flush_all,
+	input  logic                   flush_asid_valid,
+	input  logic [ASID_WIDTH-1:0]  flush_asid,
+	input  logic                   flush_vpn_valid,
+	input  logic [44:0]            flush_vpn,
+	input  logic [3:0]             flush_mode
+);
+
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+	localparam logic [3:0] SATP_MODE_SV39 = 4'd8;
+	localparam logic [3:0] SATP_MODE_SV48 = 4'd9;
+	localparam logic [3:0] SATP_MODE_SV57 = 4'd10;
+	localparam logic [3:0] SATP_MODE_SV64 = 4'd11; // Reserved/future stub.
+
+	typedef struct packed {
+		logic                   valid;
+		logic [3:0]             mode;
+		logic [ASID_WIDTH-1:0]  asid;
+		logic [44:0]            vpn;
+		logic [43:0]            ppn;
+		logic [2:0]             level;
+		logic                   g;
+		logic                   u;
+		logic                   r;
+		logic                   w;
+		logic                   x;
+		logic                   a;
+		logic                   d;
+	} tlb_entry_t;
+
+	tlb_entry_t entries [0:ENTRIES-1];
+	logic [$clog2(ENTRIES)-1:0] repl_ptr;
+
+	logic [44:0] req_vpn;
+	logic [43:0] hit_ppn;
+	logic [2:0]  hit_level;
+	logic [3:0]  hit_mode;
+	logic        hit_u, hit_r, hit_w, hit_x, hit_a, hit_d;
+	logic        hit_found;
+	logic [5:0]  shift;
+	logic [XLEN-1:0] low_mask;
+	logic        req_priv_u;
+	logic [XLEN-1:0] hit_ppn_xlen;
+	int unsigned shift_index;
+
+	function automatic logic is_supported_mode(input logic [3:0] mode);
+		return (mode == SATP_MODE_BARE) || (mode == SATP_MODE_SV39);
+	endfunction
+
+	function automatic int vpn_width(input logic [3:0] mode);
+		case (mode)
+			SATP_MODE_SV39: vpn_width = 27;
+			default:        vpn_width = 0;
+		endcase
+	endfunction
+
+	function automatic int vpn_stride(input logic [3:0] mode);
+		case (mode)
+			SATP_MODE_SV39: vpn_stride = 9;
+			default:        vpn_stride = 0;
+		endcase
+	endfunction
+
+	function automatic logic [44:0] make_vpn_mask(
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		logic [44:0] mask;
+		int width;
+		int stride;
+		int ignore;
+		int i;
+		begin
+			mask = '0;
+			width = vpn_width(mode);
+			stride = vpn_stride(mode);
+			ignore = level * stride;
+
+			if (width > 0) begin
+				for (i = ignore; i < width; i++) begin
+					mask[i] = 1'b1;
+				end
+			end
+
+			make_vpn_mask = mask;
+		end
+	endfunction
+
+	function automatic logic [44:0] vaddr_to_vpn(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		logic [44:0] vpn;
+		begin
+			vpn = '0;
+			case (mode)
+				SATP_MODE_SV39: vpn[26:0] = vaddr[38:12];
+				default:         vpn = '0;
+			endcase
+			vaddr_to_vpn = vpn;
+		end
+	endfunction
+
+	function automatic logic vaddr_is_canonical(
+		input logic [XLEN-1:0] vaddr,
+		input logic [3:0] mode
+	);
+		case (mode)
+			SATP_MODE_SV39: vaddr_is_canonical = (vaddr[63:39] == {25{vaddr[38]}});
+			default:        vaddr_is_canonical = 1'b1;
+		endcase
+	endfunction
+
+	function automatic logic [5:0] page_shift(
+		input logic [3:0] mode,
+		input logic [2:0] level
+	);
+		int stride;
+		begin
+			stride = vpn_stride(mode);
+			page_shift = 6'(12 + level * stride);
+		end
+	endfunction
+
+	function automatic logic entry_match(
+		input tlb_entry_t e,
+		input logic [44:0] vpn,
+		input logic [ASID_WIDTH-1:0] asid,
+		input logic [3:0] mode
+	);
+		logic [44:0] mask;
+		begin
+			mask = make_vpn_mask(e.mode, e.level);
+			entry_match = e.valid && (e.mode == mode) && (e.g || (e.asid == asid)) &&
+						  ((((e.vpn ^ vpn) & mask) == '0));
+		end
+	endfunction
+
+	function automatic logic allow_access(
+		input logic is_fetch,
+		input logic is_store,
+		input logic priv_u,
+		input logic sum,
+		input logic mxr,
+		input logic u,
+		input logic r,
+		input logic w,
+		input logic x,
+		input logic a,
+		input logic d
+	);
+		logic is_load;
+		logic readable;
+		logic priv_ok;
+		begin
+			is_load = ~is_fetch && ~is_store;
+			readable = r || (mxr && x);
+
+			// U=1 page: only U-mode unless SUM permits S-mode data access
+			// U=0 page: S/M-mode only
+			if (u) begin
+				if (priv_u) begin
+					priv_ok = 1'b1;
+				end else begin
+					priv_ok = ~is_fetch && sum;
+				end
+			end else begin
+				priv_ok = ~priv_u;
+			end
+
+			allow_access = a && priv_ok && (
+				(is_fetch && x) ||
+				(is_store && w && d) ||
+				(is_load  && readable)
+			);
+		end
+	endfunction
+
+	integer k;
+	always_comb begin
+		req_vpn = vaddr_to_vpn(req_vaddr, req_mode);
+
+		resp_valid = req_valid;
+		resp_hit = 1'b0;
+		resp_paddr = req_vaddr;
+		resp_page_fault = 1'b0;
+		resp_need_ptw = 1'b0;
+
+		shift = '0;
+		low_mask = '0;
+		req_priv_u = 1'b0;
+		hit_ppn_xlen = '0;
+		shift_index = 0;
+
+		hit_found = 1'b0;
+		hit_ppn = '0;
+		hit_level = '0;
+		hit_mode = '0;
+		hit_u = 1'b0;
+		hit_r = 1'b0;
+		hit_w = 1'b0;
+		hit_x = 1'b0;
+		hit_a = 1'b0;
+		hit_d = 1'b0;
+
+		if (req_valid) begin
+			if (!is_supported_mode(req_mode) || !vaddr_is_canonical(req_vaddr, req_mode)) begin
+				resp_page_fault = 1'b1;
+			end else if (req_mode == SATP_MODE_BARE || req_priv == 2'b11) begin
+				// Bare mode or M-mode bypasses translation.
+				resp_hit = 1'b1;
+				resp_paddr = req_vaddr;
+			end else begin
+				for (k = 0; k < ENTRIES; k++) begin
+					if (!hit_found && entry_match(entries[k], req_vpn, req_asid, req_mode)) begin
+						hit_found = 1'b1;
+						hit_ppn = entries[k].ppn;
+						hit_level = entries[k].level;
+						hit_mode = entries[k].mode;
+						hit_u = entries[k].u;
+						hit_r = entries[k].r;
+						hit_w = entries[k].w;
+						hit_x = entries[k].x;
+						hit_a = entries[k].a;
+						hit_d = entries[k].d;
+					end
+				end
+
+				if (hit_found) begin
+					shift = page_shift(hit_mode, hit_level);
+					shift_index = int'(shift);
+					if (shift_index >= XLEN) begin
+						low_mask = {XLEN{1'b1}};
+					end else begin
+						low_mask = ({{(XLEN-1){1'b0}}, 1'b1} << shift_index) - 1;
+					end
+
+					hit_ppn_xlen = {{(XLEN-56){1'b0}}, hit_ppn, 12'b0};
+
+					req_priv_u = (req_priv == 2'b00);
+					if (!allow_access(req_is_fetch, req_is_store, req_priv_u, req_sum, req_mxr,
+									  hit_u, hit_r, hit_w, hit_x, hit_a, hit_d)) begin
+						resp_page_fault = 1'b1;
+					end else begin
+						resp_hit = 1'b1;
+						resp_paddr = (hit_ppn_xlen & ~low_mask) | (req_vaddr & low_mask);
+					end
+				end else begin
+					resp_need_ptw = 1'b1;
+				end
+			end
+		end
+	end
+
+	integer i;
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			repl_ptr <= '0;
+			for (i = 0; i < ENTRIES; i++) begin
+				entries[i].valid <= 1'b0;
+				entries[i].mode <= SATP_MODE_BARE;
+				entries[i].asid <= '0;
+				entries[i].vpn <= '0;
+				entries[i].ppn <= '0;
+				entries[i].level <= '0;
+				entries[i].g <= 1'b0;
+				entries[i].u <= 1'b0;
+				entries[i].r <= 1'b0;
+				entries[i].w <= 1'b0;
+				entries[i].x <= 1'b0;
+				entries[i].a <= 1'b0;
+				entries[i].d <= 1'b0;
+			end
+		end else begin
+			if (flush_all) begin
+				for (i = 0; i < ENTRIES; i++) begin
+					entries[i].valid <= 1'b0;
+				end
+			end else if (flush_asid_valid || flush_vpn_valid) begin
+				for (i = 0; i < ENTRIES; i++) begin
+					logic asid_match;
+					logic vpn_match;
+					logic mode_match;
+					logic [44:0] mask;
+
+					asid_match = !flush_asid_valid || (!entries[i].g && (entries[i].asid == flush_asid));
+					mode_match = !flush_vpn_valid || (entries[i].mode == flush_mode);
+					if (flush_vpn_valid) begin
+						mask = make_vpn_mask(entries[i].mode, entries[i].level);
+						vpn_match = (((entries[i].vpn ^ flush_vpn) & mask) == '0);
+					end else begin
+						vpn_match = 1'b1;
+					end
+
+					if (entries[i].valid && asid_match && mode_match && vpn_match) begin
+						entries[i].valid <= 1'b0;
+					end
+				end
+			end
+
+			if (refill_valid && is_supported_mode(refill_mode) && (refill_mode != SATP_MODE_BARE)) begin
+				entries[repl_ptr].valid <= 1'b1;
+				entries[repl_ptr].mode <= refill_mode;
+				entries[repl_ptr].asid <= refill_asid;
+				entries[repl_ptr].vpn <= refill_vpn;
+				entries[repl_ptr].ppn <= refill_ppn;
+				entries[repl_ptr].level <= refill_level;
+				entries[repl_ptr].g <= refill_g;
+				entries[repl_ptr].u <= refill_u;
+				entries[repl_ptr].r <= refill_r;
+				entries[repl_ptr].w <= refill_w;
+				entries[repl_ptr].x <= refill_x;
+				entries[repl_ptr].a <= refill_a;
+				entries[repl_ptr].d <= refill_d;
+				repl_ptr <= repl_ptr + 1'b1;
+			end
+		end
+	end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/ex_mem_reg.sv b/rv64cpu/rtl/core/pipeline/ex_mem_reg.sv
new file mode 100644
index 0000000..b8e5b9e
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/ex_mem_reg.sv
@@ -0,0 +1,281 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/ex_mem_reg.sv
+// Description: Execute to Memory Pipeline Register
+//              Latches results from the EX stage and bypassed control signals from ID/EX.
+//              Supports stall and flush operations.
+
+module ex_mem_reg
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Pipeline Control
+    input  logic                   stall_en,
+    input  logic                   flush_en,
+
+    // ----------------------
+    // Inputs from EX Stage (computed results)
+    input  logic [XLEN-1:0]        ex_alu_result,
+    input  logic                   ex_branch_taken,
+    input  logic [XLEN-1:0]        ex_branch_target,
+
+    // ----------------------
+    // Bypassed Inputs from ID/EX Register
+
+    // Data/Operands
+    input  logic [XLEN-1:0]        ex_rs1_rdata,  // Needed for CSRRW/RS/RC
+    input  logic [XLEN-1:0]        ex_rs2_rdata,  // Needed for store data
+    input  logic [XLEN-1:0]        ex_fs1_rdata,
+    input  logic [XLEN-1:0]        ex_fs2_rdata,
+    input  logic [XLEN-1:0]        ex_fs3_rdata,
+    input  logic [4:0]             ex_rs1_addr,   // Serves as zimm for CSRRWI/SI/CI
+    input  logic [4:0]             ex_fs3_addr,
+    input  logic [XLEN-1:0]        ex_pc,         // PC passed along for exceptions/CSRs
+    input  logic [XLEN-1:0]        ex_imm,        // Imm passed for CSR writes
+    input  logic [INST_WIDTH-1:0]  ex_instr,
+    input  logic [2:0]             ex_instr_len,
+
+    // Memory Control
+    input  logic                   ex_is_load,
+    input  logic                   ex_is_store,
+    input  logic [2:0]             ex_mem_size,
+    input  logic                   ex_is_amo,
+    input  amo_op_t                ex_amo_op,
+    input  logic                   ex_is_fp,
+    input  logic                   ex_fp_reg_write_en,
+    input  logic                   ex_fp_store_data_sel,
+    input  fp_op_t                 ex_fp_op,
+    input  fp_fmt_t                ex_fp_fmt,
+    input  logic [2:0]             ex_fp_rm,
+    input  logic [XLEN-1:0]        ex_fp_result,
+    input  logic [4:0]             ex_fp_fflags,
+
+    // Writeback Control
+    input  logic                   ex_reg_write_en,
+    input  logic [4:0]             ex_rd_addr,
+    input  logic [1:0]             ex_wb_sel,
+
+    // System / Exception Control
+    input  logic                   ex_is_csr,
+    input  logic [2:0]             ex_csr_op,
+    input  logic                   ex_illegal_instr,
+    input  logic                   ex_instr_access_fault,
+    input  logic                   ex_instr_page_fault,
+    input  logic                   ex_is_ecall,
+    input  logic                   ex_is_ebreak,
+    input  logic                   ex_is_mret,
+    input  logic                   ex_is_sret,
+    input  logic                   ex_is_wfi,
+    input  logic                   ex_is_sfence_vma,
+    input  logic                   ex_is_fence,
+    input  logic                   ex_is_fence_i,
+
+
+    // ----------------------
+    // Outputs to MEM Stage
+
+    // Data/Operands
+    output logic [XLEN-1:0]        mem_alu_result,
+    output logic                   mem_branch_taken,
+    output logic [XLEN-1:0]        mem_branch_target,
+    output logic [XLEN-1:0]        mem_rs1_rdata,
+    output logic [XLEN-1:0]        mem_rs2_rdata,
+    output logic [XLEN-1:0]        mem_fs1_rdata,
+    output logic [XLEN-1:0]        mem_fs2_rdata,
+    output logic [XLEN-1:0]        mem_fs3_rdata,
+    output logic [4:0]             mem_rs1_addr,
+    output logic [4:0]             mem_fs3_addr,
+    output logic [XLEN-1:0]        mem_pc,
+    output logic [XLEN-1:0]        mem_imm,
+    output logic [INST_WIDTH-1:0]  mem_instr,
+    output logic [2:0]             mem_instr_len,
+
+    // Memory Control
+    output logic                   mem_is_load,
+    output logic                   mem_is_store,
+    output logic [2:0]             mem_mem_size,
+    output logic                   mem_is_amo,
+    output amo_op_t                mem_amo_op,
+    output logic                   mem_is_fp,
+    output logic                   mem_fp_reg_write_en,
+    output logic                   mem_fp_store_data_sel,
+    output fp_op_t                 mem_fp_op,
+    output fp_fmt_t                mem_fp_fmt,
+    output logic [2:0]             mem_fp_rm,
+    output logic [XLEN-1:0]        mem_fp_result,
+    output logic [4:0]             mem_fp_fflags,
+
+    // Writeback Control
+    output logic                   mem_reg_write_en,
+    output logic [4:0]             mem_rd_addr,
+    output logic [1:0]             mem_wb_sel,
+
+    // System / Exception Control
+    output logic                   mem_is_csr,
+    output logic [2:0]             mem_csr_op,
+    output logic                   mem_illegal_instr,
+    output logic                   mem_instr_access_fault,
+    output logic                   mem_instr_page_fault,
+    output logic                   mem_is_ecall,
+    output logic                   mem_is_ebreak,
+    output logic                   mem_is_mret,
+    output logic                   mem_is_sret,
+    output logic                   mem_is_wfi,
+    output logic                   mem_is_sfence_vma,
+    output logic                   mem_is_fence,
+    output logic                   mem_is_fence_i
+);
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            mem_instr        <= '0;
+            mem_instr_len      <= 3'd4;
+            mem_alu_result    <= '0;
+            mem_branch_taken  <= '0;
+            mem_branch_target <= '0;
+            mem_rs1_rdata     <= '0;
+            mem_rs2_rdata     <= '0;
+            mem_fs1_rdata     <= '0;
+            mem_fs2_rdata     <= '0;
+            mem_fs3_rdata     <= '0;
+            mem_rs1_addr      <= '0;
+            mem_fs3_addr      <= '0;
+            mem_pc            <= '0;
+            mem_imm           <= '0;
+
+            // Critical control signals must be zeroed out
+            mem_is_load       <= '0;
+            mem_is_store      <= '0;
+            mem_mem_size      <= '0;
+            mem_is_amo        <= '0;
+            mem_amo_op        <= AMO_NONE;
+            mem_is_fp         <= '0;
+            mem_fp_reg_write_en <= '0;
+            mem_fp_store_data_sel <= '0;
+            mem_fp_op         <= FP_OP_NONE;
+            mem_fp_fmt        <= FP_FMT_S;
+            mem_fp_rm         <= '0;
+            mem_fp_result     <= '0;
+            mem_fp_fflags     <= '0;
+            mem_reg_write_en  <= '0;
+            mem_rd_addr       <= '0;
+            mem_wb_sel        <= '0;
+
+            // Exceptions and system control
+            mem_is_csr             <= '0;
+            mem_csr_op             <= '0;
+            mem_illegal_instr      <= '0;
+            mem_instr_access_fault <= '0;
+            mem_instr_page_fault   <= '0;
+            mem_is_ecall           <= '0;
+            mem_is_ebreak          <= '0;
+            mem_is_mret            <= '0;
+            mem_is_sret            <= '0;
+            mem_is_wfi             <= '0;
+            mem_is_sfence_vma      <= '0;
+            mem_is_fence           <= '0;
+            mem_is_fence_i         <= '0;
+
+        end else if (flush_en) begin
+            mem_instr        <= '0;
+            mem_instr_len      <= 3'd4;
+            // Critical control signals must be zeroed out
+            mem_branch_taken  <= '0;
+            mem_is_load       <= '0;
+            mem_is_store      <= '0;
+            mem_mem_size      <= '0;
+            mem_is_amo        <= '0;
+            mem_amo_op        <= AMO_NONE;
+            mem_is_fp         <= '0;
+            mem_fp_reg_write_en <= '0;
+            mem_fp_store_data_sel <= '0;
+            mem_fp_op         <= FP_OP_NONE;
+            mem_fp_fmt        <= FP_FMT_S;
+            mem_fp_rm         <= '0;
+            mem_fp_result     <= '0;
+            mem_fp_fflags     <= '0;
+            mem_reg_write_en  <= '0;
+            mem_rd_addr       <= '0;
+            mem_wb_sel        <= '0;
+
+            // Exceptions and system control
+            mem_is_csr             <= '0;
+            mem_csr_op             <= '0;
+            mem_illegal_instr      <= '0;
+            mem_instr_access_fault <= '0;
+            mem_instr_page_fault   <= '0;
+            mem_is_ecall           <= '0;
+            mem_is_ebreak          <= '0;
+            mem_is_mret            <= '0;
+            mem_is_sret            <= '0;
+            mem_is_wfi             <= '0;
+            mem_is_sfence_vma      <= '0;
+            mem_is_fence           <= '0;
+            mem_is_fence_i         <= '0;
+        end else if (~stall_en) begin
+            mem_alu_result         <= ex_alu_result;
+            mem_branch_taken       <= ex_branch_taken;
+            mem_branch_target      <= ex_branch_target;
+            mem_rs1_rdata          <= ex_rs1_rdata;
+            mem_rs2_rdata          <= ex_rs2_rdata;
+            mem_fs1_rdata          <= ex_fs1_rdata;
+            mem_fs2_rdata          <= ex_fs2_rdata;
+            mem_fs3_rdata          <= ex_fs3_rdata;
+            mem_rs1_addr           <= ex_rs1_addr;
+            mem_fs3_addr           <= ex_fs3_addr;
+            mem_pc                 <= ex_pc;
+            mem_imm                <= ex_imm;
+            mem_instr              <= ex_instr;
+            mem_instr_len          <= ex_instr_len;
+
+            mem_is_load            <= ex_is_load;
+            mem_is_store           <= ex_is_store;
+            mem_mem_size           <= ex_mem_size;
+            mem_is_amo             <= ex_is_amo;
+            mem_amo_op             <= ex_amo_op;
+            mem_is_fp              <= ex_is_fp;
+            mem_fp_reg_write_en    <= ex_fp_reg_write_en;
+            mem_fp_store_data_sel  <= ex_fp_store_data_sel;
+            mem_fp_op              <= ex_fp_op;
+            mem_fp_fmt             <= ex_fp_fmt;
+            mem_fp_rm              <= ex_fp_rm;
+            mem_fp_result          <= ex_fp_result;
+            mem_fp_fflags          <= ex_fp_fflags;
+            mem_reg_write_en       <= ex_reg_write_en;
+            mem_rd_addr            <= ex_rd_addr;
+            mem_wb_sel             <= ex_wb_sel;
+
+            mem_is_csr             <= ex_is_csr;
+            mem_csr_op             <= ex_csr_op;
+            mem_illegal_instr      <= ex_illegal_instr;
+            mem_instr_access_fault <= ex_instr_access_fault;
+            mem_instr_page_fault   <= ex_instr_page_fault;
+            mem_is_ecall           <= ex_is_ecall;
+            mem_is_ebreak          <= ex_is_ebreak;
+            mem_is_mret            <= ex_is_mret;
+            mem_is_sret            <= ex_is_sret;
+            mem_is_wfi             <= ex_is_wfi;
+            mem_is_sfence_vma      <= ex_is_sfence_vma;
+            mem_is_fence           <= ex_is_fence;
+            mem_is_fence_i         <= ex_is_fence_i;
+        end
+        // If stall_en && !flush_en, maintain current state
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/ex_stage.sv b/rv64cpu/rtl/core/pipeline/ex_stage.sv
new file mode 100644
index 0000000..2758ffc
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/ex_stage.sv
@@ -0,0 +1,120 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/ex_stage.sv
+// Description: Execute Stage
+//              Performs ALU operations, branch/jump calculations, and generates memory addresses.
+
+module ex_stage
+    import rv64_pkg::*;
+(
+    // Input from ID/EX level pipeline register
+    input  logic [XLEN-1:0]    ex_pc,
+    input  logic [XLEN-1:0]    ex_rs1_rdata,
+    input  logic [XLEN-1:0]    ex_rs2_rdata,
+    input  logic [XLEN-1:0]    ex_imm,
+
+    // Execution Control Signals
+    input  alu_op_t            ex_alu_op,
+    input  logic               ex_alu_src1_sel,    // 0: rs1_rdata, 1: PC
+    input  logic               ex_alu_src2_sel,    // 0: rs2_rdata, 1: imm
+    input  logic               ex_is_branch,
+    input  branch_op_t         ex_branch_op,
+    input  logic               ex_is_jump,
+    input  logic               ex_is_word_op,
+
+    // Output to EX/MEM pipeline register & hazard unit
+    output logic [XLEN-1:0]    ex_alu_result,
+    output logic               ex_branch_taken,
+    output logic [XLEN-1:0]    ex_branch_target
+);
+
+    logic [XLEN-1:0]    alu_mux_a;
+    logic [XLEN-1:0]    alu_mux_b;
+    logic [XLEN-1:0]    alu_operand_a;
+    logic [XLEN-1:0]    alu_operand_b;
+    logic [XLEN-1:0]    alu_raw_result;
+    logic               branch_unit_taken;
+
+    // -------------
+    // ALU Input Multiplexers (MUX)
+    assign alu_mux_a = (ex_alu_src1_sel == 1'b1) ? ex_pc : ex_rs1_rdata;
+    assign alu_mux_b = (ex_alu_src2_sel == 1'b1) ? ex_imm : ex_rs2_rdata;
+    
+    // -------------
+    // RV64 Word Operation (32-bit) Pre-conditioning
+    // RV64 requires word operations (ADDW/SLLW/SRLW/SRAW) to operate on the 
+    // lower 32 bits and sign-extend the result.
+    always_comb begin
+        if (ex_is_word_op) begin
+            // Shift operations require specific 32-bit extension forms
+            if (ex_alu_op == ALU_SRA)
+                alu_operand_a = { {32{alu_mux_a[31]}}, alu_mux_a[31:0] }; // Sign extend for arithmetic shift
+            else if ((ex_alu_op == ALU_SRL) || (ex_alu_op == ALU_DIVU) || (ex_alu_op == ALU_REMU))
+                alu_operand_a = { 32'b0, alu_mux_a[31:0] };              // Zero extend for logical shift
+            else if ((ex_alu_op == ALU_DIV) || (ex_alu_op == ALU_REM))
+                alu_operand_a = { {32{alu_mux_a[31]}}, alu_mux_a[31:0] };
+            else
+                alu_operand_a = alu_mux_a;                               // ADDW/SUBW/SLLW don't care about upper bits
+            
+            // RV32 shift amount is strictly 5 bits (0-31), ignoring bit 5 which might be set in 64-bit registers
+            if (ex_alu_op == ALU_SLL || ex_alu_op == ALU_SRL || ex_alu_op == ALU_SRA)
+                alu_operand_b = { 59'b0, alu_mux_b[4:0] };
+            else if ((ex_alu_op == ALU_DIVU) || (ex_alu_op == ALU_REMU))
+                alu_operand_b = { 32'b0, alu_mux_b[31:0] };
+            else if ((ex_alu_op == ALU_DIV) || (ex_alu_op == ALU_REM))
+                alu_operand_b = { {32{alu_mux_b[31]}}, alu_mux_b[31:0] };
+            else
+                alu_operand_b = alu_mux_b;
+        end else begin
+            // 64-bit operations pass through unchanged
+            alu_operand_a = alu_mux_a;
+            alu_operand_b = alu_mux_b;
+        end
+    end
+
+    // --------------
+    // Instantiate the core computing engine ALU
+    integer_alu alu_inst (
+        .alu_op         (ex_alu_op),
+        .operand_a      (alu_operand_a),
+        .operand_b      (alu_operand_b),
+        .alu_result     (alu_raw_result)
+    );
+
+    // -------------
+    // Branch Unit
+    branch_unit bu_inst (
+        .is_branch    (ex_is_branch),
+        .branch_op    (ex_branch_op),
+        .operand_a    (ex_rs1_rdata),
+        .operand_b    (ex_rs2_rdata),
+        .branch_taken (branch_unit_taken)
+    );
+
+    // ------------- 
+    // 32-bit instruction result truncation and sign extension
+    assign ex_alu_result = ex_is_word_op ? 
+                           { {32{alu_raw_result[31]}}, alu_raw_result[31:0] } : 
+                           alu_raw_result;
+
+    // ------------- 
+    // Branch Target Calculation
+    assign ex_branch_target = alu_raw_result & ~64'b1;
+
+    assign ex_branch_taken  = ex_is_jump | (ex_is_branch & branch_unit_taken);
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/id_ex_reg.sv b/rv64cpu/rtl/core/pipeline/id_ex_reg.sv
new file mode 100644
index 0000000..777cc46
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/id_ex_reg.sv
@@ -0,0 +1,287 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/id_ex_reg.sv
+// Description: ID/EX Pipeline Register
+//              Latches control signals and data from ID stage and passes them to EX stage.
+//              Supports stall (freeze) and flush.
+
+module id_ex_reg
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Pipeline Control
+    input  logic                   stall_en,
+    input  logic                   flush_en,
+
+
+    // Inputs from ID Stage
+
+    // Instruction payload
+    input  logic [INST_WIDTH-1:0]  id_instr,
+    input  logic [2:0]             id_instr_len,
+
+    // Data/Operands
+    input  logic [XLEN-1:0]        id_pc,
+    input  logic [XLEN-1:0]        id_rs1_rdata,
+    input  logic [XLEN-1:0]        id_rs2_rdata,
+    input  logic [XLEN-1:0]        id_fs1_rdata,
+    input  logic [XLEN-1:0]        id_fs2_rdata,
+    input  logic [XLEN-1:0]        id_fs3_rdata,
+    input  logic [4:0]             id_rs1_addr,
+    input  logic [4:0]             id_rs2_addr,
+    input  logic [4:0]             id_fs3_addr,
+    input  logic [XLEN-1:0]        id_imm,
+
+    // Execution Control
+    input  alu_op_t                id_alu_op,
+    input  logic                   id_alu_src1_sel,
+    input  logic                   id_alu_src2_sel,
+    input  logic                   id_is_word_op,
+    input  logic                   id_is_branch,
+    input  branch_op_t             id_branch_op,
+    input  logic                   id_is_jump,
+
+    // Memory & Writeback Control
+    input  logic                   id_is_load,
+    input  logic                   id_is_store,
+    input  logic [2:0]             id_mem_size,
+    input  logic                   id_is_amo,
+    input  amo_op_t                id_amo_op,
+    input  logic                   id_is_fp,
+    input  logic                   id_fp_reg_write_en,
+    input  logic                   id_fp_store_data_sel,
+    input  fp_op_t                 id_fp_op,
+    input  fp_fmt_t                id_fp_fmt,
+    input  logic [2:0]             id_fp_rm,
+    input  logic                   id_reg_write_en,
+    input  logic [4:0]             id_rd_addr,
+    input  logic [1:0]             id_wb_sel,
+
+    // System / Exception
+    input  logic                   id_is_csr,
+    input  logic [2:0]             id_csr_op,
+    input  logic                   id_illegal_instr,
+    input  logic                   id_instr_access_fault,
+    input  logic                   id_instr_page_fault,
+    input  logic                   id_is_ecall,
+    input  logic                   id_is_ebreak,
+    input  logic                   id_is_mret,
+    input  logic                   id_is_sret,
+    input  logic                   id_is_wfi,
+    input  logic                   id_is_sfence_vma,
+    input  logic                   id_is_fence,
+    input  logic                   id_is_fence_i,
+
+
+    // Outputs to EX Stage
+
+    // Instruction payload
+    output logic [INST_WIDTH-1:0]  ex_instr,
+    output logic [2:0]             ex_instr_len,
+
+    // Data/Operands
+    output logic [XLEN-1:0]        ex_pc,
+    output logic [XLEN-1:0]        ex_rs1_rdata,
+    output logic [XLEN-1:0]        ex_rs2_rdata,
+    output logic [XLEN-1:0]        ex_fs1_rdata,
+    output logic [XLEN-1:0]        ex_fs2_rdata,
+    output logic [XLEN-1:0]        ex_fs3_rdata,
+    output logic [4:0]             ex_rs1_addr,
+    output logic [4:0]             ex_rs2_addr,
+    output logic [4:0]             ex_fs3_addr,
+    output logic [XLEN-1:0]        ex_imm,
+
+    // Execution Control
+    output alu_op_t                ex_alu_op,
+    output logic                   ex_alu_src1_sel,
+    output logic                   ex_alu_src2_sel,
+    output logic                   ex_is_word_op,
+    output logic                   ex_is_branch,
+    output branch_op_t             ex_branch_op,
+    output logic                   ex_is_jump,
+
+    // Memory & Writeback Control
+    output logic                   ex_is_load,
+    output logic                   ex_is_store,
+    output logic [2:0]             ex_mem_size,
+    output logic                   ex_is_amo,
+    output amo_op_t                ex_amo_op,
+    output logic                   ex_is_fp,
+    output logic                   ex_fp_reg_write_en,
+    output logic                   ex_fp_store_data_sel,
+    output fp_op_t                 ex_fp_op,
+    output fp_fmt_t                ex_fp_fmt,
+    output logic [2:0]             ex_fp_rm,
+    output logic                   ex_reg_write_en,
+    output logic [4:0]             ex_rd_addr,
+    output logic [1:0]             ex_wb_sel,
+
+    // System / Exception
+    output logic                   ex_is_csr,
+    output logic [2:0]             ex_csr_op,
+    output logic                   ex_illegal_instr,
+    output logic                   ex_instr_access_fault,
+    output logic                   ex_instr_page_fault,
+    output logic                   ex_is_ecall,
+    output logic                   ex_is_ebreak,
+    output logic                   ex_is_mret,
+    output logic                   ex_is_sret,
+    output logic                   ex_is_wfi,
+    output logic                   ex_is_sfence_vma,
+    output logic                   ex_is_fence,
+    output logic                   ex_is_fence_i
+);
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            ex_instr         <= '0;
+            ex_instr_len     <= 3'd4;
+            ex_pc            <= '0;
+            ex_rs1_rdata     <= '0;
+            ex_rs2_rdata     <= '0;
+            ex_fs1_rdata     <= '0;
+            ex_fs2_rdata     <= '0;
+            ex_fs3_rdata     <= '0;
+            ex_rs1_addr      <= '0;
+            ex_rs2_addr      <= '0;
+            ex_fs3_addr      <= '0;
+            ex_imm           <= '0;
+            ex_alu_op        <= ALU_ADD; // NOP is effectively ADD x0, x0, 0
+            ex_alu_src1_sel  <= '0;
+            ex_alu_src2_sel  <= '0;
+            ex_is_word_op    <= '0;
+            ex_is_branch     <= '0;
+            ex_branch_op     <= BRANCH_EQ;
+            ex_is_jump       <= '0;
+            ex_is_load       <= '0;
+            ex_is_store      <= '0;
+            ex_mem_size      <= '0;
+            ex_is_amo        <= '0;
+            ex_amo_op        <= AMO_NONE;
+            ex_is_fp         <= '0;
+            ex_fp_reg_write_en <= '0;
+            ex_fp_store_data_sel <= '0;
+            ex_fp_op         <= FP_OP_NONE;
+            ex_fp_fmt        <= FP_FMT_S;
+            ex_fp_rm         <= '0;
+            ex_reg_write_en  <= '0; // VERY IMPORTANT: Do not write to regfile on NOP
+            ex_rd_addr       <= '0;
+            ex_wb_sel        <= '0;
+            ex_is_csr             <= '0;
+            ex_csr_op             <= '0;
+            ex_illegal_instr      <= '0;
+            ex_instr_access_fault <= '0;
+            ex_instr_page_fault   <= '0;
+            ex_is_ecall           <= '0;
+            ex_is_ebreak          <= '0;
+            ex_is_mret            <= '0;
+            ex_is_sret            <= '0;
+            ex_is_wfi             <= '0;
+            ex_is_sfence_vma      <= '0;
+            ex_is_fence           <= '0;
+            ex_is_fence_i         <= '0;
+        end else if (flush_en) begin
+            ex_instr         <= '0;
+            ex_instr_len     <= 3'd4;
+            ex_alu_op        <= ALU_ADD; // NOP is effectively ADD x0, x0, 0
+            ex_alu_src1_sel  <= '0;
+            ex_alu_src2_sel  <= '0;
+            ex_is_word_op    <= '0;
+            ex_is_branch     <= '0;
+            ex_branch_op     <= BRANCH_EQ;
+            ex_is_jump       <= '0;
+            ex_is_load       <= '0;
+            ex_is_store      <= '0;
+            ex_mem_size      <= '0;
+            ex_is_amo        <= '0;
+            ex_amo_op        <= AMO_NONE;
+            ex_is_fp         <= '0;
+            ex_fp_reg_write_en <= '0;
+            ex_fp_store_data_sel <= '0;
+            ex_fp_op         <= FP_OP_NONE;
+            ex_fp_fmt        <= FP_FMT_S;
+            ex_fp_rm         <= '0;
+            ex_reg_write_en  <= '0; // VERY IMPORTANT: Do not write to regfile on NOP
+            ex_rd_addr       <= '0;
+            ex_wb_sel        <= '0;
+            ex_is_csr             <= '0;
+            ex_csr_op             <= '0;
+            ex_illegal_instr      <= '0;
+            ex_instr_access_fault <= '0;
+            ex_instr_page_fault   <= '0;
+            ex_is_ecall           <= '0;
+            ex_is_ebreak          <= '0;
+            ex_is_mret            <= '0;
+            ex_is_sret            <= '0;
+            ex_is_wfi             <= '0;
+            ex_is_sfence_vma      <= '0;
+            ex_is_fence           <= '0;
+            ex_is_fence_i         <= '0;
+        end else if (~stall_en) begin
+            ex_instr         <= id_instr;
+            ex_instr_len     <= id_instr_len;
+            ex_pc            <= id_pc;
+            ex_rs1_rdata     <= id_rs1_rdata;
+            ex_rs2_rdata     <= id_rs2_rdata;
+            ex_fs1_rdata     <= id_fs1_rdata;
+            ex_fs2_rdata     <= id_fs2_rdata;
+            ex_fs3_rdata     <= id_fs3_rdata;
+            ex_rs1_addr      <= id_rs1_addr;
+            ex_rs2_addr      <= id_rs2_addr;
+            ex_fs3_addr      <= id_fs3_addr;
+            ex_imm           <= id_imm;
+            ex_alu_op        <= id_alu_op;
+            ex_alu_src1_sel  <= id_alu_src1_sel;
+            ex_alu_src2_sel  <= id_alu_src2_sel;
+            ex_is_word_op    <= id_is_word_op;
+            ex_is_branch     <= id_is_branch;
+            ex_branch_op     <= id_branch_op;
+            ex_is_jump       <= id_is_jump;
+            ex_is_load       <= id_is_load;
+            ex_is_store      <= id_is_store;
+            ex_mem_size      <= id_mem_size;
+            ex_is_amo        <= id_is_amo;
+            ex_amo_op        <= id_amo_op;
+            ex_is_fp         <= id_is_fp;
+            ex_fp_reg_write_en <= id_fp_reg_write_en;
+            ex_fp_store_data_sel <= id_fp_store_data_sel;
+            ex_fp_op         <= id_fp_op;
+            ex_fp_fmt        <= id_fp_fmt;
+            ex_fp_rm         <= id_fp_rm;
+            ex_reg_write_en  <= id_reg_write_en;
+            ex_rd_addr       <= id_rd_addr;
+            ex_wb_sel        <= id_wb_sel;
+            ex_is_csr             <= id_is_csr;
+            ex_csr_op             <= id_csr_op;
+            ex_illegal_instr      <= id_illegal_instr;
+            ex_instr_access_fault <= id_instr_access_fault;
+            ex_instr_page_fault   <= id_instr_page_fault;
+            ex_is_ecall           <= id_is_ecall;
+            ex_is_ebreak          <= id_is_ebreak;
+            ex_is_mret            <= id_is_mret;
+            ex_is_sret            <= id_is_sret;
+            ex_is_wfi             <= id_is_wfi;
+            ex_is_sfence_vma      <= id_is_sfence_vma;
+            ex_is_fence           <= id_is_fence;
+            ex_is_fence_i         <= id_is_fence_i;
+        end
+        // If stall_en && !flush_en, maintain current state
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/id_stage.sv b/rv64cpu/rtl/core/pipeline/id_stage.sv
new file mode 100644
index 0000000..1cf447a
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/id_stage.sv
@@ -0,0 +1,197 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/id_stage.sv
+// Description: Instruction Decode Stage, instantiate the Decoder and RegFile, extract control signals and operands
+
+module id_stage
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Inputs from IF Stage (via IF/ID Pipeline Register)
+    input  logic [XLEN-1:0]        if_pc,        // PC of the current instruction
+    input  logic [INST_WIDTH-1:0]  if_instr,     // The 32-bit instruction fetched
+    input  logic [2:0]             if_instr_len,
+    input  logic [1:0]             csr_priv_mode, // Current privilege mode from CSR
+    input  logic                   csr_mstatus_tsr,
+    input  logic                   csr_mstatus_tw,
+    input  logic                   csr_mstatus_tvm,
+
+    // Write-back Inputs from WB Stage (to RegFile)
+    input  logic                   wb_reg_write_en, // Register write enable from WB stage
+    input  logic [4:0]             wb_rd_addr,     // Destination register address from WB
+    input  logic [XLEN-1:0]        wb_rd_wdata,    // Data to write back to register
+    input  logic                   wb_fp_write_en,
+    input  logic [4:0]             wb_fd_addr,
+    input  logic [XLEN-1:0]        wb_fd_wdata,
+    
+    // Outputs to EX Stage
+    // (Notice the 'id_' prefix to signify signals leaving the ID stage)
+    // Data/Operands
+    output logic [XLEN-1:0]        id_pc,          // Pass-through PC for branch/jump calculation
+    output logic [2:0]             id_instr_len,
+    output logic [XLEN-1:0]        id_rs1_rdata,   // Source register 1 data
+    output logic [XLEN-1:0]        id_rs2_rdata,   // Source register 2 data
+    output logic [XLEN-1:0]        id_fs1_rdata,
+    output logic [XLEN-1:0]        id_fs2_rdata,
+    output logic [XLEN-1:0]        id_fs3_rdata,
+    output logic [4:0]             id_rs1_addr,    // Rs1 index (for Forwarding & CSR zimm)
+    output logic [4:0]             id_rs2_addr,    // Rs2 index (for Forwarding)
+    output logic [4:0]             id_fs3_addr,
+    output logic [XLEN-1:0]        id_imm,         // Sign-extended generic immediate
+
+    // Execution Control (ALU & Branch)
+    output alu_op_t                id_alu_op,      // ALU operation code
+    output logic                   id_alu_src1_sel,// 0: rs1_rdata, 1: PC
+    output logic                   id_alu_src2_sel,// 0: rs2_rdata, 1: imm
+    output logic                   id_is_word_op,  // 1: RV64 32-bit operation (e.g. ADDW)
+    output logic                   id_is_branch,   // 1: B-type branch instruction
+    output branch_op_t             id_branch_op,   // Specific condition for branch (BEQ, BNE...)
+    output logic                   id_is_jump,     // 1: JAL or JALR
+
+    // Memory & Writeback Control (Passed down to MEM/WB stages eventually)
+    output logic                   id_is_load,     // 1: Load instruction
+    output logic                   id_is_store,    // 1: Store instruction
+    output logic [2:0]             id_mem_size,    // Memory access size (Byte/Half/Word/Double)
+    output logic                   id_is_amo,
+    output amo_op_t                id_amo_op,
+    output logic                   id_is_fp,
+    output logic                   id_fp_reg_write_en,
+    output logic                   id_fp_store_data_sel,
+    output fp_op_t                 id_fp_op,
+    output fp_fmt_t                id_fp_fmt,
+    output logic [2:0]             id_fp_rm,
+    output logic                   id_reg_write_en,// 1: Instruction will write to a register
+    output logic [4:0]             id_rd_addr,     // Destination register index
+    output logic [1:0]             id_wb_sel,      // Writeback source (0: ALU, 1: MEM, 2: PC+4)
+
+    // System / Exception
+    output logic                   id_is_csr,
+    output logic [2:0]             id_csr_op,
+    output logic                   id_illegal_instr,
+    output logic                   id_is_ecall,
+    output logic                   id_is_ebreak,
+    output logic                   id_is_mret,
+    output logic                   id_is_sret,
+    output logic                   id_is_wfi,
+    output logic                   id_is_sfence_vma,
+    output logic                   id_is_fence,
+    output logic                   id_is_fence_i
+);
+    // Internal Wires for connecting sub-modules
+    logic [4:0] rs1_addr;
+    logic [4:0] rs2_addr;
+    logic [4:0] fs3_addr;
+
+    // Pass-through PC and reg indices directly to next stage
+    assign id_pc = if_pc;
+    assign id_instr_len = if_instr_len;
+    assign id_rs1_addr = rs1_addr;
+    assign id_rs2_addr = rs2_addr;
+    assign fs3_addr = if_instr[31:27];
+    assign id_fs3_addr = fs3_addr;
+
+    // Instruction Decoder Instantiation
+
+    instruction_decoder u_decoder (
+        .instr          (if_instr),
+        .priv_mode      (csr_priv_mode),
+        .mstatus_tsr    (csr_mstatus_tsr),
+        .mstatus_tw     (csr_mstatus_tw),
+        .mstatus_tvm    (csr_mstatus_tvm),
+
+        // Outputs to RegFile read ports
+        .rs1_addr       (rs1_addr),
+        .rs2_addr       (rs2_addr),
+        .rd_addr        (id_rd_addr),
+
+        // Outputs to EX stage (ALU & execution control)
+        .alu_op         (id_alu_op),
+        .alu_src1_sel   (id_alu_src1_sel),
+        .alu_src2_sel   (id_alu_src2_sel),
+        .is_word_op     (id_is_word_op),
+        .imm            (id_imm),
+        
+        // Branch & Jump control
+        .is_branch      (id_is_branch),
+        .branch_op      (id_branch_op),
+        .is_jump        (id_is_jump),
+        
+        // Memory control
+        .is_load        (id_is_load),
+        .is_store       (id_is_store),
+        .mem_size       (id_mem_size),
+        .is_amo         (id_is_amo),
+        .amo_op         (id_amo_op),
+        .is_fp          (id_is_fp),
+        .fp_reg_write_en(id_fp_reg_write_en),
+        .fp_store_data_sel(id_fp_store_data_sel),
+        .fp_op          (id_fp_op),
+        .fp_fmt         (id_fp_fmt),
+        .fp_rm          (id_fp_rm),
+        
+        // Writeback control
+        .reg_write_en   (id_reg_write_en),
+        .wb_sel         (id_wb_sel),
+        
+        // System Control
+        .is_csr         (id_is_csr),
+        .csr_op         (id_csr_op),
+        .is_ecall       (id_is_ecall),
+        .is_ebreak      (id_is_ebreak),
+        .is_mret        (id_is_mret),
+        .is_sret        (id_is_sret),
+        .is_wfi         (id_is_wfi),
+        .is_sfence_vma  (id_is_sfence_vma),
+        .is_fence       (id_is_fence),
+        .is_fence_i     (id_is_fence_i),
+        .illegal_instr  (id_illegal_instr)
+    );
+
+    // Register File Instantiation
+    gpr_file u_regfile (
+        .clk            (clk),
+        .rst            (rst),
+        
+        // Read ports (from Decoder)
+        .rs1_addr       (rs1_addr),
+        .rs2_addr       (rs2_addr),
+        .rs1_rdata      (id_rs1_rdata),
+        .rs2_rdata      (id_rs2_rdata),
+        
+        // Write port (from WB stage)
+        .reg_write_en   (wb_reg_write_en),
+        .rd_addr        (wb_rd_addr),
+        .rd_wdata       (wb_rd_wdata)
+    );
+
+    fpr_file u_fp_regfile (
+        .clk            (clk),
+        .rst            (rst),
+        .fs1_addr       (rs1_addr),
+        .fs2_addr       (rs2_addr),
+        .fs3_addr       (fs3_addr),
+        .fs1_rdata      (id_fs1_rdata),
+        .fs2_rdata      (id_fs2_rdata),
+        .fs3_rdata      (id_fs3_rdata),
+        .fp_write_en    (wb_fp_write_en),
+        .fd_addr        (wb_fd_addr),
+        .fd_wdata       (wb_fd_wdata)
+    );
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/if_id_reg.sv b/rv64cpu/rtl/core/pipeline/if_id_reg.sv
new file mode 100644
index 0000000..0ea4fae
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/if_id_reg.sv
@@ -0,0 +1,72 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/if_id_reg.sv
+// Description: IF/ID Pipeline Register
+//              Latches PC and instruction from IF stage and passes them to ID stage.
+//              Supports stall (freeze) and flush (insert NOP).
+
+module if_id_reg
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Pipeline Control from Hazard Unit
+    input  logic                   stall_en,   // 1: Freeze pipeline register state
+    input  logic                   flush_en,   // 1: Clear register (insert NOP)
+
+    // Inputs from IF Stage
+    input  logic [XLEN-1:0]        if_pc,      // PC from IF stage
+    input  logic [INST_WIDTH-1:0]  if_instr,   // Fetched instruction from IF stage
+    input  logic [2:0]             if_instr_len,
+    input  logic                   if_instr_access_fault,
+    input  logic                   if_instr_page_fault,
+
+    // Outputs to ID Stage
+    output logic [XLEN-1:0]        id_pc,      // Passed to ID stage
+    output logic [INST_WIDTH-1:0]  id_instr,   // Passed to ID stage
+    output logic [2:0]             id_instr_len,
+    output logic                   id_instr_access_fault,
+    output logic                   id_instr_page_fault
+);
+
+    localparam logic [31:0] NOP_INSTR = 32'h0000_0013;
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            id_pc                  <= '0;
+            id_instr               <= NOP_INSTR;
+            id_instr_len           <= 3'd4;
+            id_instr_access_fault <= 1'b0;
+            id_instr_page_fault   <= 1'b0;
+        end else if (flush_en) begin
+            id_pc                  <= '0;
+            id_instr               <= NOP_INSTR;
+            id_instr_len           <= 3'd4;
+            id_instr_access_fault <= 1'b0;
+            id_instr_page_fault   <= 1'b0;
+        end else if (~stall_en) begin
+            id_pc                  <= if_pc;
+            id_instr               <= if_instr;
+            id_instr_len           <= if_instr_len;
+            id_instr_access_fault <= if_instr_access_fault;
+            id_instr_page_fault   <= if_instr_page_fault;
+        end
+        // If stall_en && !flush_en, maintain current state
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/if_stage.sv b/rv64cpu/rtl/core/pipeline/if_stage.sv
new file mode 100644
index 0000000..c3d417b
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/if_stage.sv
@@ -0,0 +1,64 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/if_stage.sv
+// Description: Instruction Fetching Stage, includes PC registers and next-state PC calculation logic
+
+module if_stage 
+    import rv64_pkg::*;
+(
+    input logic                clk,
+    input logic                rst,
+
+    // Pipeline Control Signals (from Hazard Unit & Trap Controller)
+    input logic                stall_en,          // Freeze PC (stall), 0: Normal operation
+    input logic                pc_redirect_sel,   // Branch/Trap redirect request
+    input logic [XLEN-1:0]     pc_redirect_target,// Redirect target address
+    input logic                pc_redirect_is_trap, // 1: Redirect request comes from trap controller
+    input logic                if_instr_compressed,
+    
+    output logic [XLEN-1:0]    if_pc
+);
+
+    // PC Register
+    logic [XLEN-1:0] pc_reg;
+
+    // Combinational logic signal: the PC value to be written in the next clock cycle
+    logic [XLEN-1:0] next_pc;
+
+    always_comb begin
+        next_pc = pc_reg + (if_instr_compressed ? 64'd2 : 64'd4);
+    end
+
+    // Sequential Logic: Update PC Register
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            pc_reg <= 64'h0000_0000_8000_0000;
+        end else if (pc_redirect_sel && pc_redirect_is_trap) begin
+            pc_reg <= pc_redirect_target;
+        end else if (~stall_en) begin
+            if (pc_redirect_sel) begin
+                pc_reg <= pc_redirect_target;
+            end else begin
+                pc_reg <= next_pc;
+            end
+        end
+    end
+    
+    // Output wiring
+    assign if_pc = pc_reg;
+    
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/mem_stage.sv b/rv64cpu/rtl/core/pipeline/mem_stage.sv
new file mode 100644
index 0000000..d5a97dc
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/mem_stage.sv
@@ -0,0 +1,430 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/mem_stage.sv
+// Description: Memory Stage
+//              Latches results from the EX stage and bypassed control signals from ID/EX.
+//              Supports stall and flush operations.
+
+module mem_stage
+    import rv64_pkg::*;
+(
+    input  logic                   clk,
+    input  logic                   rst,
+
+    // Inputs from EX/MEM pipeline register
+    input  logic [XLEN-1:0]        mem_alu_result,    // Used as memory address or ALU result
+    input  logic                   mem_branch_taken,
+    input  logic [XLEN-1:0]        mem_branch_target,
+    input  logic [XLEN-1:0]        mem_rs1_rdata,
+    input  logic [XLEN-1:0]        mem_rs2_rdata,     // Store data
+    input  logic [XLEN-1:0]        mem_fs2_rdata,
+    input  logic [4:0]             mem_rs1_addr,
+    input  logic [XLEN-1:0]        mem_pc,
+    input  logic [XLEN-1:0]        mem_imm,
+    input  logic [INST_WIDTH-1:0]  mem_instr,
+    input  logic [2:0]             mem_instr_len,
+
+    // Memory Control
+    input  logic                   mem_is_load,
+    input  logic                   mem_is_store,
+    input  logic [2:0]             mem_mem_size,      // LB/LH/LW/LD/LBU/LHU/LWU
+    input  logic                   mem_is_amo,
+    input  amo_op_t                mem_amo_op,
+    input  logic                   mem_is_fp,
+    input  logic                   mem_fp_reg_write_en,
+    input  logic                   mem_fp_store_data_sel,
+    input  logic [XLEN-1:0]        mem_fp_result,
+    input  logic [4:0]             mem_fp_fflags,
+
+    // Writeback Control
+    input  logic                   mem_reg_write_en,
+    input  logic [4:0]             mem_rd_addr,
+    input  logic [1:0]             mem_wb_sel,
+
+    // Exceptions and System (bypassed through or generated)
+    input  logic                   mem_is_csr,
+    input  logic [2:0]             mem_csr_op,
+    input  logic                   mem_illegal_instr,
+    input  logic                   mem_instr_access_fault,
+    input  logic                   mem_instr_page_fault,
+    input  logic                   mem_is_ecall,
+    input  logic                   mem_is_ebreak,
+    input  logic                   mem_is_mret,
+    input  logic                   mem_is_sret,
+    input  logic                   mem_is_wfi,
+    input  logic                   mem_is_sfence_vma,
+    input  logic                   mem_is_fence,
+    input  logic                   mem_is_fence_i,
+
+    // Data Memory Interface (to Data Cache / Memory Controller)
+    output logic                   dmem_read_req,
+    output logic                   dmem_write_req,
+    output logic [XLEN-1:0]        dmem_addr,
+    output logic [XLEN-1:0]        dmem_wdata,
+    output logic [7:0]             dmem_wstrb,        // Write strobe depending on size
+    input  logic                   dmem_req_ready,    // Wait signal from D-Cache
+    input  logic                   dmem_rsp_valid,    // Data valid from D-Cache
+    input  logic [XLEN-1:0]        dmem_rdata,
+    input  logic                   dmem_load_access_fault,
+    input  logic                   dmem_store_access_fault,
+    input  logic                   dmem_load_page_fault,
+    input  logic                   dmem_store_page_fault,
+
+    // Outputs to MEM/WB Pipeline Register
+    output logic [XLEN-1:0]        mem_wb_alu_result,
+    output logic [XLEN-1:0]        mem_wb_mem_rdata,
+    output logic [XLEN-1:0]        mem_wb_fp_result,
+    output logic [4:0]             mem_wb_fp_fflags,
+    output logic [XLEN-1:0]        mem_wb_pc,
+    output logic [INST_WIDTH-1:0]  mem_wb_instr,
+    output logic [2:0]             mem_wb_instr_len,
+
+    output logic                   mem_wb_reg_write_en,
+    output logic                   mem_wb_fp_reg_write_en,
+    output logic [4:0]             mem_wb_rd_addr,
+    output logic [1:0]             mem_wb_wb_sel,
+
+    output logic                   mem_wb_is_csr,
+    output logic [2:0]             mem_wb_csr_op,
+    output logic [XLEN-1:0]        mem_wb_rs1_rdata,
+    output logic [4:0]             mem_wb_rs1_addr,
+    output logic [XLEN-1:0]        mem_wb_imm,
+
+    output logic                   mem_wb_illegal_instr,
+    output logic                   mem_wb_instr_access_fault,
+    output logic                   mem_wb_instr_page_fault,
+    output logic                   mem_wb_is_ecall,
+    output logic                   mem_wb_is_ebreak,
+    output logic                   mem_wb_is_mret,
+    output logic                   mem_wb_is_sret,
+    output logic                   mem_wb_load_fault,
+    output logic                   mem_wb_store_fault,
+    output logic                   mem_wb_load_page_fault,
+    output logic                   mem_wb_store_page_fault,
+    output logic [XLEN-1:0]        mem_wb_fault_addr,
+
+    // Control to Hazard Unit / Stall Logic
+    output logic                   mem_stall_req      // Stall pipeline if D-Cache doesn't respond
+);
+
+    logic [XLEN-1:0] load_data_aligned;
+    logic [XLEN-1:0] store_data_aligned;
+    logic [XLEN-1:0] amo_write_data;
+    logic [XLEN-1:0] amo_wb_data;
+    logic [XLEN-1:0] selected_store_data;
+    logic [XLEN-1:0] fp_load_data;
+    logic [7:0]      write_strobe;
+    logic            load_addr_misaligned;
+    logic            store_addr_misaligned;
+    logic            amo_is_lr;
+    logic            amo_is_sc;
+    logic            amo_is_word;
+    logic            amo_is_double;
+    logic            amo_reads_memory;
+    logic            amo_writes_memory;
+    logic            amo_store_access;
+    logic            amo_load_access;
+    logic            amo_addr_misaligned;
+    logic            amo_sc_success;
+    logic            amo_rmw_op;
+    logic            amo_rmw_read_phase;
+    logic            amo_rmw_write_phase;
+    logic [XLEN-1:0] amo_memory_rdata;
+    logic            amo_rmw_busy_q;
+    logic [XLEN-1:0] amo_rmw_old_data_q;
+    logic            reservation_valid_q;
+    logic [XLEN-1:0] reservation_addr_q;
+
+    function automatic logic addr_misaligned(
+        input logic [2:0]      size,
+        input logic [XLEN-1:0] addr
+    );
+        logic result;
+        case (size)
+            3'b000, 3'b100: result = 1'b0;         // Byte L/S
+            3'b001, 3'b101: result = addr[0];      // Halfword requires bit0 = 0
+            3'b010, 3'b110: result = |addr[1:0];   // Word requires bits[1:0] = 0
+            3'b011:          result = |addr[2:0];   // Doubleword requires bits[2:0] = 0
+            default:         result = 1'b0;
+        endcase
+        return result;
+    endfunction
+
+    assign amo_is_lr          = mem_is_amo && (mem_amo_op == AMO_LR);
+    assign amo_is_sc          = mem_is_amo && (mem_amo_op == AMO_SC);
+    assign amo_is_word        = mem_is_amo && (mem_mem_size == 3'b010);
+    assign amo_is_double      = mem_is_amo && (mem_mem_size == 3'b011);
+    assign amo_rmw_op         = mem_is_amo && !amo_is_lr && !amo_is_sc;
+    assign amo_rmw_read_phase = amo_rmw_op && !amo_rmw_busy_q;
+    assign amo_rmw_write_phase = amo_rmw_op && amo_rmw_busy_q;
+    assign amo_reads_memory   = amo_is_lr || amo_rmw_read_phase;
+    assign amo_store_access   = mem_is_amo && !amo_is_lr;
+    assign amo_load_access    = mem_is_amo && amo_is_lr;
+    assign amo_addr_misaligned = mem_is_amo && addr_misaligned(mem_mem_size, mem_alu_result);
+    assign amo_sc_success     = amo_is_sc && reservation_valid_q &&
+                                (reservation_addr_q == mem_alu_result) &&
+                                !amo_addr_misaligned && !dmem_store_access_fault &&
+                                !dmem_store_page_fault && !mem_instr_access_fault &&
+                                !mem_instr_page_fault;
+    assign amo_writes_memory  = (amo_is_sc && amo_sc_success) || amo_rmw_write_phase;
+    assign amo_memory_rdata   = amo_rmw_busy_q ? amo_rmw_old_data_q : dmem_rdata;
+
+    assign load_addr_misaligned  = mem_is_load && addr_misaligned(mem_mem_size, mem_alu_result);
+    assign store_addr_misaligned = (mem_is_store && addr_misaligned(mem_mem_size, mem_alu_result)) |
+                                   (amo_store_access && amo_addr_misaligned);
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            amo_rmw_busy_q      <= 1'b0;
+            amo_rmw_old_data_q  <= '0;
+            reservation_valid_q <= 1'b0;
+            reservation_addr_q  <= '0;
+        end else begin
+            if (amo_rmw_read_phase && dmem_read_req && dmem_rsp_valid && !dmem_load_access_fault &&
+                !dmem_load_page_fault && !mem_instr_access_fault && !mem_instr_page_fault) begin
+                amo_rmw_busy_q     <= 1'b1;
+                amo_rmw_old_data_q <= dmem_rdata;
+            end else if (amo_rmw_write_phase && (!dmem_write_req || dmem_req_ready)) begin
+                amo_rmw_busy_q     <= 1'b0;
+                amo_rmw_old_data_q <= '0;
+            end else if (!mem_is_amo) begin
+                amo_rmw_busy_q     <= 1'b0;
+                amo_rmw_old_data_q <= '0;
+            end
+
+            if (amo_is_lr && dmem_read_req && dmem_rsp_valid && !dmem_load_access_fault &&
+                !dmem_load_page_fault) begin
+                reservation_valid_q <= 1'b1;
+                reservation_addr_q  <= mem_alu_result;
+            end else if (amo_is_sc && dmem_req_ready) begin
+                reservation_valid_q <= 1'b0;
+                reservation_addr_q  <= '0;
+            end
+        end
+    end
+
+    // --------------------------
+    // Memory Request Logic
+    assign dmem_read_req  = (mem_is_load | amo_reads_memory) &
+                            ~(load_addr_misaligned | (amo_reads_memory & amo_addr_misaligned)) &
+                            ~dmem_load_access_fault & ~dmem_load_page_fault &
+                            ~mem_instr_access_fault & ~mem_instr_page_fault;
+    assign dmem_write_req = (mem_is_store | amo_writes_memory) & ~store_addr_misaligned & ~dmem_store_access_fault &
+                            ~dmem_store_page_fault & ~mem_instr_access_fault & ~mem_instr_page_fault;
+    assign dmem_addr      = mem_alu_result; // ALU result is the computed address
+
+    // --------------------------
+    // Stall generation: Stall if memory request is not yet ready or valid
+    // For a simple synchronous SRAM, this might be 0, but for cache it will depend on hits/misses
+    // --------------------------
+    always_comb begin
+        if (amo_rmw_read_phase && dmem_read_req) begin
+            mem_stall_req = 1'b1;
+        end else if ((dmem_read_req || dmem_write_req) && (!dmem_req_ready || (dmem_read_req && !dmem_rsp_valid))) begin
+            mem_stall_req = 1'b1;
+        end else begin
+            mem_stall_req = 1'b0;
+        end
+    end
+
+    // --------------------------
+    // Store Data Alignment and Strobe Generation
+    // --------------------------
+    always_comb begin
+        logic [2:0] align_offset;
+        store_data_aligned = '0;
+        write_strobe       = 8'b0000_0000;
+        selected_store_data = mem_fp_store_data_sel ? mem_fs2_rdata : mem_rs2_rdata;
+
+        // Mem address lower 3 bits determine the alignment within the 64-bit word
+        align_offset = mem_alu_result[2:0];
+
+        if (mem_is_store || amo_writes_memory) begin
+            case (mem_mem_size)
+                // SB: Store Byte
+                3'b000: begin
+                    store_data_aligned = {56'b0, selected_store_data[7:0]} << (align_offset * 8);
+                    write_strobe       = 8'b0000_0001 << align_offset;
+                end
+                // SH: Store Halfword
+                3'b001: begin
+                    store_data_aligned = {48'b0, selected_store_data[15:0]} << (align_offset * 8);
+                    write_strobe       = 8'b0000_0011 << align_offset;
+                end
+                // SW: Store Word
+                3'b010: begin
+                    store_data_aligned = {32'b0, amo_writes_memory ? amo_write_data[31:0] :
+                                          selected_store_data[31:0]} << (align_offset * 8);
+                    write_strobe       = 8'b0000_1111 << align_offset;
+                end
+                // SD: Store Doubleword
+                3'b011: begin
+                    store_data_aligned = amo_writes_memory ? amo_write_data : selected_store_data;
+                    write_strobe       = 8'b1111_1111;
+                end
+                default: begin
+                    store_data_aligned = '0;
+                    write_strobe       = 8'b0000_0000;
+                end
+            endcase
+        end
+    end
+
+    assign dmem_wdata = store_data_aligned;
+    assign dmem_wstrb = write_strobe;
+
+    // --------------------------
+    // Load Data Alignment and Sign/Zero Extension
+    // --------------------------
+    always_comb begin
+        logic [2:0] align_offset;
+        logic [XLEN-1:0] shifted_rdata;
+        load_data_aligned = '0;
+
+        align_offset = mem_alu_result[2:0];
+
+        // Shift data down so the target bytes are at the bottom
+        shifted_rdata = dmem_rdata >> (align_offset * 8);
+
+        case (mem_mem_size)
+            // LB: Load Byte (Sign extend)
+            3'b000: load_data_aligned = { {56{shifted_rdata[7]}}, shifted_rdata[7:0] };
+            // LH: Load Halfword (Sign extend)
+            3'b001: load_data_aligned = { {48{shifted_rdata[15]}}, shifted_rdata[15:0] };
+            // LW: Load Word (Sign extend)
+            3'b010: load_data_aligned = { {32{shifted_rdata[31]}}, shifted_rdata[31:0] };
+            // LD: Load Doubleword
+            3'b011: load_data_aligned = shifted_rdata;
+            // LBU: Load Byte Unsigned (Zero extend)
+            3'b100: load_data_aligned = { 56'b0, shifted_rdata[7:0] };
+            // LHU: Load Half Unsigned (Zero extend)
+            3'b101: load_data_aligned = { 48'b0, shifted_rdata[15:0] };
+            // LWU: Load Word Unsigned (Zero extend)
+            3'b110: load_data_aligned = { 32'b0, shifted_rdata[31:0] };
+            default: load_data_aligned = '0;
+        endcase
+    end
+
+    always_comb begin
+        fp_load_data = load_data_aligned;
+        if (mem_is_fp && (mem_mem_size == 3'b010)) begin
+            fp_load_data = {32'hFFFF_FFFF, load_data_aligned[31:0]};
+        end
+    end
+
+    always_comb begin
+        logic [2:0]        align_offset;
+        logic [XLEN-1:0]   shifted_rdata;
+        logic [31:0]       amo_old_word;
+        logic [31:0]       amo_rs2_word;
+        logic [31:0]       amo_result_word;
+        logic signed [31:0] amo_old_word_signed;
+        logic signed [31:0] amo_rs2_word_signed;
+        logic signed [XLEN-1:0] amo_old_double_signed;
+        logic signed [XLEN-1:0] amo_rs2_double_signed;
+
+        align_offset = mem_alu_result[2:0];
+        shifted_rdata = amo_memory_rdata >> (align_offset * 8);
+        amo_old_word = shifted_rdata[31:0];
+        amo_rs2_word = mem_rs2_rdata[31:0];
+        amo_old_word_signed = amo_old_word;
+        amo_rs2_word_signed = amo_rs2_word;
+        amo_old_double_signed = amo_memory_rdata;
+        amo_rs2_double_signed = mem_rs2_rdata;
+        amo_result_word = amo_old_word;
+        amo_write_data = mem_rs2_rdata;
+        amo_wb_data = '0;
+
+        if (amo_is_word) begin
+            unique case (mem_amo_op)
+                AMO_SWAP, AMO_SC: amo_result_word = amo_rs2_word;
+                AMO_ADD:          amo_result_word = amo_old_word + amo_rs2_word;
+                AMO_XOR:          amo_result_word = amo_old_word ^ amo_rs2_word;
+                AMO_AND:          amo_result_word = amo_old_word & amo_rs2_word;
+                AMO_OR:           amo_result_word = amo_old_word | amo_rs2_word;
+                AMO_MIN:          amo_result_word = (amo_old_word_signed < amo_rs2_word_signed) ?
+                                                     amo_old_word : amo_rs2_word;
+                AMO_MAX:          amo_result_word = (amo_old_word_signed > amo_rs2_word_signed) ?
+                                                     amo_old_word : amo_rs2_word;
+                AMO_MINU:         amo_result_word = (amo_old_word < amo_rs2_word) ? amo_old_word : amo_rs2_word;
+                AMO_MAXU:         amo_result_word = (amo_old_word > amo_rs2_word) ? amo_old_word : amo_rs2_word;
+                default:          amo_result_word = amo_old_word;
+            endcase
+            amo_write_data = {{32{amo_result_word[31]}}, amo_result_word};
+            amo_wb_data = amo_is_sc ? {63'b0, ~amo_sc_success} :
+                          {{32{amo_old_word[31]}}, amo_old_word};
+        end else if (amo_is_double) begin
+            unique case (mem_amo_op)
+                AMO_SWAP, AMO_SC: amo_write_data = mem_rs2_rdata;
+                AMO_ADD:          amo_write_data = amo_memory_rdata + mem_rs2_rdata;
+                AMO_XOR:          amo_write_data = amo_memory_rdata ^ mem_rs2_rdata;
+                AMO_AND:          amo_write_data = amo_memory_rdata & mem_rs2_rdata;
+                AMO_OR:           amo_write_data = amo_memory_rdata | mem_rs2_rdata;
+                AMO_MIN:          amo_write_data = (amo_old_double_signed < amo_rs2_double_signed) ?
+                                                   amo_memory_rdata : mem_rs2_rdata;
+                AMO_MAX:          amo_write_data = (amo_old_double_signed > amo_rs2_double_signed) ?
+                                                   amo_memory_rdata : mem_rs2_rdata;
+                AMO_MINU:         amo_write_data = (amo_memory_rdata < mem_rs2_rdata) ? amo_memory_rdata :
+                                                   mem_rs2_rdata;
+                AMO_MAXU:         amo_write_data = (amo_memory_rdata > mem_rs2_rdata) ? amo_memory_rdata :
+                                                   mem_rs2_rdata;
+                default:          amo_write_data = amo_memory_rdata;
+            endcase
+            amo_wb_data = amo_is_sc ? {63'b0, ~amo_sc_success} : amo_memory_rdata;
+        end
+    end
+
+    // --------------------------
+    // Pass Output Signals to WB Stage
+    // --------------------------
+    assign mem_wb_alu_result    = mem_alu_result;
+    assign mem_wb_mem_rdata     = mem_is_amo ? amo_wb_data : load_data_aligned;
+    assign mem_wb_fp_result     = mem_is_load ? fp_load_data : mem_fp_result;
+    assign mem_wb_fp_fflags     = mem_fp_fflags;
+    assign mem_wb_pc            = mem_pc;
+    assign mem_wb_instr         = mem_instr;
+    assign mem_wb_instr_len     = mem_instr_len;
+
+    assign mem_wb_reg_write_en  = mem_reg_write_en;
+    assign mem_wb_fp_reg_write_en = mem_fp_reg_write_en;
+    assign mem_wb_rd_addr       = mem_rd_addr;
+    assign mem_wb_wb_sel        = mem_wb_sel;
+
+    assign mem_wb_is_csr        = mem_is_csr;
+    assign mem_wb_csr_op        = mem_csr_op;
+    assign mem_wb_rs1_rdata     = mem_rs1_rdata;
+    assign mem_wb_rs1_addr      = mem_rs1_addr;
+    assign mem_wb_imm           = mem_imm;
+
+    assign mem_wb_illegal_instr      = mem_illegal_instr;
+    assign mem_wb_instr_access_fault = mem_instr_access_fault;
+    assign mem_wb_instr_page_fault   = mem_instr_page_fault;
+    assign mem_wb_is_ecall           = mem_is_ecall;
+    assign mem_wb_is_ebreak          = mem_is_ebreak;
+    assign mem_wb_is_mret            = mem_is_mret;
+    assign mem_wb_is_sret            = mem_is_sret;
+    assign mem_wb_load_fault         = load_addr_misaligned | (amo_load_access & amo_addr_misaligned) |
+                                       dmem_load_access_fault;
+    assign mem_wb_store_fault        = store_addr_misaligned | dmem_store_access_fault;
+    assign mem_wb_load_page_fault    = dmem_load_page_fault;
+    assign mem_wb_store_page_fault   = dmem_store_page_fault;
+    assign mem_wb_fault_addr         = (mem_instr_access_fault | mem_instr_page_fault) ? mem_pc :
+                                       ((dmem_load_page_fault | dmem_store_page_fault) ? mem_alu_result :
+                                       ((load_addr_misaligned | store_addr_misaligned |
+                                         dmem_load_access_fault | dmem_store_access_fault) ? mem_alu_result : '0));
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/mem_wb_reg.sv b/rv64cpu/rtl/core/pipeline/mem_wb_reg.sv
new file mode 100644
index 0000000..be0aec1
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/mem_wb_reg.sv
@@ -0,0 +1,188 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/mem_wb_reg.sv
+// Description: MEM/WB pipeline stage register (Memory Access to Write Back)
+//              Passes the final computation results, memory read data, and write-back control signals to the write-back stage
+
+module mem_wb_reg
+    import rv64_pkg::*;
+(
+    input  logic               clk,
+    input  logic               rst,
+
+    // -----------------------
+    // Control Signals from Hazard Unit
+    input  logic               stall_en,
+    input  logic               flush_en,
+
+    // -----------------------
+    // Inputs from MEM stage
+    // -----------------------
+    // Data and addresses
+    input  logic [XLEN-1:0]        mem_pc,
+    input  logic [XLEN-1:0]        mem_alu_result,
+    input  logic [XLEN-1:0]        mem_mem_rdata,
+    input  logic [XLEN-1:0]        mem_fp_result,
+    input  logic [4:0]             mem_fp_fflags,
+    input  logic [INST_WIDTH-1:0]  mem_instr,
+    input  logic [2:0]             mem_instr_len,
+
+    // Write back control
+    input  logic                   mem_reg_write_en,
+    input  logic                   mem_fp_reg_write_en,
+    input  logic [4:0]             mem_rd_addr,
+    input  logic [1:0]             mem_wb_sel,
+
+    // CSR & System
+    input  logic                   mem_is_csr,
+    input  logic [2:0]             mem_csr_op,
+    input  logic [XLEN-1:0]        mem_rs1_rdata,
+    input  logic [4:0]             mem_rs1_addr,
+    input  logic [XLEN-1:0]        mem_imm,
+    input  logic                   mem_illegal_instr,
+    input  logic                   mem_instr_access_fault,
+    input  logic                   mem_instr_page_fault,
+    input  logic                   mem_is_ecall,
+    input  logic                   mem_is_ebreak,
+    input  logic                   mem_is_mret,
+    input  logic                   mem_is_sret,
+    input  logic                   mem_load_fault,
+    input  logic                   mem_store_fault,
+    input  logic                   mem_load_page_fault,
+    input  logic                   mem_store_page_fault,
+    input  logic [XLEN-1:0]        mem_fault_addr,
+
+    // -----------------------
+    // Outputs to WB Stage
+    // -----------------------
+    // Data and address
+    output logic [XLEN-1:0]        wb_pc,
+    output logic [XLEN-1:0]        wb_alu_result,
+    output logic [XLEN-1:0]        wb_mem_rdata,
+    output logic [XLEN-1:0]        wb_fp_result,
+    output logic [4:0]             wb_fp_fflags,
+    output logic [INST_WIDTH-1:0]  wb_instr,
+    output logic [2:0]             wb_instr_len,
+
+    // Write back control
+    output logic                   wb_reg_write_en,
+    output logic                   wb_fp_reg_write_en,
+    output logic [4:0]             wb_rd_addr,
+    output logic [1:0]             wb_wb_sel,
+
+    // CSR & System
+    output logic                   wb_is_csr,
+    output logic [2:0]             wb_csr_op,
+    output logic [XLEN-1:0]        wb_rs1_rdata,
+    output logic [4:0]             wb_rs1_addr,
+    output logic [XLEN-1:0]        wb_imm,
+    output logic                   wb_illegal_instr,
+    output logic                   wb_instr_access_fault,
+    output logic                   wb_instr_page_fault,
+    output logic                   wb_is_ecall,
+    output logic                   wb_is_ebreak,
+    output logic                   wb_is_mret,
+    output logic                   wb_is_sret,
+    output logic                   wb_load_fault,
+    output logic                   wb_store_fault,
+    output logic                   wb_load_page_fault,
+    output logic                   wb_store_page_fault,
+    output logic [XLEN-1:0]        wb_fault_addr
+
+);
+    // Sequential Logic: State Update
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            wb_pc                  <= '0;
+            wb_alu_result          <= '0;
+            wb_mem_rdata           <= '0;
+            wb_fp_result           <= '0;
+            wb_fp_fflags           <= '0;
+            wb_instr               <= '0;
+            wb_instr_len           <= 3'd4;
+            wb_reg_write_en        <= '0;
+            wb_fp_reg_write_en     <= '0;
+            wb_rd_addr             <= '0;
+            wb_wb_sel              <= '0;
+            wb_is_csr              <= '0;
+            wb_csr_op              <= '0;
+            wb_rs1_rdata           <= '0;
+            wb_rs1_addr            <= '0;
+            wb_imm                 <= '0;
+            wb_illegal_instr       <= '0;
+            wb_instr_access_fault <= '0;
+            wb_instr_page_fault   <= '0;
+            wb_is_ecall           <= '0;
+            wb_is_ebreak          <= '0;
+            wb_is_mret            <= '0;
+            wb_is_sret            <= '0;
+            wb_load_fault         <= '0;
+            wb_store_fault        <= '0;
+            wb_load_page_fault    <= '0;
+            wb_store_page_fault   <= '0;
+            wb_fault_addr         <= '0;
+        end else if (flush_en) begin
+            // Target specific control signals to clear. Leave datapath elements intact
+            // to avoid extreme power consumption from massive flip-flop state changes array.
+            wb_reg_write_en       <= '0;
+            wb_fp_reg_write_en    <= '0;
+            wb_is_csr             <= '0;
+            wb_illegal_instr      <= '0;
+            wb_instr_access_fault <= '0;
+            wb_instr_page_fault   <= '0;
+            wb_is_ecall           <= '0;
+            wb_is_ebreak          <= '0;
+            wb_is_mret            <= '0;
+            wb_is_sret            <= '0;
+            wb_load_fault         <= '0;
+            wb_store_fault        <= '0;
+            wb_load_page_fault    <= '0;
+            wb_store_page_fault   <= '0;
+            wb_fault_addr         <= '0;
+        end else if (!stall_en) begin
+            wb_pc                  <= mem_pc;
+            wb_alu_result          <= mem_alu_result;
+            wb_mem_rdata           <= mem_mem_rdata;
+            wb_fp_result           <= mem_fp_result;
+            wb_fp_fflags           <= mem_fp_fflags;
+            wb_instr               <= mem_instr;
+            wb_instr_len           <= mem_instr_len;
+            wb_reg_write_en        <= mem_reg_write_en;
+            wb_fp_reg_write_en     <= mem_fp_reg_write_en;
+            wb_rd_addr             <= mem_rd_addr;
+            wb_wb_sel              <= mem_wb_sel;
+            wb_is_csr              <= mem_is_csr;
+            wb_csr_op              <= mem_csr_op;
+            wb_rs1_rdata           <= mem_rs1_rdata;
+            wb_rs1_addr            <= mem_rs1_addr;
+            wb_imm                 <= mem_imm;
+            wb_illegal_instr       <= mem_illegal_instr;
+            wb_instr_access_fault <= mem_instr_access_fault;
+            wb_instr_page_fault   <= mem_instr_page_fault;
+            wb_is_ecall           <= mem_is_ecall;
+            wb_is_ebreak          <= mem_is_ebreak;
+            wb_is_mret            <= mem_is_mret;
+            wb_is_sret            <= mem_is_sret;
+            wb_load_fault         <= mem_load_fault;
+            wb_store_fault        <= mem_store_fault;
+            wb_load_page_fault    <= mem_load_page_fault;
+            wb_store_page_fault   <= mem_store_page_fault;
+            wb_fault_addr         <= mem_fault_addr;
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/pipeline/wb_stage.sv b/rv64cpu/rtl/core/pipeline/wb_stage.sv
new file mode 100644
index 0000000..b19cb67
--- /dev/null
+++ b/rv64cpu/rtl/core/pipeline/wb_stage.sv
@@ -0,0 +1,168 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/wb_stage.sv
+// Description: Write Back Stage
+// Responsible for writing back to the general-purpose register file after multiplexing the final data (ALU result, memory read data, or PC+4)
+
+module wb_stage
+    import rv64_pkg::*;
+(
+    // Inputs from MEM/WB pipeline register
+    input  logic [XLEN-1:0]        mem_wb_alu_result,
+    input  logic [XLEN-1:0]        mem_wb_mem_rdata,
+    input  logic [XLEN-1:0]        mem_wb_fp_result,
+    input  logic [4:0]             mem_wb_fp_fflags,
+    input  logic [XLEN-1:0]        mem_wb_pc,
+    input  logic [XLEN-1:0]        mem_wb_pc_plus_4,
+    input  logic [INST_WIDTH-1:0]  mem_wb_instr,
+    
+    input  logic                   mem_wb_reg_write_en,
+    input  logic                   mem_wb_fp_reg_write_en,
+    input  logic [4:0]             mem_wb_rd_addr,
+    input  logic [1:0]             mem_wb_wb_sel,
+
+    input  logic                   mem_wb_is_csr,
+    input  logic [2:0]             mem_wb_csr_op,
+    input  logic [XLEN-1:0]        mem_wb_rs1_rdata,
+    input  logic [4:0]             mem_wb_rs1_addr,
+    input  logic [XLEN-1:0]        mem_wb_imm,
+    
+    input  logic                   mem_wb_illegal_instr,
+    input  logic                   mem_wb_is_ecall,
+    input  logic                   mem_wb_is_ebreak,
+    input  logic                   mem_wb_is_mret,
+    input  logic                   mem_wb_is_sret,
+    input  logic                   mem_wb_instr_access_fault,
+    input  logic                   mem_wb_instr_page_fault,
+    input  logic                   mem_wb_load_fault,
+    input  logic                   mem_wb_store_fault,
+    input  logic                   mem_wb_load_page_fault,
+    input  logic                   mem_wb_store_page_fault,
+    input  logic [XLEN-1:0]        mem_wb_fault_addr,
+
+    // Inputs from CSR Unit
+    input  logic [XLEN-1:0]        csr_rdata,         // Data read from CSR unit
+    input  logic                   csr_illegal_access,// CSR rejected the request
+
+    // Outputs to Register File
+    output logic [XLEN-1:0]        wb_rd_wdata,
+    output logic [4:0]             wb_rd_addr,
+    output logic                   wb_reg_write_en,
+    output logic [XLEN-1:0]        wb_fd_wdata,
+    output logic [4:0]             wb_fd_addr,
+    output logic                   wb_fp_write_en,
+    output logic [4:0]             wb_fp_fflags,
+    output logic                   wb_fp_state_dirty,
+    output logic                   wb_fp_fflags_valid,
+
+    // Outputs to CSR Unit
+    output logic                   csr_req,           // CSR request valid
+    output logic [2:0]             csr_op,            // CSR operation type
+    output logic [11:0]            csr_addr,          // CSR address
+    output logic [XLEN-1:0]        csr_rs1_data,      // Source data (from rs1_rdata or zimm)
+
+    // Outputs to Exception Handler (Trap Controller)
+    output logic                   trap_illegal_instr,
+    output logic                   trap_is_ecall,
+    output logic                   trap_is_ebreak,
+    output logic                   trap_is_mret,
+    output logic                   trap_is_sret,
+    output logic [XLEN-1:0]        trap_epc,
+    output logic                   trap_instr_access_fault,
+    output logic                   trap_instr_page_fault,
+    output logic                   trap_load_fault,
+    output logic                   trap_store_fault,
+    output logic                   trap_load_page_fault,
+    output logic                   trap_store_page_fault,
+    output logic [XLEN-1:0]        trap_bad_addr,
+    output logic [INST_WIDTH-1:0]  trap_bad_instr
+);
+
+    // --------------------------
+    // Exception/Trap Detection
+    // --------------------------
+    logic has_trap;
+    logic trap_blocker;
+    assign has_trap     = mem_wb_illegal_instr | mem_wb_is_ecall | mem_wb_is_ebreak |
+                           mem_wb_instr_access_fault | mem_wb_load_fault | mem_wb_store_fault |
+                           mem_wb_instr_page_fault | mem_wb_load_page_fault | mem_wb_store_page_fault;
+    assign trap_blocker = has_trap | csr_illegal_access;
+
+    // --------------------------
+    // CSR Signal Routing
+    // --------------------------
+    assign csr_req  = mem_wb_is_csr & ~has_trap;
+    assign csr_op   = mem_wb_csr_op;
+    assign csr_addr = mem_wb_imm[11:0];
+    
+    // For immediate CSR instructions (CSRRWI, CSRRSI, CSRRCI), 
+    // the source data is the zero-extended rs1_addr field.
+    // mem_wb_csr_op[2] is 1 for immediate variants.
+    assign csr_rs1_data = (mem_wb_csr_op[2]) ? {59'b0, mem_wb_rs1_addr} : mem_wb_rs1_rdata;
+
+    // --------------------------
+    // Data Multiplexer for Register File
+    // --------------------------
+    always_comb begin
+        if (mem_wb_is_csr) begin
+            // CSR reads write the previous CSR value to the destination register
+            wb_rd_wdata = csr_rdata;
+        end else begin
+            case (mem_wb_wb_sel)
+                2'b00: wb_rd_wdata = mem_wb_alu_result;
+                2'b01: wb_rd_wdata = mem_wb_mem_rdata;
+                2'b10: wb_rd_wdata = mem_wb_pc_plus_4;
+                default: wb_rd_wdata = '0;
+            endcase
+        end
+    end
+
+    // --------------------------
+    // Register File Write Control
+    // --------------------------
+    // We only write back if the instruction didn't trap.
+    assign wb_rd_addr      = mem_wb_rd_addr;
+    assign wb_reg_write_en = mem_wb_reg_write_en & ~trap_blocker;
+    assign wb_fd_addr      = mem_wb_rd_addr;
+    assign wb_fd_wdata     = mem_wb_fp_result;
+    assign wb_fp_write_en  = mem_wb_fp_reg_write_en & ~trap_blocker;
+    assign wb_fp_fflags    = mem_wb_fp_fflags;
+    assign wb_fp_state_dirty = wb_fp_write_en | ((|mem_wb_fp_fflags) & ~trap_blocker);
+    assign wb_fp_fflags_valid = (|mem_wb_fp_fflags) & ~trap_blocker;
+
+    // --------------------------
+    // Trap / Exception Signals
+    // --------------------------
+    assign trap_illegal_instr      = (mem_wb_illegal_instr &
+                                      ~mem_wb_instr_access_fault &
+                                      ~mem_wb_instr_page_fault) |
+                                     csr_illegal_access;
+    assign trap_is_ecall           = mem_wb_is_ecall;
+    assign trap_is_ebreak          = mem_wb_is_ebreak;
+    assign trap_is_mret            = mem_wb_is_mret;
+    assign trap_is_sret            = mem_wb_is_sret;
+    assign trap_epc                = mem_wb_pc;
+    assign trap_instr_access_fault = mem_wb_instr_access_fault;
+    assign trap_instr_page_fault   = mem_wb_instr_page_fault;
+    assign trap_load_fault         = mem_wb_load_fault;
+    assign trap_store_fault        = mem_wb_store_fault;
+    assign trap_load_page_fault    = mem_wb_load_page_fault;
+    assign trap_store_page_fault   = mem_wb_store_page_fault;
+    assign trap_bad_addr           = mem_wb_fault_addr;
+    assign trap_bad_instr          = mem_wb_instr;
+
+endmodule
diff --git a/rv64cpu/rtl/core/privileged/csr_unit.sv b/rv64cpu/rtl/core/privileged/csr_unit.sv
new file mode 100644
index 0000000..a25e17a
--- /dev/null
+++ b/rv64cpu/rtl/core/privileged/csr_unit.sv
@@ -0,0 +1,1221 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/csr_unit.sv
+
+module csr_unit
+	import rv64_pkg::*;
+	import csr_pkg::*;
+#(
+	parameter logic [XLEN-1:0] HART_ID         = '0,
+	parameter logic [XLEN-1:0] VENDOR_ID       = '0,
+	parameter logic [XLEN-1:0] ARCH_ID         = '0,
+	parameter logic [XLEN-1:0] IMPL_ID         = '0,
+	parameter logic [XLEN-1:0] MISA_VALUE      = 64'h8000_0000_0014_11ad,
+	parameter int              PHYS_ADDR_WIDTH = 56,
+	parameter int              PMP_GRANULARITY = 0
+)(
+	input  logic              clk,
+	input  logic              rst,
+	input  logic              csr_req,
+	input  logic [2:0]        csr_op,
+	input  logic [11:0]       csr_addr,
+	input  logic [XLEN-1:0]   csr_rs1_data,
+	input  logic              trap_ctrl_trigger,
+	input  logic              trap_ctrl_to_s_mode,
+	input  logic [XLEN-1:0]   trap_ctrl_mepc,
+	input  logic [XLEN-1:0]   trap_ctrl_mcause,
+	input  logic [XLEN-1:0]   trap_ctrl_mtval,
+	input  logic              trap_ctrl_mret_en,
+	input  logic              trap_ctrl_sret_en,
+	input  logic              m_timer_interrupt,
+	input  logic              m_software_interrupt,
+	input  logic              m_external_interrupt,
+	input  logic              s_timer_interrupt,
+	input  logic              s_software_interrupt,
+	input  logic              s_external_interrupt,
+	input  logic              fp_state_dirty,
+	input  logic              fp_fflags_valid,
+	input  logic [4:0]        fp_fflags,
+	output logic [XLEN-1:0]   csr_rdata,
+	output logic [XLEN-1:0]   csr_mstatus,
+	output logic [XLEN-1:0]   csr_sstatus,
+	output logic [XLEN-1:0]   csr_medeleg,
+	output logic [XLEN-1:0]   csr_mideleg,
+	output logic [XLEN-1:0]   csr_mie,
+	output logic [XLEN-1:0]   csr_sie,
+	output logic [XLEN-1:0]   csr_mip,
+	output logic [XLEN-1:0]   csr_sip,
+	output logic [XLEN-1:0]   csr_mtvec,
+	output logic [XLEN-1:0]   csr_stvec,
+	output logic [XLEN-1:0]   csr_mepc,
+	output logic [XLEN-1:0]   csr_sepc,
+	output logic [XLEN-1:0]   csr_satp,
+	output logic [2:0]        csr_frm,
+	output logic [1:0]        csr_priv_mode,
+	output logic [XLEN-1:0]   csr_trap_vector,
+	output logic              csr_trap_to_s_mode,
+	output logic [XLEN-1:0]   csr_trap_vector_next,
+	output logic              csr_trap_to_s_mode_next,
+	output pmpcfg_entry_t     csr_pmpcfg_entries [PMP_ENTRY_COUNT],
+	output pmpaddr_entry_t    csr_pmpaddr_entries [PMP_ENTRY_COUNT],
+	output logic              csr_illegal_access
+);
+
+	typedef enum logic [1:0] {
+		PRIV_MODE_U = 2'b00,
+		PRIV_MODE_S = 2'b01,
+		PRIV_MODE_M = 2'b11
+	} priv_mode_t;
+
+	localparam int MSTATUS_MIE_BIT  = 3;
+	localparam int MSTATUS_MPIE_BIT = 7;
+	localparam int MSTATUS_VS_LO    = 9;
+	localparam int MSTATUS_VS_HI    = 10;
+	localparam int MSTATUS_MPP_LO   = 11;
+	localparam int MSTATUS_MPP_HI   = 12;
+	localparam int MSTATUS_FS_LO    = 13;
+	localparam int MSTATUS_FS_HI    = 14;
+	localparam int MSTATUS_XS_LO    = 15;
+	localparam int MSTATUS_XS_HI    = 16;
+	localparam int MSTATUS_MPRV_BIT = 17;
+	localparam int MSTATUS_SUM_BIT  = 18;
+	localparam int MSTATUS_MXR_BIT  = 19;
+	localparam int MSTATUS_TVM_BIT  = 20;
+	localparam int MSTATUS_TW_BIT   = 21;
+	localparam int MSTATUS_TSR_BIT  = 22;
+	localparam int MSTATUS_UXL_LO   = 32;
+	localparam int MSTATUS_UXL_HI   = 33;
+	localparam int MSTATUS_SXL_LO   = 34;
+	localparam int MSTATUS_SXL_HI   = 35;
+	localparam int MSTATUS_SD_BIT   = 63;
+	localparam logic [1:0] STATUS_XLEN_64 = 2'b10;
+	localparam int SSTATUS_SIE_BIT  = 1;
+	localparam int SSTATUS_SPIE_BIT = 5;
+	localparam int SSTATUS_SPP_BIT  = 8;
+	localparam int SSTATUS_VS_LO    = MSTATUS_VS_LO;
+	localparam int SSTATUS_VS_HI    = MSTATUS_VS_HI;
+	localparam int SSTATUS_FS_LO    = MSTATUS_FS_LO;
+	localparam int SSTATUS_FS_HI    = MSTATUS_FS_HI;
+	localparam int SSTATUS_XS_LO    = MSTATUS_XS_LO;
+	localparam int SSTATUS_XS_HI    = MSTATUS_XS_HI;
+	localparam int SSTATUS_SUM_BIT  = MSTATUS_SUM_BIT;
+	localparam int SSTATUS_MXR_BIT  = MSTATUS_MXR_BIT;
+	localparam int SSTATUS_UXL_LO   = MSTATUS_UXL_LO;
+	localparam int SSTATUS_UXL_HI   = MSTATUS_UXL_HI;
+	localparam int SSTATUS_SD_BIT   = MSTATUS_SD_BIT;
+	localparam int SATP_MODE_LO     = 60;
+	localparam int SATP_MODE_HI     = 63;
+	localparam int SATP_ASID_LO     = 44;
+	localparam int SATP_ASID_HI     = 59;
+	localparam int SATP_PPN_LO      = 0;
+	localparam int SATP_PPN_HI      = 43;
+	localparam logic [3:0] SATP_MODE_BARE = 4'd0;
+	localparam logic [3:0] SATP_MODE_SV39 = 4'd8;
+	localparam logic [3:0] SATP_MODE_SV48 = 4'd9;  // Stub: unsupported until Sv48 is implemented.
+	localparam logic [3:0] SATP_MODE_SV57 = 4'd10; // Stub: unsupported until Sv57 is implemented.
+	localparam logic [3:0] SATP_MODE_SV64 = 4'd11; // Reserved/future stub.
+	localparam int MCOUNTINHIBIT_CY_BIT = 0;
+	localparam int MCOUNTINHIBIT_IR_BIT = 2;
+	localparam int COUNTEREN_CY_BIT = 0;
+	localparam int COUNTEREN_TM_BIT = 1;
+	localparam int COUNTEREN_IR_BIT = 2;
+	localparam int HPM_COUNTER_COUNT = 29;
+	localparam int INTERRUPT_S_SOFTWARE_BIT = int'(INTERRUPT_S_SOFTWARE);
+	localparam int INTERRUPT_S_TIMER_BIT    = int'(INTERRUPT_S_TIMER);
+	localparam int INTERRUPT_S_EXTERNAL_BIT = int'(INTERRUPT_S_EXTERNAL);
+	localparam int INTERRUPT_M_SOFTWARE_BIT = int'(INTERRUPT_M_SOFTWARE);
+	localparam int INTERRUPT_M_TIMER_BIT    = int'(INTERRUPT_M_TIMER);
+	localparam int INTERRUPT_M_EXTERNAL_BIT = int'(INTERRUPT_M_EXTERNAL);
+	localparam logic [XLEN-1:0] S_INTERRUPT_MASK =
+		(64'h1 << INTERRUPT_S_SOFTWARE) |
+		(64'h1 << INTERRUPT_S_TIMER) |
+		(64'h1 << INTERRUPT_S_EXTERNAL);
+	localparam logic [XLEN-1:0] M_INTERRUPT_MASK =
+		(64'h1 << INTERRUPT_M_SOFTWARE) |
+		(64'h1 << INTERRUPT_M_TIMER) |
+		(64'h1 << INTERRUPT_M_EXTERNAL);
+	localparam logic [XLEN-1:0] INTERRUPT_MASK = S_INTERRUPT_MASK | M_INTERRUPT_MASK;
+
+	// Machine-mode CSRs
+	logic [XLEN-1:0] mstatus_reg;
+	logic [XLEN-1:0] medeleg_reg;
+	logic [XLEN-1:0] mideleg_reg;
+	logic [XLEN-1:0] mie_reg;
+	logic [XLEN-1:0] mtvec_reg;
+	logic [XLEN-1:0] mcounteren_reg;
+	logic [XLEN-1:0] mscratch_reg;
+	logic [XLEN-1:0] mepc_reg;
+	logic [XLEN-1:0] mcause_reg;
+	logic [XLEN-1:0] mtval_reg;
+	logic [XLEN-1:0] mip_reg;
+	logic [XLEN-1:0] mtinst_reg;
+	logic [XLEN-1:0] mtval2_reg;
+	logic [XLEN-1:0] menvcfg_reg;
+	logic [XLEN-1:0] mstateen_reg [4];
+	pmpcfg_entry_t  pmpcfg_entries [PMP_ENTRY_COUNT];
+	pmpaddr_entry_t pmpaddr_entries [PMP_ENTRY_COUNT];
+
+	// Supervisor-mode CSRs
+	logic [XLEN-1:0] sstatus_reg;
+	logic [XLEN-1:0] stvec_reg;
+	logic [XLEN-1:0] scounteren_reg;
+	logic [XLEN-1:0] senvcfg_reg;
+	logic [XLEN-1:0] sstateen_reg [4];
+	logic [XLEN-1:0] sscratch_reg;
+	logic [XLEN-1:0] sepc_reg;
+	logic [XLEN-1:0] scause_reg;
+	logic [XLEN-1:0] stval_reg;
+	logic [XLEN-1:0] satp_reg;
+	logic [XLEN-1:0] hstatus_reg;
+	logic [XLEN-1:0] hedeleg_reg;
+	logic [XLEN-1:0] hideleg_reg;
+	logic [XLEN-1:0] hie_reg;
+	logic [XLEN-1:0] htimedelta_reg;
+	logic [XLEN-1:0] hcounteren_reg;
+	logic [XLEN-1:0] hgeie_reg;
+	logic [XLEN-1:0] henvcfg_reg;
+	logic [XLEN-1:0] hstateen_reg [4];
+	logic [XLEN-1:0] htval_reg;
+	logic [XLEN-1:0] hip_reg;
+	logic [XLEN-1:0] hvip_reg;
+	logic [XLEN-1:0] htinst_reg;
+	logic [XLEN-1:0] hgatp_reg;
+	logic [XLEN-1:0] vsstatus_reg;
+	logic [XLEN-1:0] vsie_reg;
+	logic [XLEN-1:0] vstvec_reg;
+	logic [XLEN-1:0] vsscratch_reg;
+	logic [XLEN-1:0] vsepc_reg;
+	logic [XLEN-1:0] vscause_reg;
+	logic [XLEN-1:0] vstval_reg;
+	logic [XLEN-1:0] vsip_reg;
+	logic [XLEN-1:0] vsatp_reg;
+
+	// Counters
+	logic [XLEN-1:0] mcycle_reg;
+	logic [XLEN-1:0] minstret_reg;
+	logic [XLEN-1:0] time_reg;
+	logic [XLEN-1:0] mcountinhibit_reg;
+	logic [XLEN-1:0] mhpmcounter_reg [HPM_COUNTER_COUNT];
+
+	// Floating-point CSRs
+	logic [4:0]       fflags_reg;
+	logic [2:0]       frm_reg;
+
+	priv_mode_t      priv_mode_q;
+
+	logic [11:0]     csr_addr_canonical;
+	logic            is_write_cmd;
+	logic            is_set_cmd;
+	logic            is_clear_cmd;
+	logic            has_non_zero_data;
+	logic            csr_write_en;
+	logic [XLEN-1:0] trap_vector_target;
+	logic            csr_addr_supported;
+	logic            csr_privilege_illegal;
+	logic            csr_write_illegal;
+	logic            csr_stateen_illegal;
+	logic [XLEN-1:0] csr_read_data;
+	logic [XLEN-1:0] mip_value;
+	logic [XLEN-1:0] sip_value;
+	logic [XLEN-1:0] sie_value;
+	logic [XLEN-1:0] trap_vector_q;
+	logic            trap_to_s_q;
+	logic            trap_taken;
+	logic            trap_taken_to_s_mode;
+	logic [XLEN-1:0] trap_taken_mepc;
+	logic [XLEN-1:0] trap_taken_mcause;
+	logic [XLEN-1:0] trap_taken_mtval;
+
+	// Canonicalize addresses, decode operations, and prepare CSR/trap metadata.
+	assign csr_addr_canonical = canonical_addr(csr_addr);
+	assign is_write_cmd       = (csr_op == 3'b001) || (csr_op == 3'b101);
+	assign is_set_cmd         = (csr_op == 3'b010) || (csr_op == 3'b110);
+	assign is_clear_cmd       = (csr_op == 3'b011) || (csr_op == 3'b111);
+	assign has_non_zero_data  = |csr_rs1_data;
+	assign csr_addr_supported = csr_is_supported(csr_addr_canonical);
+	assign csr_write_en       = csr_req & csr_addr_supported & !csr_privilege_illegal &
+	                            !csr_write_illegal & !csr_stateen_illegal &
+	                            (is_write_cmd | ((is_set_cmd | is_clear_cmd) & has_non_zero_data));
+	assign trap_taken               = trap_ctrl_trigger;
+	assign trap_taken_to_s_mode     = trap_ctrl_to_s_mode;
+	assign trap_taken_mepc          = trap_ctrl_mepc;
+	assign trap_taken_mcause        = trap_ctrl_mcause;
+	assign trap_taken_mtval         = trap_ctrl_mtval;
+
+	assign trap_vector_target      = trap_taken_to_s_mode ? stvec_reg : mtvec_reg;
+	assign csr_read_data           = read_csr(csr_addr_canonical);
+	assign csr_rdata               = csr_read_data;
+	assign mip_value               = compose_mip();
+	assign sip_value               = mip_value & S_INTERRUPT_MASK;
+	assign sie_value               = mie_reg & S_INTERRUPT_MASK;
+	assign csr_mstatus             = mstatus_reg;
+	assign csr_sstatus             = sstatus_reg;
+	assign csr_medeleg             = medeleg_reg;
+	assign csr_mideleg             = mideleg_reg;
+	assign csr_mie                 = mie_reg;
+	assign csr_sie                 = sie_value;
+	assign csr_mip                 = mip_value;
+	assign csr_sip                 = sip_value;
+	assign csr_mtvec               = mtvec_reg;
+	assign csr_stvec               = stvec_reg;
+	assign csr_mepc                = mepc_reg;
+	assign csr_sepc                = sepc_reg;
+	assign csr_satp                = satp_reg;
+	assign csr_frm                 = frm_reg;
+	assign csr_priv_mode           = priv_mode_q;
+	assign csr_trap_vector         = trap_vector_q;
+	assign csr_trap_to_s_mode      = trap_to_s_q;
+	assign csr_trap_vector_next    = trap_vector_target;
+	assign csr_trap_to_s_mode_next = trap_taken_to_s_mode;
+	assign csr_write_illegal       = csr_req && csr_is_read_only_by_encoding(csr_addr) &&
+	                                 (is_write_cmd || ((is_set_cmd || is_clear_cmd) && has_non_zero_data));
+	assign csr_privilege_illegal   = csr_req && csr_addr_supported &&
+	                                 !csr_privilege_allows_access(csr_addr, csr_addr_canonical);
+	assign csr_stateen_illegal     = csr_req && csr_access_blocked_by_stateen(csr_addr, csr_addr_canonical);
+	assign csr_illegal_access      = csr_req && (!csr_addr_supported || csr_privilege_illegal ||
+	                                             csr_write_illegal || csr_stateen_illegal);
+
+	for (genvar pmp_entry_idx = 0; pmp_entry_idx < PMP_ENTRY_COUNT; pmp_entry_idx++) begin : gen_pmp_outputs
+		assign csr_pmpcfg_entries[pmp_entry_idx] = pmpcfg_entries[pmp_entry_idx];
+		assign csr_pmpaddr_entries[pmp_entry_idx] = pmpaddr_entries[pmp_entry_idx];
+	end
+
+	// Main CSR state machine: counts cycles, processes CSR writes, and reacts to traps/returns.
+	always_ff @(posedge clk) begin
+		if (rst) begin
+			mstatus_reg      <= reset_mstatus_value();
+			medeleg_reg      <= legalize_medeleg('0);
+			mideleg_reg      <= '0;
+			mie_reg          <= '0;
+			mtvec_reg        <= '0;
+			mcounteren_reg   <= '0;
+			mscratch_reg     <= '0;
+			mepc_reg         <= '0;
+			mcause_reg       <= '0;
+			mtval_reg        <= '0;
+			mip_reg          <= '0;
+			mtinst_reg       <= '0;
+			mtval2_reg       <= '0;
+			menvcfg_reg      <= '0;
+			for (int unsigned stateen_idx = 0; stateen_idx < 4; stateen_idx++) begin
+				mstateen_reg[stateen_idx] <= '0;
+				sstateen_reg[stateen_idx] <= '0;
+				hstateen_reg[stateen_idx] <= '0;
+			end
+			for (int unsigned pmp_entry_idx = 0; pmp_entry_idx < PMP_ENTRY_COUNT; pmp_entry_idx++) begin
+				pmpcfg_entries[pmp_entry_idx] <= '0;
+				pmpaddr_entries[pmp_entry_idx] <= '0;
+			end
+			sstatus_reg      <= reset_sstatus_value();
+			stvec_reg        <= '0;
+			scounteren_reg   <= '0;
+			senvcfg_reg      <= '0;
+			sscratch_reg     <= '0;
+			sepc_reg         <= '0;
+			scause_reg       <= '0;
+			stval_reg        <= '0;
+			satp_reg         <= '0;
+			hstatus_reg      <= '0;
+			hedeleg_reg      <= '0;
+			hideleg_reg      <= '0;
+			hie_reg          <= '0;
+			htimedelta_reg   <= '0;
+			hcounteren_reg   <= '0;
+			hgeie_reg        <= '0;
+			henvcfg_reg      <= '0;
+			htval_reg        <= '0;
+			hip_reg          <= '0;
+			hvip_reg         <= '0;
+			htinst_reg       <= '0;
+			hgatp_reg        <= '0;
+			vsstatus_reg     <= reset_sstatus_value();
+			vsie_reg         <= '0;
+			vstvec_reg       <= '0;
+			vsscratch_reg    <= '0;
+			vsepc_reg        <= '0;
+			vscause_reg      <= '0;
+			vstval_reg       <= '0;
+			vsip_reg         <= '0;
+			vsatp_reg        <= '0;
+			mcycle_reg         <= '0;
+			minstret_reg       <= '0;
+			time_reg           <= '0;
+			mcountinhibit_reg  <= '0;
+			for (int unsigned hpm_idx = 0; hpm_idx < HPM_COUNTER_COUNT; hpm_idx++) begin
+				mhpmcounter_reg[hpm_idx] <= '0;
+			end
+			fflags_reg       <= '0;
+			frm_reg          <= '0;
+			priv_mode_q      <= PRIV_MODE_M;
+			trap_vector_q    <= '0;
+			trap_to_s_q      <= 1'b0;
+		end else begin
+			if (!mcountinhibit_reg[MCOUNTINHIBIT_CY_BIT]) begin
+				mcycle_reg <= mcycle_reg + 64'd1;
+			end
+			time_reg <= time_reg + 64'd1;
+			if (!trap_taken && !mcountinhibit_reg[MCOUNTINHIBIT_IR_BIT]) begin
+				minstret_reg <= minstret_reg + 64'd1;
+			end
+
+			if (csr_write_en && csr_is_writable(csr_addr_canonical)) begin
+				if (csr_is_pmpcfg(csr_addr_canonical)) begin
+					write_pmpcfg_csr(csr_addr_canonical, csr_write_next(read_csr(csr_addr_canonical), csr_rs1_data, csr_op));
+				end else if (csr_is_pmpaddr(csr_addr_canonical)) begin
+					write_pmpaddr_csr(csr_addr_canonical, csr_write_next(read_csr(csr_addr_canonical), csr_rs1_data, csr_op));
+				end else if (csr_is_mhpmcounter(csr_addr_canonical)) begin
+					mhpmcounter_reg[hpmcounter_index(csr_addr_canonical)] <=
+						csr_write_next(read_csr(csr_addr_canonical), csr_rs1_data, csr_op);
+				end else begin
+					unique case (csr_addr_canonical)
+						CSR_MSTATUS: begin
+							logic [XLEN-1:0] next_data;
+							logic [XLEN-1:0] legal_data;
+							next_data    = csr_write_next(mstatus_reg, csr_rs1_data, csr_op);
+							legal_data   = legalize_mstatus(next_data);
+							mstatus_reg  <= legal_data;
+							sstatus_reg  <= mirror_mstatus_to_sstatus(sstatus_reg, legal_data);
+						end
+						CSR_MEDELEG:    medeleg_reg    <= legalize_medeleg(csr_write_next(medeleg_reg, csr_rs1_data, csr_op));
+						CSR_MIDELEG:    mideleg_reg    <= legalize_mideleg(csr_write_next(mideleg_reg, csr_rs1_data, csr_op));
+						CSR_MIE:        mie_reg        <= legalize_interrupt_csr(csr_write_next(mie_reg, csr_rs1_data, csr_op));
+						CSR_MTVEC:      mtvec_reg      <= csr_write_next(mtvec_reg, csr_rs1_data, csr_op);
+						CSR_MSCRATCH:   mscratch_reg   <= csr_write_next(mscratch_reg, csr_rs1_data, csr_op);
+						CSR_MEPC:       mepc_reg       <= csr_write_next(mepc_reg, csr_rs1_data, csr_op);
+						CSR_MCAUSE:     mcause_reg     <= csr_write_next(mcause_reg, csr_rs1_data, csr_op);
+						CSR_MTVAL:      mtval_reg      <= csr_write_next(mtval_reg, csr_rs1_data, csr_op);
+						CSR_MIP:        mip_reg        <= legalize_interrupt_csr(csr_write_next(mip_reg, csr_rs1_data, csr_op));
+						CSR_MTINST:     mtinst_reg     <= '0;
+						CSR_MTVAL2:     mtval2_reg     <= csr_write_next(mtval2_reg, csr_rs1_data, csr_op);
+						CSR_MCOUNTEREN: mcounteren_reg <= legalize_counteren(csr_write_next(mcounteren_reg, csr_rs1_data, csr_op));
+						CSR_MCOUNTINHIBIT: mcountinhibit_reg <= legalize_mcountinhibit(csr_write_next(mcountinhibit_reg, csr_rs1_data, csr_op));
+						CSR_MENVCFG:    menvcfg_reg    <= legalize_menvcfg(csr_write_next(menvcfg_reg, csr_rs1_data, csr_op));
+						CSR_MSTATEEN0:  mstateen_reg[0] <= legalize_mstateen(0, csr_write_next(mstateen_reg[0], csr_rs1_data, csr_op));
+						CSR_MSTATEEN1:  mstateen_reg[1] <= legalize_mstateen(1, csr_write_next(mstateen_reg[1], csr_rs1_data, csr_op));
+						CSR_MSTATEEN2:  mstateen_reg[2] <= legalize_mstateen(2, csr_write_next(mstateen_reg[2], csr_rs1_data, csr_op));
+						CSR_MSTATEEN3:  mstateen_reg[3] <= legalize_mstateen(3, csr_write_next(mstateen_reg[3], csr_rs1_data, csr_op));
+						CSR_SSTATUS: begin
+							logic [XLEN-1:0] next_sstatus;
+							next_sstatus = csr_write_next(sstatus_reg, csr_rs1_data, csr_op);
+							sstatus_reg  <= legalize_sstatus(next_sstatus);
+							mstatus_reg  <= apply_sstatus_to_mstatus(mstatus_reg, next_sstatus);
+						end
+						CSR_SIE:        mie_reg        <= (mie_reg & ~S_INTERRUPT_MASK) |
+						                                  (legalize_interrupt_csr(csr_write_next(sie_value, csr_rs1_data, csr_op)) &
+						                                   S_INTERRUPT_MASK);
+						CSR_STVEC:      stvec_reg      <= csr_write_next(stvec_reg, csr_rs1_data, csr_op);
+						CSR_SCOUNTEREN: scounteren_reg <= legalize_counteren(csr_write_next(scounteren_reg, csr_rs1_data, csr_op));
+						CSR_SENVCFG:    senvcfg_reg    <= legalize_senvcfg(csr_write_next(senvcfg_reg, csr_rs1_data, csr_op));
+						CSR_SSTATEEN0:  sstateen_reg[0] <= legalize_sstateen(0, csr_write_next(sstateen_reg[0], csr_rs1_data, csr_op));
+						CSR_SSTATEEN1:  sstateen_reg[1] <= legalize_sstateen(1, csr_write_next(sstateen_reg[1], csr_rs1_data, csr_op));
+						CSR_SSTATEEN2:  sstateen_reg[2] <= legalize_sstateen(2, csr_write_next(sstateen_reg[2], csr_rs1_data, csr_op));
+						CSR_SSTATEEN3:  sstateen_reg[3] <= legalize_sstateen(3, csr_write_next(sstateen_reg[3], csr_rs1_data, csr_op));
+						CSR_SSCRATCH:   sscratch_reg   <= csr_write_next(sscratch_reg, csr_rs1_data, csr_op);
+						CSR_SEPC:       sepc_reg       <= csr_write_next(sepc_reg, csr_rs1_data, csr_op);
+						CSR_SCAUSE:     scause_reg     <= csr_write_next(scause_reg, csr_rs1_data, csr_op);
+						CSR_STVAL:      stval_reg      <= csr_write_next(stval_reg, csr_rs1_data, csr_op);
+						CSR_SIP:        mip_reg        <= (mip_reg & ~S_INTERRUPT_MASK) |
+						                                  (legalize_interrupt_csr(csr_write_next(sip_value, csr_rs1_data, csr_op)) &
+						                                   S_INTERRUPT_MASK);
+						CSR_SATP: begin
+							logic [XLEN-1:0] next_satp;
+							next_satp = csr_write_next(satp_reg, csr_rs1_data, csr_op);
+							satp_reg  <= legalize_satp(next_satp, satp_reg);
+						end
+						CSR_HSTATUS:    hstatus_reg    <= legalize_hstatus(csr_write_next(hstatus_reg, csr_rs1_data, csr_op));
+						CSR_HEDELEG:    hedeleg_reg    <= legalize_medeleg(csr_write_next(hedeleg_reg, csr_rs1_data, csr_op));
+						CSR_HIDELEG:    hideleg_reg    <= legalize_mideleg(csr_write_next(hideleg_reg, csr_rs1_data, csr_op));
+						CSR_HIE:        hie_reg        <= '0;
+						CSR_HTIMEDELTA: htimedelta_reg <= csr_write_next(htimedelta_reg, csr_rs1_data, csr_op);
+						CSR_HCOUNTEREN: hcounteren_reg <= legalize_counteren(csr_write_next(hcounteren_reg, csr_rs1_data, csr_op));
+						CSR_HGEIE:      hgeie_reg      <= '0;
+						CSR_HENVCFG:    henvcfg_reg    <= legalize_menvcfg(csr_write_next(henvcfg_reg, csr_rs1_data, csr_op));
+						CSR_HSTATEEN0:  hstateen_reg[0] <= legalize_mstateen(0, csr_write_next(hstateen_reg[0], csr_rs1_data, csr_op));
+						CSR_HSTATEEN1:  hstateen_reg[1] <= legalize_mstateen(1, csr_write_next(hstateen_reg[1], csr_rs1_data, csr_op));
+						CSR_HSTATEEN2:  hstateen_reg[2] <= legalize_mstateen(2, csr_write_next(hstateen_reg[2], csr_rs1_data, csr_op));
+						CSR_HSTATEEN3:  hstateen_reg[3] <= legalize_mstateen(3, csr_write_next(hstateen_reg[3], csr_rs1_data, csr_op));
+						CSR_HTVAL:      htval_reg      <= csr_write_next(htval_reg, csr_rs1_data, csr_op);
+						CSR_HIP:        hip_reg        <= '0;
+						CSR_HVIP:       hvip_reg       <= '0;
+						CSR_HTINST:     htinst_reg     <= '0;
+						CSR_HGATP:      hgatp_reg      <= legalize_hgatp(csr_write_next(hgatp_reg, csr_rs1_data, csr_op), hgatp_reg);
+						CSR_VSSTATUS:   vsstatus_reg   <= legalize_sstatus(csr_write_next(vsstatus_reg, csr_rs1_data, csr_op));
+						CSR_VSIE:       vsie_reg       <= '0;
+						CSR_VSTVEC:     vstvec_reg     <= csr_write_next(vstvec_reg, csr_rs1_data, csr_op);
+						CSR_VSSCRATCH:  vsscratch_reg  <= csr_write_next(vsscratch_reg, csr_rs1_data, csr_op);
+						CSR_VSEPC:      vsepc_reg      <= csr_write_next(vsepc_reg, csr_rs1_data, csr_op);
+						CSR_VSCAUSE:    vscause_reg    <= csr_write_next(vscause_reg, csr_rs1_data, csr_op);
+						CSR_VSTVAL:     vstval_reg     <= csr_write_next(vstval_reg, csr_rs1_data, csr_op);
+						CSR_VSIP:       vsip_reg       <= '0;
+						CSR_VSATP: begin
+							logic [XLEN-1:0] next_vsatp;
+							next_vsatp = csr_write_next(vsatp_reg, csr_rs1_data, csr_op);
+							vsatp_reg  <= legalize_satp(next_vsatp, vsatp_reg);
+						end
+						CSR_MCYCLE:     mcycle_reg     <= csr_write_next(mcycle_reg, csr_rs1_data, csr_op);
+						CSR_MINSTRET:   minstret_reg   <= csr_write_next(minstret_reg, csr_rs1_data, csr_op);
+						CSR_FFLAGS: begin
+							logic [XLEN-1:0] next_flags;
+							next_flags  = csr_write_next({{(XLEN-5){1'b0}}, fflags_reg}, csr_rs1_data, csr_op);
+							fflags_reg  <= next_flags[4:0];
+							mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+							sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+						end
+						CSR_FRM: begin
+							logic [XLEN-1:0] next_frm;
+							next_frm    = csr_write_next({{(XLEN-3){1'b0}}, frm_reg}, csr_rs1_data, csr_op);
+							frm_reg     <= next_frm[2:0];
+							mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+							sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+						end
+						CSR_FCSR: begin
+							logic [XLEN-1:0] next_fcsr;
+							next_fcsr   = csr_write_next(compose_fcsr(), csr_rs1_data, csr_op);
+							fflags_reg  <= next_fcsr[4:0];
+							frm_reg     <= next_fcsr[7:5];
+							mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+							sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+						end
+						default: ;
+					endcase
+				end
+			end
+
+			if (fp_state_dirty) begin
+				mstatus_reg <= set_mstatus_fs_dirty(mstatus_reg);
+				sstatus_reg <= set_sstatus_fs_dirty(sstatus_reg);
+			end
+
+			if (fp_fflags_valid) begin
+				fflags_reg <= fflags_reg | fp_fflags;
+			end
+
+			if (trap_taken) begin
+				trap_vector_q <= trap_vector_target;
+				trap_to_s_q   <= trap_taken_to_s_mode;
+				// Update privilege-specific CSRs based on whether the trap was delegated.
+				if (trap_taken_to_s_mode) begin
+					logic [XLEN-1:0] next_sstatus;
+					next_sstatus = sstatus_reg;
+					next_sstatus[SSTATUS_SPIE_BIT] = sstatus_reg[SSTATUS_SIE_BIT];
+					next_sstatus[SSTATUS_SIE_BIT]  = 1'b0;
+					next_sstatus[SSTATUS_SPP_BIT]  = (priv_mode_q == PRIV_MODE_S);
+					sstatus_reg <= next_sstatus;
+					mstatus_reg <= apply_sstatus_to_mstatus(mstatus_reg, next_sstatus);
+					sepc_reg    <= trap_taken_mepc;
+					scause_reg  <= trap_taken_mcause;
+					stval_reg   <= trap_taken_mtval;
+					priv_mode_q <= PRIV_MODE_S;
+				end else begin
+					logic [XLEN-1:0] next_mstatus;
+					next_mstatus = mstatus_reg;
+					next_mstatus[MSTATUS_MPIE_BIT]              = mstatus_reg[MSTATUS_MIE_BIT];
+					next_mstatus[MSTATUS_MIE_BIT]               = 1'b0;
+					next_mstatus[MSTATUS_MPP_HI:MSTATUS_MPP_LO] = encode_priv_mode(priv_mode_q);
+					mstatus_reg <= legalize_mstatus(next_mstatus);
+					sstatus_reg <= mirror_mstatus_to_sstatus(sstatus_reg, legalize_mstatus(next_mstatus));
+					mepc_reg    <= trap_taken_mepc;
+					mcause_reg  <= trap_taken_mcause;
+					mtval_reg   <= trap_taken_mtval;
+					priv_mode_q <= PRIV_MODE_M;
+				end
+			end else begin
+				if (trap_ctrl_mret_en) begin
+					priv_mode_t return_mode;
+					logic [XLEN-1:0] next_mstatus;
+					return_mode  = decode_priv_mode(mstatus_reg[MSTATUS_MPP_HI:MSTATUS_MPP_LO]);
+					next_mstatus = mstatus_reg;
+					next_mstatus[MSTATUS_MIE_BIT]               = mstatus_reg[MSTATUS_MPIE_BIT];
+					next_mstatus[MSTATUS_MPIE_BIT]              = 1'b1;
+					next_mstatus[MSTATUS_MPP_HI:MSTATUS_MPP_LO] = encode_priv_mode(PRIV_MODE_U);
+					if (return_mode != PRIV_MODE_M) begin
+						next_mstatus[MSTATUS_MPRV_BIT] = 1'b0;
+					end
+					mstatus_reg <= legalize_mstatus(next_mstatus);
+					sstatus_reg <= mirror_mstatus_to_sstatus(sstatus_reg, legalize_mstatus(next_mstatus));
+					priv_mode_q <= return_mode;
+				end
+
+				if (trap_ctrl_sret_en) begin
+					logic [XLEN-1:0] next_sstatus;
+					logic [XLEN-1:0] next_mstatus;
+					next_sstatus = sstatus_reg;
+					next_sstatus[SSTATUS_SIE_BIT]  = sstatus_reg[SSTATUS_SPIE_BIT];
+					next_sstatus[SSTATUS_SPIE_BIT] = 1'b1;
+					next_sstatus[SSTATUS_SPP_BIT]  = 1'b0;
+					next_mstatus = apply_sstatus_to_mstatus(mstatus_reg, next_sstatus);
+					next_mstatus[MSTATUS_MPRV_BIT] = 1'b0;
+					sstatus_reg <= legalize_sstatus(next_sstatus);
+					mstatus_reg <= legalize_mstatus(next_mstatus);
+					priv_mode_q <= (sstatus_reg[SSTATUS_SPP_BIT]) ? PRIV_MODE_S : PRIV_MODE_U;
+				end
+			end
+		end
+	end
+
+	// Map CSR aliases (cycle/instret) to their canonical machine addresses.
+	function automatic logic [11:0] canonical_addr(input logic [11:0] addr);
+		case (addr)
+			CSR_CYCLE:   canonical_addr = CSR_MCYCLE;
+			CSR_INSTRET: canonical_addr = CSR_MINSTRET;
+			default:     canonical_addr = addr;
+		endcase
+	endfunction
+
+	function automatic bit csr_is_pmpcfg(input logic [11:0] addr);
+		csr_is_pmpcfg = (addr >= CSR_PMPCFG0) && (addr <= CSR_PMPCFG15) && (addr[0] == 1'b0);
+	endfunction
+
+	function automatic bit csr_is_pmpaddr(input logic [11:0] addr);
+		csr_is_pmpaddr = (addr >= CSR_PMPADDR0) && (addr <= CSR_PMPADDR63);
+	endfunction
+
+	function automatic bit csr_is_mhpmcounter(input logic [11:0] addr);
+		csr_is_mhpmcounter = (addr >= CSR_MHPMCOUNTER3) && (addr <= CSR_MHPMCOUNTER31);
+	endfunction
+
+	function automatic bit csr_is_hpmcounter(input logic [11:0] addr);
+		csr_is_hpmcounter = (addr >= CSR_HPMCOUNTER3) && (addr <= CSR_HPMCOUNTER31);
+	endfunction
+
+	function automatic bit csr_is_mhpmevent(input logic [11:0] addr);
+		csr_is_mhpmevent = (addr >= CSR_MHPMEVENT3) && (addr <= CSR_MHPMEVENT31);
+	endfunction
+
+	function automatic bit csr_is_counter(input logic [11:0] addr);
+		return (addr == CSR_CYCLE) || (addr == CSR_TIME) || (addr == CSR_INSTRET) || csr_is_hpmcounter(addr);
+	endfunction
+
+	function automatic int unsigned counter_index(input logic [11:0] addr);
+		if (csr_is_hpmcounter(addr)) begin
+			return int'(addr) - int'(CSR_CYCLE);
+		end
+		case (addr)
+			CSR_CYCLE:   return COUNTEREN_CY_BIT;
+			CSR_TIME:    return COUNTEREN_TM_BIT;
+			CSR_INSTRET: return COUNTEREN_IR_BIT;
+			default:     return COUNTEREN_CY_BIT;
+		endcase
+	endfunction
+
+	function automatic int unsigned hpmcounter_index(input logic [11:0] addr);
+		if (csr_is_hpmcounter(addr)) begin
+			return int'(addr) - int'(CSR_HPMCOUNTER3);
+		end
+		return int'(addr) - int'(CSR_MHPMCOUNTER3);
+	endfunction
+
+	function automatic int unsigned pmpcfg_csr_base_entry(input logic [11:0] addr);
+		return (int'(addr) - int'(CSR_PMPCFG0)) * 4;
+	endfunction
+
+	function automatic int unsigned pmpaddr_csr_entry(input logic [11:0] addr);
+		return int'(addr) - int'(CSR_PMPADDR0);
+	endfunction
+
+	function automatic logic [XLEN-1:0] pmpaddr_writable_mask();
+		logic [XLEN-1:0] mask;
+		mask = '0;
+		for (int bit_idx = 0; bit_idx < (PHYS_ADDR_WIDTH - 2); bit_idx++) begin
+			mask[bit_idx] = 1'b1;
+		end
+		return mask;
+	endfunction
+
+	function automatic bit pmpcfg_byte_is_legal(input pmpcfg_entry_t cfg);
+		logic [1:0] address_matching;
+		address_matching = cfg[PMPCFG_A_HI:PMPCFG_A_LO];
+		if (!cfg[PMPCFG_R_BIT] && cfg[PMPCFG_W_BIT]) begin
+			return 1'b0;
+		end
+		if ((PMP_GRANULARITY >= 2) && (address_matching == PMP_MODE_NA4)) begin
+			return 1'b0;
+		end
+		return 1'b1;
+	endfunction
+
+	function automatic pmpcfg_entry_t legalize_pmpcfg_byte(input pmpcfg_entry_t cfg);
+		pmpcfg_entry_t legal_cfg;
+		legal_cfg = cfg;
+		legal_cfg[6:5] = 2'b00;
+		return legal_cfg;
+	endfunction
+
+	function automatic bit pmpaddr_is_locked(input int unsigned entry_idx);
+		bit locked;
+		locked = pmpcfg_entries[entry_idx][PMPCFG_L_BIT];
+		if (entry_idx < (PMP_ENTRY_COUNT - 1)) begin
+			locked |= pmpcfg_entries[entry_idx + 1][PMPCFG_L_BIT] &&
+			          (pmpcfg_entries[entry_idx + 1][PMPCFG_A_HI:PMPCFG_A_LO] == PMP_MODE_TOR);
+		end
+		return locked;
+	endfunction
+
+	function automatic logic [XLEN-1:0] read_pmpcfg_csr(input logic [11:0] addr);
+		logic [XLEN-1:0] data;
+		int unsigned base_entry;
+		data = '0;
+		base_entry = pmpcfg_csr_base_entry(addr);
+		for (int byte_idx = 0; byte_idx < PMP_CFG_ENTRIES_PER_RV64_CSR; byte_idx++) begin
+			if ((base_entry + byte_idx) < PMP_ENTRY_COUNT) begin
+				data[(byte_idx * 8) +: 8] = pmpcfg_entries[base_entry + byte_idx];
+			end
+		end
+		return data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] read_pmpaddr_csr(input logic [11:0] addr);
+		return pmpaddr_entries[pmpaddr_csr_entry(addr)] & pmpaddr_writable_mask();
+	endfunction
+
+	task automatic write_pmpcfg_csr(input logic [11:0] addr, input logic [XLEN-1:0] write_data);
+		int unsigned base_entry;
+		pmpcfg_entry_t next_cfg;
+		base_entry = pmpcfg_csr_base_entry(addr);
+		for (int byte_idx = 0; byte_idx < PMP_CFG_ENTRIES_PER_RV64_CSR; byte_idx++) begin
+			if ((base_entry + byte_idx) < PMP_ENTRY_COUNT) begin
+				next_cfg = legalize_pmpcfg_byte(write_data[(byte_idx * 8) +: 8]);
+				if (!pmpcfg_entries[base_entry + byte_idx][PMPCFG_L_BIT] && pmpcfg_byte_is_legal(next_cfg)) begin
+					pmpcfg_entries[base_entry + byte_idx] <= next_cfg;
+				end
+			end
+		end
+	endtask
+
+	task automatic write_pmpaddr_csr(input logic [11:0] addr, input logic [XLEN-1:0] write_data);
+		int unsigned entry_idx;
+		entry_idx = pmpaddr_csr_entry(addr);
+		if (!pmpaddr_is_locked(entry_idx)) begin
+			pmpaddr_entries[entry_idx] <= write_data & pmpaddr_writable_mask();
+		end
+	endtask
+
+	// Apply the CSR op semantics (write/set/clear) for a single register.
+	function automatic logic [XLEN-1:0] csr_write_next(
+		input logic [XLEN-1:0] current_data,
+		input logic [XLEN-1:0] write_data,
+		input logic [2:0]      op_code
+	);
+		logic [XLEN-1:0] next_data;
+		unique case (op_code)
+			3'b001, 3'b101: next_data = write_data;                     // CSRRW / CSRRWI
+			3'b010, 3'b110: next_data = current_data | write_data;      // CSRRS / CSRRSI
+			3'b011, 3'b111: next_data = current_data & ~write_data;     // CSRRC / CSRRCI
+			default:         next_data = current_data;
+		endcase
+		return next_data;
+	endfunction
+
+	function automatic bit csr_is_writable(input logic [11:0] addr);
+		case (addr)
+			CSR_MVENDORID,
+			CSR_MARCHID,
+			CSR_MIMPID,
+			CSR_MHARTID,
+			CSR_MCONFIGPTR,
+			CSR_TIME: csr_is_writable = 1'b0;
+			default:  csr_is_writable = 1'b1;
+		endcase
+	endfunction
+
+	function automatic bit csr_is_read_only_by_encoding(input logic [11:0] addr);
+		return addr[11:10] == 2'b11;
+	endfunction
+
+	function automatic bit csr_is_supported(input logic [11:0] addr);
+		bit supported;
+		if (csr_is_pmpcfg(addr) || csr_is_pmpaddr(addr) ||
+		    csr_is_mhpmcounter(addr) || csr_is_hpmcounter(addr) || csr_is_mhpmevent(addr)) begin
+			supported = 1'b1;
+		end else begin
+			unique case (addr)
+				CSR_MSTATUS,
+				CSR_MISA,
+				CSR_MEDELEG,
+				CSR_MIDELEG,
+				CSR_MIE,
+				CSR_MTVEC,
+				CSR_MSCRATCH,
+				CSR_MEPC,
+				CSR_MCAUSE,
+				CSR_MTVAL,
+				CSR_MIP,
+				CSR_MTINST,
+				CSR_MTVAL2,
+				CSR_MCOUNTEREN,
+				CSR_MCOUNTINHIBIT,
+				CSR_MENVCFG,
+				CSR_MSTATEEN0,
+				CSR_MSTATEEN1,
+				CSR_MSTATEEN2,
+				CSR_MSTATEEN3,
+				CSR_MVENDORID,
+				CSR_MARCHID,
+				CSR_MIMPID,
+				CSR_MHARTID,
+				CSR_MCONFIGPTR,
+				CSR_SSTATUS,
+				CSR_SIE,
+				CSR_STVEC,
+				CSR_SCOUNTEREN,
+				CSR_SENVCFG,
+				CSR_SSTATEEN0,
+				CSR_SSTATEEN1,
+				CSR_SSTATEEN2,
+				CSR_SSTATEEN3,
+				CSR_SSCRATCH,
+				CSR_SEPC,
+				CSR_SCAUSE,
+				CSR_STVAL,
+				CSR_SIP,
+				CSR_SATP,
+				CSR_HSTATUS,
+				CSR_HEDELEG,
+				CSR_HIDELEG,
+				CSR_HIE,
+				CSR_HTIMEDELTA,
+				CSR_HCOUNTEREN,
+				CSR_HGEIE,
+				CSR_HENVCFG,
+				CSR_HSTATEEN0,
+				CSR_HSTATEEN1,
+				CSR_HSTATEEN2,
+				CSR_HSTATEEN3,
+				CSR_HTVAL,
+				CSR_HIP,
+				CSR_HVIP,
+				CSR_HTINST,
+				CSR_HGATP,
+				CSR_HGEIP,
+				CSR_VSSTATUS,
+				CSR_VSIE,
+				CSR_VSTVEC,
+				CSR_VSSCRATCH,
+				CSR_VSEPC,
+				CSR_VSCAUSE,
+				CSR_VSTVAL,
+				CSR_VSIP,
+				CSR_VSATP,
+				CSR_MCYCLE,
+				CSR_MINSTRET,
+				CSR_CYCLE,
+				CSR_INSTRET,
+				CSR_TIME,
+				CSR_FFLAGS,
+				CSR_FRM,
+				CSR_FCSR:
+					supported = 1'b1;
+				default:
+					supported = 1'b0;
+			endcase
+		end
+		return supported;
+	endfunction
+
+	function automatic bit csr_privilege_allows_access(
+		input logic [11:0] requested_addr,
+		input logic [11:0] canonicalized_addr
+	);
+		logic [1:0] minimum_privilege;
+		minimum_privilege = requested_addr[9:8];
+		if (!csr_is_supported(canonicalized_addr)) begin
+			return 1'b0;
+		end
+		if (minimum_privilege == 2'b10) begin
+			return 1'b0;
+		end
+		return encode_priv_mode(priv_mode_q) >= minimum_privilege;
+	endfunction
+
+	function automatic bit csr_access_blocked_by_stateen(
+		input logic [11:0] requested_addr,
+		input logic [11:0] canonicalized_addr
+	);
+		int unsigned current_counter_index;
+		if (csr_is_counter(requested_addr) && (priv_mode_q != PRIV_MODE_M)) begin
+			current_counter_index = counter_index(requested_addr);
+			if (!mcounteren_reg[current_counter_index]) begin
+				return 1'b1;
+			end
+			if ((priv_mode_q == PRIV_MODE_U) && !scounteren_reg[current_counter_index]) begin
+				return 1'b1;
+			end
+		end
+		if ((canonicalized_addr == CSR_SENVCFG) && (priv_mode_q == PRIV_MODE_S)) begin
+			return !mstateen_reg[0][STATEEN0_ENVCFG_BIT];
+		end
+		return 1'b0;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mstatus(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = data;
+		if (legal_data[MSTATUS_MPP_HI:MSTATUS_MPP_LO] == 2'b10) begin
+			legal_data[MSTATUS_MPP_HI:MSTATUS_MPP_LO] = encode_priv_mode(PRIV_MODE_U);
+		end
+		legal_data[MSTATUS_XS_HI:MSTATUS_XS_LO] = 2'b00;
+		legal_data[MSTATUS_UXL_HI:MSTATUS_UXL_LO] = STATUS_XLEN_64;
+		legal_data[MSTATUS_SXL_HI:MSTATUS_SXL_LO] = STATUS_XLEN_64;
+		legal_data[MSTATUS_SD_BIT] = (legal_data[MSTATUS_FS_HI:MSTATUS_FS_LO] == 2'b11) ||
+		                             (legal_data[MSTATUS_XS_HI:MSTATUS_XS_LO] == 2'b11) ||
+		                             (legal_data[MSTATUS_VS_HI:MSTATUS_VS_LO] == 2'b11);
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_sstatus(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[SSTATUS_SIE_BIT]                  = data[SSTATUS_SIE_BIT];
+		legal_data[SSTATUS_SPIE_BIT]                 = data[SSTATUS_SPIE_BIT];
+		legal_data[SSTATUS_SPP_BIT]                  = data[SSTATUS_SPP_BIT];
+		legal_data[SSTATUS_VS_HI:SSTATUS_VS_LO]      = data[SSTATUS_VS_HI:SSTATUS_VS_LO];
+		legal_data[SSTATUS_FS_HI:SSTATUS_FS_LO]      = data[SSTATUS_FS_HI:SSTATUS_FS_LO];
+		legal_data[SSTATUS_SUM_BIT]                  = data[SSTATUS_SUM_BIT];
+		legal_data[SSTATUS_MXR_BIT]                  = data[SSTATUS_MXR_BIT];
+		legal_data[SSTATUS_UXL_HI:SSTATUS_UXL_LO]    = STATUS_XLEN_64;
+		legal_data[SSTATUS_SD_BIT]                   = (legal_data[SSTATUS_FS_HI:SSTATUS_FS_LO] == 2'b11) ||
+		                                               (legal_data[SSTATUS_XS_HI:SSTATUS_XS_LO] == 2'b11) ||
+		                                               (legal_data[SSTATUS_VS_HI:SSTATUS_VS_LO] == 2'b11);
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] reset_mstatus_value();
+		return legalize_mstatus('0);
+	endfunction
+
+	function automatic logic [XLEN-1:0] reset_sstatus_value();
+		return legalize_sstatus('0);
+	endfunction
+
+	function automatic logic [XLEN-1:0] set_mstatus_fs_dirty(input logic [XLEN-1:0] current);
+		logic [XLEN-1:0] next_data;
+		next_data = current;
+		next_data[MSTATUS_FS_HI:MSTATUS_FS_LO] = 2'b11;
+		return legalize_mstatus(next_data);
+	endfunction
+
+	function automatic logic [XLEN-1:0] set_sstatus_fs_dirty(input logic [XLEN-1:0] current);
+		logic [XLEN-1:0] next_data;
+		next_data = current;
+		next_data[SSTATUS_FS_HI:SSTATUS_FS_LO] = 2'b11;
+		return legalize_sstatus(next_data);
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_medeleg(input logic [XLEN-1:0] data);
+		return (data & MEDELEG_WRITABLE_MASK) | MEDELEG_READ_ONLY_ONE_MASK;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mideleg(input logic [XLEN-1:0] data);
+		return data & MIDELEG_WRITABLE_MASK;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_interrupt_csr(input logic [XLEN-1:0] data);
+		return data & INTERRUPT_MASK;
+	endfunction
+
+	function automatic logic [XLEN-1:0] compose_mip();
+		logic [XLEN-1:0] pending;
+		pending = legalize_interrupt_csr(mip_reg);
+		pending[INTERRUPT_M_SOFTWARE_BIT] |= m_software_interrupt;
+		pending[INTERRUPT_M_TIMER_BIT]    |= m_timer_interrupt;
+		pending[INTERRUPT_M_EXTERNAL_BIT] |= m_external_interrupt;
+		pending[INTERRUPT_S_SOFTWARE_BIT] |= s_software_interrupt;
+		pending[INTERRUPT_S_TIMER_BIT]    |= s_timer_interrupt;
+		pending[INTERRUPT_S_EXTERNAL_BIT] |= s_external_interrupt;
+		return pending;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mcountinhibit(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[MCOUNTINHIBIT_CY_BIT] = data[MCOUNTINHIBIT_CY_BIT];
+		legal_data[MCOUNTINHIBIT_IR_BIT] = data[MCOUNTINHIBIT_IR_BIT];
+		legal_data[31:3] = data[31:3];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_counteren(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[COUNTEREN_CY_BIT] = data[COUNTEREN_CY_BIT];
+		legal_data[COUNTEREN_TM_BIT] = data[COUNTEREN_TM_BIT];
+		legal_data[COUNTEREN_IR_BIT] = data[COUNTEREN_IR_BIT];
+		legal_data[31:3] = data[31:3];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_senvcfg(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[ENVCFG_FIOM_BIT] = data[ENVCFG_FIOM_BIT];
+		legal_data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO] =
+			menvcfg_reg[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO] & data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO];
+		legal_data[ENVCFG_CBCFE_BIT] = menvcfg_reg[ENVCFG_CBCFE_BIT] & data[ENVCFG_CBCFE_BIT];
+		legal_data[ENVCFG_CBZE_BIT] = menvcfg_reg[ENVCFG_CBZE_BIT] & data[ENVCFG_CBZE_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_menvcfg(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[ENVCFG_FIOM_BIT] = data[ENVCFG_FIOM_BIT];
+		legal_data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO] = data[ENVCFG_CBIE_HI:ENVCFG_CBIE_LO];
+		legal_data[ENVCFG_CBCFE_BIT] = data[ENVCFG_CBCFE_BIT];
+		legal_data[ENVCFG_CBZE_BIT] = data[ENVCFG_CBZE_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_hstatus(input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		legal_data[6]  = data[6];   // VTSR
+		legal_data[7]  = data[7];   // VTW
+		legal_data[8]  = data[8];   // VTVM
+		legal_data[12] = data[12];  // SPVP
+		legal_data[14] = data[14];  // SPV
+		legal_data[16:15] = data[16:15]; // GVA and VSBE are WARL here.
+		legal_data[33:32] = STATUS_XLEN_64; // VSXL
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_mstateen(input int unsigned index, input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		if (index == 0) begin
+			legal_data[STATEEN0_ENVCFG_BIT] = data[STATEEN0_ENVCFG_BIT];
+		end
+		legal_data[STATEEN_SE0_BIT] = data[STATEEN_SE0_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_sstateen(input int unsigned index, input logic [XLEN-1:0] data);
+		logic [XLEN-1:0] legal_data;
+		legal_data = '0;
+		if (index == 0) begin
+			legal_data[STATEEN0_ENVCFG_BIT] = data[STATEEN0_ENVCFG_BIT] & mstateen_reg[0][STATEEN0_ENVCFG_BIT];
+		end
+		legal_data[STATEEN_SE0_BIT] = data[STATEEN_SE0_BIT] & mstateen_reg[index][STATEEN_SE0_BIT];
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_satp(
+		input logic [XLEN-1:0] data,
+		input logic [XLEN-1:0] current
+	);
+		logic [XLEN-1:0] legal_data;
+		logic [3:0] mode;
+		mode = data[SATP_MODE_HI:SATP_MODE_LO];
+		legal_data = current;
+
+		unique case (mode)
+			SATP_MODE_BARE: begin
+				legal_data = '0;
+			end
+			SATP_MODE_SV39: begin
+				legal_data = '0;
+				legal_data[SATP_MODE_HI:SATP_MODE_LO] = SATP_MODE_SV39;
+				legal_data[SATP_ASID_HI:SATP_ASID_LO] = data[SATP_ASID_HI:SATP_ASID_LO];
+				legal_data[SATP_PPN_HI:SATP_PPN_LO]   = data[SATP_PPN_HI:SATP_PPN_LO];
+			end
+			SATP_MODE_SV48,
+			SATP_MODE_SV57,
+			SATP_MODE_SV64: begin
+				legal_data = current;
+			end
+			default: begin
+				legal_data = current;
+			end
+		endcase
+
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] legalize_hgatp(
+		input logic [XLEN-1:0] data,
+		input logic [XLEN-1:0] current
+	);
+		logic [XLEN-1:0] legal_data;
+		logic [3:0] mode;
+		mode = data[SATP_MODE_HI:SATP_MODE_LO];
+		legal_data = current;
+
+		unique case (mode)
+			SATP_MODE_BARE: legal_data = '0;
+			SATP_MODE_SV39: legal_data = data;
+			SATP_MODE_SV48,
+			SATP_MODE_SV57,
+			SATP_MODE_SV64: legal_data = current;
+			default: legal_data = current;
+		endcase
+
+		return legal_data;
+	endfunction
+
+	function automatic logic [XLEN-1:0] apply_sstatus_to_mstatus(
+		input logic [XLEN-1:0] mstatus_cur,
+		input logic [XLEN-1:0] sstatus_data
+	);
+		logic [XLEN-1:0] next_data;
+		next_data = mstatus_cur;
+		next_data[SSTATUS_SIE_BIT]                  = sstatus_data[SSTATUS_SIE_BIT];
+		next_data[SSTATUS_SPIE_BIT]                 = sstatus_data[SSTATUS_SPIE_BIT];
+		next_data[SSTATUS_SPP_BIT]                  = sstatus_data[SSTATUS_SPP_BIT];
+		next_data[SSTATUS_VS_HI:SSTATUS_VS_LO]      = sstatus_data[SSTATUS_VS_HI:SSTATUS_VS_LO];
+		next_data[SSTATUS_FS_HI:SSTATUS_FS_LO]      = sstatus_data[SSTATUS_FS_HI:SSTATUS_FS_LO];
+		next_data[SSTATUS_SUM_BIT]                  = sstatus_data[SSTATUS_SUM_BIT];
+		next_data[SSTATUS_MXR_BIT]                  = sstatus_data[SSTATUS_MXR_BIT];
+		next_data[MSTATUS_UXL_HI:MSTATUS_UXL_LO]    = STATUS_XLEN_64;
+		return legalize_mstatus(next_data);
+	endfunction
+
+	function automatic logic [XLEN-1:0] mirror_mstatus_to_sstatus(
+		input logic [XLEN-1:0] sstatus_cur,
+		input logic [XLEN-1:0] mstatus_data
+	);
+		logic [XLEN-1:0] next_data;
+		next_data = legalize_sstatus(sstatus_cur);
+		next_data[SSTATUS_SIE_BIT]                  = mstatus_data[SSTATUS_SIE_BIT];
+		next_data[SSTATUS_SPIE_BIT]                 = mstatus_data[SSTATUS_SPIE_BIT];
+		next_data[SSTATUS_SPP_BIT]                  = mstatus_data[SSTATUS_SPP_BIT];
+		next_data[SSTATUS_VS_HI:SSTATUS_VS_LO]      = mstatus_data[MSTATUS_VS_HI:MSTATUS_VS_LO];
+		next_data[SSTATUS_FS_HI:SSTATUS_FS_LO]      = mstatus_data[SSTATUS_FS_HI:SSTATUS_FS_LO];
+		next_data[SSTATUS_SUM_BIT]                  = mstatus_data[SSTATUS_SUM_BIT];
+		next_data[SSTATUS_MXR_BIT]                  = mstatus_data[MSTATUS_MXR_BIT];
+		next_data[SSTATUS_UXL_HI:SSTATUS_UXL_LO]    = STATUS_XLEN_64;
+		next_data[SSTATUS_SD_BIT]                   = mstatus_data[MSTATUS_SD_BIT];
+		return legalize_sstatus(next_data);
+	endfunction
+
+	// Pack the discrete FP status bits into the architectural FCSR layout.
+	function automatic logic [XLEN-1:0] compose_fcsr();
+		logic [XLEN-1:0] value;
+		value = '0;
+		value[4:0] = fflags_reg;
+		value[7:5] = frm_reg;
+		return value;
+	endfunction
+
+	function automatic priv_mode_t decode_priv_mode(input logic [1:0] encoded);
+		case (encoded)
+			2'b00: decode_priv_mode = PRIV_MODE_U;
+			2'b01: decode_priv_mode = PRIV_MODE_S;
+			default: decode_priv_mode = PRIV_MODE_M;
+		endcase
+	endfunction
+
+	function automatic logic [1:0] encode_priv_mode(input priv_mode_t mode);
+		case (mode)
+			PRIV_MODE_U: encode_priv_mode = 2'b00;
+			PRIV_MODE_S: encode_priv_mode = 2'b01;
+			default:     encode_priv_mode = 2'b11;
+		endcase
+	endfunction
+
+	function automatic logic [XLEN-1:0] read_csr(input logic [11:0] addr);
+		logic [XLEN-1:0] data_out;
+		if (csr_is_pmpcfg(addr)) begin
+			data_out = read_pmpcfg_csr(addr);
+		end else if (csr_is_pmpaddr(addr)) begin
+			data_out = read_pmpaddr_csr(addr);
+		end else if (csr_is_mhpmcounter(addr) || csr_is_hpmcounter(addr)) begin
+			data_out = mhpmcounter_reg[hpmcounter_index(addr)];
+		end else if (csr_is_mhpmevent(addr)) begin
+			data_out = '0;
+		end else begin
+			unique case (addr)
+				CSR_FFLAGS:    data_out = {{(XLEN-5){1'b0}}, fflags_reg};
+				CSR_FRM:       data_out = {{(XLEN-3){1'b0}}, frm_reg};
+				CSR_FCSR:      data_out = compose_fcsr();
+				CSR_MSTATUS:   data_out = mstatus_reg;
+				CSR_MISA:      data_out = MISA_VALUE;
+				CSR_MEDELEG:   data_out = medeleg_reg;
+				CSR_MIDELEG:   data_out = mideleg_reg;
+				CSR_MIE:       data_out = mie_reg;
+				CSR_MTVEC:     data_out = mtvec_reg;
+				CSR_MSCRATCH:  data_out = mscratch_reg;
+				CSR_MEPC:      data_out = mepc_reg;
+				CSR_MCAUSE:    data_out = mcause_reg;
+				CSR_MTVAL:     data_out = mtval_reg;
+				CSR_MIP:       data_out = mip_value;
+				CSR_MTINST:    data_out = mtinst_reg;
+				CSR_MTVAL2:    data_out = mtval2_reg;
+				CSR_MCOUNTEREN: data_out = mcounteren_reg;
+				CSR_MCOUNTINHIBIT: data_out = mcountinhibit_reg;
+				CSR_MENVCFG:   data_out = menvcfg_reg;
+				CSR_MSTATEEN0: data_out = mstateen_reg[0];
+				CSR_MSTATEEN1: data_out = mstateen_reg[1];
+				CSR_MSTATEEN2: data_out = mstateen_reg[2];
+				CSR_MSTATEEN3: data_out = mstateen_reg[3];
+				CSR_MVENDORID: data_out = VENDOR_ID;
+				CSR_MARCHID:   data_out = ARCH_ID;
+				CSR_MIMPID:    data_out = IMPL_ID;
+				CSR_MHARTID:   data_out = HART_ID;
+				CSR_MCONFIGPTR: data_out = '0;
+				CSR_SSTATUS:   data_out = sstatus_reg;
+				CSR_SIE:       data_out = sie_value;
+				CSR_STVEC:     data_out = stvec_reg;
+				CSR_SCOUNTEREN: data_out = scounteren_reg;
+				CSR_SENVCFG:   data_out = legalize_senvcfg(senvcfg_reg);
+				CSR_SSTATEEN0: data_out = sstateen_reg[0];
+				CSR_SSTATEEN1: data_out = sstateen_reg[1];
+				CSR_SSTATEEN2: data_out = sstateen_reg[2];
+				CSR_SSTATEEN3: data_out = sstateen_reg[3];
+				CSR_SSCRATCH:  data_out = sscratch_reg;
+				CSR_SEPC:      data_out = sepc_reg;
+				CSR_SCAUSE:    data_out = scause_reg;
+				CSR_STVAL:     data_out = stval_reg;
+				CSR_SIP:       data_out = sip_value;
+				CSR_SATP:      data_out = satp_reg;
+				CSR_HSTATUS:   data_out = hstatus_reg;
+				CSR_HEDELEG:   data_out = hedeleg_reg;
+				CSR_HIDELEG:   data_out = hideleg_reg;
+				CSR_HIE:       data_out = hie_reg;
+				CSR_HTIMEDELTA: data_out = htimedelta_reg;
+				CSR_HCOUNTEREN: data_out = hcounteren_reg;
+				CSR_HGEIE:     data_out = hgeie_reg;
+				CSR_HENVCFG:   data_out = henvcfg_reg;
+				CSR_HSTATEEN0: data_out = hstateen_reg[0];
+				CSR_HSTATEEN1: data_out = hstateen_reg[1];
+				CSR_HSTATEEN2: data_out = hstateen_reg[2];
+				CSR_HSTATEEN3: data_out = hstateen_reg[3];
+				CSR_HTVAL:     data_out = htval_reg;
+				CSR_HIP:       data_out = hip_reg;
+				CSR_HVIP:      data_out = hvip_reg;
+				CSR_HTINST:    data_out = htinst_reg;
+				CSR_HGATP:     data_out = hgatp_reg;
+				CSR_HGEIP:     data_out = '0;
+				CSR_VSSTATUS:  data_out = vsstatus_reg;
+				CSR_VSIE:      data_out = vsie_reg;
+				CSR_VSTVEC:    data_out = vstvec_reg;
+				CSR_VSSCRATCH: data_out = vsscratch_reg;
+				CSR_VSEPC:     data_out = vsepc_reg;
+				CSR_VSCAUSE:   data_out = vscause_reg;
+				CSR_VSTVAL:    data_out = vstval_reg;
+				CSR_VSIP:      data_out = vsip_reg;
+				CSR_VSATP:     data_out = vsatp_reg;
+				CSR_MCYCLE:    data_out = mcycle_reg;
+				CSR_MINSTRET:  data_out = minstret_reg;
+				CSR_CYCLE:     data_out = mcycle_reg;
+				CSR_INSTRET:   data_out = minstret_reg;
+				CSR_TIME:       data_out = time_reg;
+				default:        data_out = '0;
+			endcase
+		end
+		return data_out;
+	endfunction
+
+endmodule
diff --git a/rv64cpu/rtl/core/privileged/pmp_checker.sv b/rv64cpu/rtl/core/privileged/pmp_checker.sv
new file mode 100644
index 0000000..361e507
--- /dev/null
+++ b/rv64cpu/rtl/core/privileged/pmp_checker.sv
@@ -0,0 +1,181 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/pmp_checker.sv
+// Description: RISC-V Physical Memory Protection access checker.
+
+module pmp_checker
+    import rv64_pkg::*;
+    import csr_pkg::*;
+#(
+    parameter int PHYS_ADDR_WIDTH = 56
+)(
+    input  logic              req_valid,
+    input  logic [XLEN-1:0]   req_paddr,
+    input  logic [3:0]        req_size,
+    input  logic              req_is_fetch,
+    input  logic              req_is_store,
+    input  logic [1:0]        priv_mode,
+    input  pmpcfg_entry_t     pmpcfg_entries [PMP_ENTRY_COUNT],
+    input  pmpaddr_entry_t    pmpaddr_entries [PMP_ENTRY_COUNT],
+    output logic              access_fault
+);
+
+    localparam logic [1:0] PRIV_MODE_M = 2'b11;
+
+    typedef struct packed {
+        logic valid;
+        logic [XLEN-1:0] lower_bound;
+        logic [XLEN-1:0] upper_bound;
+    } pmp_region_t;
+
+    function automatic logic [XLEN-1:0] pmpaddr_to_paddr(input pmpaddr_entry_t pmpaddr);
+        logic [XLEN-1:0] paddr;
+        paddr = pmpaddr << 2;
+        if (PHYS_ADDR_WIDTH < XLEN) begin
+            paddr[XLEN-1:PHYS_ADDR_WIDTH] = '0;
+        end
+        return paddr;
+    endfunction
+
+    function automatic int unsigned napot_trailing_ones(input pmpaddr_entry_t pmpaddr);
+        int unsigned ones;
+        ones = 0;
+        for (int bit_idx = 0; bit_idx < (PHYS_ADDR_WIDTH - 2); bit_idx++) begin
+            if (pmpaddr[bit_idx]) begin
+                ones++;
+            end else begin
+                break;
+            end
+        end
+        return ones;
+    endfunction
+
+    function automatic pmp_region_t pmp_region(input int unsigned entry_idx);
+        pmp_region_t region;
+        logic [1:0] address_matching;
+        int unsigned ones;
+        logic [XLEN-1:0] napot_mask;
+        logic [XLEN-1:0] napot_base;
+
+        region.valid = 1'b0;
+        region.lower_bound = '0;
+        region.upper_bound = '0;
+        address_matching = pmpcfg_entries[entry_idx][PMPCFG_A_HI:PMPCFG_A_LO];
+
+        unique case (address_matching)
+            PMP_MODE_OFF: begin
+                region.valid = 1'b0;
+            end
+            PMP_MODE_TOR: begin
+                region.valid = 1'b1;
+                region.lower_bound = (entry_idx == 0) ? '0 : pmpaddr_to_paddr(pmpaddr_entries[entry_idx - 1]);
+                region.upper_bound = pmpaddr_to_paddr(pmpaddr_entries[entry_idx]);
+                if (region.upper_bound <= region.lower_bound) begin
+                    region.valid = 1'b0;
+                end
+            end
+            PMP_MODE_NA4: begin
+                region.valid = 1'b1;
+                region.lower_bound = pmpaddr_to_paddr(pmpaddr_entries[entry_idx]);
+                region.upper_bound = region.lower_bound + 64'd4;
+            end
+            PMP_MODE_NAPOT: begin
+                region.valid = 1'b1;
+                ones = napot_trailing_ones(pmpaddr_entries[entry_idx]);
+                napot_mask = (64'd1 << (ones + 1)) - 64'd1;
+                napot_base = (pmpaddr_entries[entry_idx] & ~napot_mask) << 2;
+                if (PHYS_ADDR_WIDTH < XLEN) begin
+                    napot_base[XLEN-1:PHYS_ADDR_WIDTH] = '0;
+                end
+                region.lower_bound = napot_base;
+                region.upper_bound = napot_base + (64'd1 << (ones + 3));
+            end
+            default: begin
+                region.valid = 1'b0;
+            end
+        endcase
+
+        return region;
+    endfunction
+
+    function automatic logic ranges_overlap(
+        input logic [XLEN-1:0] access_lower,
+        input logic [XLEN-1:0] access_upper,
+        input logic [XLEN-1:0] region_lower,
+        input logic [XLEN-1:0] region_upper
+    );
+        return (access_lower < region_upper) && (access_upper > region_lower);
+    endfunction
+
+    function automatic logic range_contains(
+        input logic [XLEN-1:0] access_lower,
+        input logic [XLEN-1:0] access_upper,
+        input logic [XLEN-1:0] region_lower,
+        input logic [XLEN-1:0] region_upper
+    );
+        return (access_lower >= region_lower) && (access_upper <= region_upper);
+    endfunction
+
+    function automatic logic permissions_allow(
+        input pmpcfg_entry_t cfg,
+        input logic is_fetch,
+        input logic is_store
+    );
+        if (is_fetch) begin
+            return cfg[PMPCFG_X_BIT];
+        end
+        if (is_store) begin
+            return cfg[PMPCFG_W_BIT];
+        end
+        return cfg[PMPCFG_R_BIT];
+    endfunction
+
+    always_comb begin
+        logic [XLEN-1:0] access_lower;
+        logic [XLEN-1:0] access_upper;
+        logic [3:0] normalized_size;
+        logic matched;
+        pmp_region_t region;
+
+        normalized_size = (req_size == 4'd0) ? 4'd1 : req_size;
+        access_lower = req_paddr;
+        access_upper = req_paddr + {{(XLEN-4){1'b0}}, normalized_size};
+        matched = 1'b0;
+        region = '0;
+        access_fault = 1'b0;
+
+        if (req_valid) begin
+            for (int unsigned entry_idx = 0; entry_idx < PMP_ENTRY_COUNT; entry_idx++) begin
+                region = pmp_region(entry_idx);
+                if (!matched && region.valid &&
+                    ranges_overlap(access_lower, access_upper, region.lower_bound, region.upper_bound)) begin
+                    matched = 1'b1;
+                    if (!range_contains(access_lower, access_upper, region.lower_bound, region.upper_bound)) begin
+                        access_fault = 1'b1;
+                    end else if ((priv_mode != PRIV_MODE_M) || pmpcfg_entries[entry_idx][PMPCFG_L_BIT]) begin
+                        access_fault = !permissions_allow(pmpcfg_entries[entry_idx], req_is_fetch, req_is_store);
+                    end
+                end
+            end
+
+            if (!matched && (priv_mode != PRIV_MODE_M)) begin
+                access_fault = 1'b1;
+            end
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/privileged/trap_controller.sv b/rv64cpu/rtl/core/privileged/trap_controller.sv
new file mode 100644
index 0000000..246b392
--- /dev/null
+++ b/rv64cpu/rtl/core/privileged/trap_controller.sv
@@ -0,0 +1,279 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/trap_ctrl.sv
+// Description: Privileged-level Trap Controller (Exception and Interrupt Handling)
+//              Detects exceptions/interrupts, triggers context saving, and generates global pipeline flush and PC jump signals
+
+module trap_controller
+    import rv64_pkg::*;
+    import csr_pkg::*;
+(
+    // Submission signals from the WB stage (guarantee of precise exceptions)
+    input  logic [XLEN-1:0]        wb_pc,
+    input  logic                  trap_illegal_instr,
+    input  logic                  trap_is_ecall,
+    input  logic                  trap_is_ebreak,
+    input  logic                  trap_is_mret,
+    input  logic                  trap_is_sret,
+    input  logic                  trap_instr_access_fault,
+    input  logic                  trap_load_fault,
+    input  logic                  trap_store_fault,
+    input  logic                  trap_instr_page_fault,
+    input  logic                  trap_load_page_fault,
+    input  logic                  trap_store_page_fault,
+    input  logic [XLEN-1:0]       trap_bad_addr,
+    input  logic [INST_WIDTH-1:0] trap_bad_instr,
+
+    // Status read from CSR Unit
+    input  logic                  csr_mstatus_mie,    // Global interrupt enable (MIE)
+    input  logic [XLEN-1:0]       csr_mie,            // Machine interrupt enable CSR
+    input  logic [XLEN-1:0]       csr_sie,            // Supervisor interrupt enable CSR view
+    input  logic [XLEN-1:0]       csr_mip,            // Machine interrupt pending CSR
+    input  logic [XLEN-1:0]       csr_sip,            // Supervisor interrupt pending CSR view
+    input  logic [XLEN-1:0]       csr_mstatus,        // Machine status CSR
+    input  logic [XLEN-1:0]       csr_mtvec,          // Machine trap vector
+    input  logic [XLEN-1:0]       csr_stvec,          // Supervisor trap vector
+    input  logic [XLEN-1:0]       csr_mepc,           // Machine exception PC (for MRET)
+    input  logic [XLEN-1:0]       csr_sepc,           // Supervisor exception PC (for SRET)
+    input  logic [XLEN-1:0]       csr_medeleg,        // Exception delegation mask
+    input  logic [XLEN-1:0]       csr_mideleg,        // Interrupt delegation mask
+    input  logic [1:0]            csr_priv_mode,      // Current privilege level
+
+    // Status update signal sent to the CSR Unit
+    output logic                  trap_trigger,       // Trigger exception/interrupt context save
+    output logic [XLEN-1:0]       trap_mepc,          // Saved PC (MEPC/SEPC selected by trap_to_s_mode)
+    output logic [XLEN-1:0]       trap_mcause,        // Cause code (bit63 indicates interrupt)
+    output logic [XLEN-1:0]       trap_mtval,         // Additional information saved to mtval/stval
+    output logic                  trap_to_s_mode,     // 1: delegated to Supervisor mode
+    output logic                  mret_en,            // Trigger MRET context restore
+    output logic                  sret_en,            // Trigger SRET context restore
+
+    // Control flow interference sent to the Hazard Unit and IF stage
+    output logic                  trap_flush_req,     // Global pipeline flush request
+    output logic                  trap_pc_sel,        // Forcibly take over PC MUX (0: normal, 1: Trap/Mret/Sret)
+    output logic [XLEN-1:0]       trap_next_pc        // Target address for forced jump
+);
+
+    typedef enum logic [1:0] {
+        PRIV_MODE_U = 2'b00,
+        PRIV_MODE_S = 2'b01,
+        PRIV_MODE_M = 2'b11
+    } priv_mode_t;
+
+    localparam logic [XLEN-1:0] INTERRUPT_BIT = {1'b1, {(XLEN-1){1'b0}}};
+    localparam int INTERRUPT_M_SOFTWARE_BIT = int'(INTERRUPT_M_SOFTWARE);
+    localparam int INTERRUPT_M_TIMER_BIT    = int'(INTERRUPT_M_TIMER);
+    localparam int INTERRUPT_M_EXTERNAL_BIT = int'(INTERRUPT_M_EXTERNAL);
+    localparam int INTERRUPT_S_SOFTWARE_BIT = int'(INTERRUPT_S_SOFTWARE);
+    localparam int INTERRUPT_S_TIMER_BIT    = int'(INTERRUPT_S_TIMER);
+    localparam int INTERRUPT_S_EXTERNAL_BIT = int'(INTERRUPT_S_EXTERNAL);
+    localparam int MSTATUS_SIE_BIT = 1;
+    localparam int MSTATUS_TSR_BIT = 22;
+
+    logic                 has_sync_exception;
+    logic [XLEN-1:0]      sync_cause;
+    logic [XLEN-1:0]      sync_mtval;
+    logic [XLEN-1:0]      illegal_instr_value;
+    logic                 sret_tsr_illegal;
+
+    assign illegal_instr_value = {{(XLEN-INST_WIDTH){1'b0}}, trap_bad_instr};
+    assign sret_tsr_illegal    = trap_is_sret &&
+                                  (priv_mode_t'(csr_priv_mode) == PRIV_MODE_S) &&
+                                  csr_mstatus[MSTATUS_TSR_BIT];
+
+    always_comb begin
+        has_sync_exception = 1'b0;
+        sync_cause         = '0;
+        sync_mtval         = '0;
+
+        if (trap_instr_access_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_INST_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_instr_page_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_INSTR_PAGE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_illegal_instr || sret_tsr_illegal) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_INST_ILLEGAL;
+            sync_mtval         = illegal_instr_value;
+        end else if (trap_is_ebreak) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_BREAKPOINT;
+            sync_mtval         = wb_pc;
+        end else if (trap_is_ecall) begin
+            has_sync_exception = 1'b1;
+            case (priv_mode_t'(csr_priv_mode))
+                PRIV_MODE_U: sync_cause = EXC_ECALL_FROM_U;
+                PRIV_MODE_S: sync_cause = EXC_ECALL_FROM_S;
+                default:     sync_cause = EXC_ECALL_FROM_M;
+            endcase
+            sync_mtval = '0;
+        end else if (trap_load_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_LOAD_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_store_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_STORE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_load_page_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_LOAD_PAGE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end else if (trap_store_page_fault) begin
+            has_sync_exception = 1'b1;
+            sync_cause         = EXC_STORE_PAGE_FAULT;
+            sync_mtval         = trap_bad_addr;
+        end
+    end
+
+    // Interrupt Detection - Asynchronous, gated by mie and privilege global-enable rules.
+    logic               has_interrupt;
+    logic [XLEN-1:0]    interrupt_cause;
+    logic               machine_interrupts_globally_enabled;
+    logic               supervisor_interrupts_globally_enabled;
+    logic               interrupt_targets_s_mode;
+
+    assign machine_interrupts_globally_enabled = (priv_mode_t'(csr_priv_mode) != PRIV_MODE_M) ||
+                                                 csr_mstatus_mie;
+    assign supervisor_interrupts_globally_enabled = (priv_mode_t'(csr_priv_mode) == PRIV_MODE_U) ||
+                                                    ((priv_mode_t'(csr_priv_mode) == PRIV_MODE_S) &&
+                                                     csr_mstatus[MSTATUS_SIE_BIT]);
+
+    function automatic logic interrupt_pending_enabled(
+        input logic [XLEN-1:0] pending,
+        input logic [XLEN-1:0] enable,
+        input int              cause
+    );
+        interrupt_pending_enabled = pending[cause[5:0]] && enable[cause[5:0]];
+    endfunction
+
+    task automatic take_interrupt(
+        input  logic [XLEN-1:0] cause,
+        input  logic            to_s_mode,
+        output logic            taken,
+        output logic [XLEN-1:0] taken_cause,
+        output logic            taken_targets_s_mode
+    );
+        taken                = 1'b1;
+        taken_cause          = INTERRUPT_BIT | cause;
+        taken_targets_s_mode = to_s_mode;
+    endtask
+
+    function automatic logic s_interrupt_cause_delegated(input int cause);
+        s_interrupt_cause_delegated = csr_mideleg[cause[5:0]];
+    endfunction
+
+    function automatic logic s_interrupt_cause_takeable(input int cause);
+        if (s_interrupt_cause_delegated(cause)) begin
+            s_interrupt_cause_takeable = supervisor_interrupts_globally_enabled &&
+                                         interrupt_pending_enabled(csr_sip, csr_sie, cause);
+        end else begin
+            s_interrupt_cause_takeable = machine_interrupts_globally_enabled &&
+                                         interrupt_pending_enabled(csr_mip, csr_mie, cause);
+        end
+    endfunction
+
+    always_comb begin
+        has_interrupt             = 1'b0;
+        interrupt_cause           = '0;
+        interrupt_targets_s_mode  = 1'b0;
+
+        if (machine_interrupts_globally_enabled &&
+            interrupt_pending_enabled(csr_mip, csr_mie, INTERRUPT_M_EXTERNAL_BIT)) begin
+            take_interrupt(INTERRUPT_M_EXTERNAL, 1'b0, has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (machine_interrupts_globally_enabled &&
+                     interrupt_pending_enabled(csr_mip, csr_mie, INTERRUPT_M_SOFTWARE_BIT)) begin
+            take_interrupt(INTERRUPT_M_SOFTWARE, 1'b0, has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (machine_interrupts_globally_enabled &&
+                     interrupt_pending_enabled(csr_mip, csr_mie, INTERRUPT_M_TIMER_BIT)) begin
+            take_interrupt(INTERRUPT_M_TIMER, 1'b0, has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (s_interrupt_cause_takeable(INTERRUPT_S_EXTERNAL_BIT)) begin
+            take_interrupt(INTERRUPT_S_EXTERNAL, s_interrupt_cause_delegated(INTERRUPT_S_EXTERNAL_BIT),
+                           has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (s_interrupt_cause_takeable(INTERRUPT_S_SOFTWARE_BIT)) begin
+            take_interrupt(INTERRUPT_S_SOFTWARE, s_interrupt_cause_delegated(INTERRUPT_S_SOFTWARE_BIT),
+                           has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end else if (s_interrupt_cause_takeable(INTERRUPT_S_TIMER_BIT)) begin
+            take_interrupt(INTERRUPT_S_TIMER, s_interrupt_cause_delegated(INTERRUPT_S_TIMER_BIT),
+                           has_interrupt, interrupt_cause, interrupt_targets_s_mode);
+        end
+    end 
+
+    logic [XLEN-1:0] trap_cause_value;
+    logic [XLEN-1:0] trap_mtval_value;
+    logic            trap_is_interrupt;
+
+    assign trap_is_interrupt = has_interrupt;
+    assign trap_cause_value  = has_interrupt ? interrupt_cause : sync_cause;
+    assign trap_mtval_value  = has_interrupt ? '0 : sync_mtval;
+
+    assign trap_trigger = has_interrupt | has_sync_exception;
+
+    logic [5:0]      trap_cause_index;
+    logic            delegate_to_s;
+    logic [XLEN-1:0] deleg_mask;
+    logic            machine_interrupt_cause;
+
+    assign trap_cause_index = trap_cause_value[5:0];
+    assign deleg_mask       = trap_is_interrupt ? csr_mideleg : csr_medeleg;
+    assign machine_interrupt_cause = trap_is_interrupt &&
+                                     ((trap_cause_index == INTERRUPT_M_SOFTWARE_BIT[5:0]) ||
+                                      (trap_cause_index == INTERRUPT_M_TIMER_BIT[5:0]) ||
+                                      (trap_cause_index == INTERRUPT_M_EXTERNAL_BIT[5:0]));
+    assign delegate_to_s    = trap_trigger && (priv_mode_t'(csr_priv_mode) != PRIV_MODE_M) &&
+                              ((trap_is_interrupt && interrupt_targets_s_mode) ||
+                               (!trap_is_interrupt && deleg_mask[trap_cause_index] && !machine_interrupt_cause));
+    assign trap_to_s_mode   = delegate_to_s;
+
+    assign trap_mcause = trap_trigger ? trap_cause_value : '0;
+    assign trap_mtval  = trap_trigger ? trap_mtval_value : '0;
+    assign trap_mepc   = trap_trigger ? wb_pc : '0;
+
+    assign mret_en = trap_is_mret & ~trap_trigger;
+    assign sret_en = trap_is_sret & ~trap_trigger;
+
+    assign trap_flush_req = trap_trigger | mret_en | sret_en;
+    assign trap_pc_sel    = trap_flush_req;
+
+    logic [XLEN-1:0] trap_vector_base;
+    logic [1:0]      trap_vector_mode;
+    logic [XLEN-1:0] trap_vector_aligned;
+
+    assign trap_vector_base    = delegate_to_s ? csr_stvec : csr_mtvec;
+    assign trap_vector_mode    = trap_vector_base[1:0];
+    assign trap_vector_aligned = {trap_vector_base[XLEN-1:2], 2'b00};
+
+    always_comb begin
+        if (trap_trigger) begin
+            if ((trap_vector_mode == 2'b01) && trap_is_interrupt) begin
+                trap_next_pc = trap_vector_aligned + (({{(XLEN-6){1'b0}}, trap_cause_index}) << 2);
+            end else begin
+                trap_next_pc = trap_vector_aligned;
+            end
+        end else if (mret_en) begin
+            trap_next_pc = csr_mepc;
+        end else if (sret_en) begin
+            trap_next_pc = csr_sepc;
+        end else begin
+            trap_next_pc = '0;
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/registers/fpr_file.sv b/rv64cpu/rtl/core/registers/fpr_file.sv
new file mode 100644
index 0000000..7522b5f
--- /dev/null
+++ b/rv64cpu/rtl/core/registers/fpr_file.sv
@@ -0,0 +1,41 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+// File path: rtl/core/fp_regfile.sv
+// Description: 32-entry 64-bit floating-point register file for RV64F/RV64D.
+
+module fpr_file
+    import rv64_pkg::*;
+(
+    input  logic            clk,
+    input  logic            rst,
+
+    input  logic [4:0]      fs1_addr,
+    input  logic [4:0]      fs2_addr,
+    input  logic [4:0]      fs3_addr,
+    output logic [XLEN-1:0] fs1_rdata,
+    output logic [XLEN-1:0] fs2_rdata,
+    output logic [XLEN-1:0] fs3_rdata,
+
+    input  logic            fp_write_en,
+    input  logic [4:0]      fd_addr,
+    input  logic [XLEN-1:0] fd_wdata
+);
+
+    logic [XLEN-1:0] regs [0:31];
+
+    assign fs1_rdata = (fp_write_en && (fs1_addr == fd_addr)) ? fd_wdata : regs[fs1_addr];
+    assign fs2_rdata = (fp_write_en && (fs2_addr == fd_addr)) ? fd_wdata : regs[fs2_addr];
+    assign fs3_rdata = (fp_write_en && (fs3_addr == fd_addr)) ? fd_wdata : regs[fs3_addr];
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            for (int unsigned reg_idx = 0; reg_idx < 32; reg_idx++) begin
+                regs[reg_idx] <= '0;
+            end
+        end else if (fp_write_en) begin
+            regs[fd_addr] <= fd_wdata;
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/registers/gpr_file.sv b/rv64cpu/rtl/core/registers/gpr_file.sv
new file mode 100644
index 0000000..4ddf200
--- /dev/null
+++ b/rv64cpu/rtl/core/registers/gpr_file.sv
@@ -0,0 +1,67 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/regfile.sv
+// Description: 64-bit Register File
+
+module gpr_file
+    import rv64_pkg::*;
+(
+    input  logic                      clk,  // Clock signal
+    input  logic                      rst,  // Reset signal active high
+
+    // Read port 1
+    input  logic [4:0]                rs1_addr, // Read address 1
+    output logic [XLEN-1:0]           rs1_rdata,  // Read data 1
+
+    // Read port 2
+    input  logic [4:0]                rs2_addr, // Read address 2
+    output logic [XLEN-1:0]           rs2_rdata,  // Read data 2
+
+    // Write port
+    input  logic [4:0]                rd_addr,  // Write address
+    input  logic [XLEN-1:0]           rd_wdata,   // Write data
+    input  logic                      reg_write_en // Write enable signal
+);
+
+    // Register file storage
+    logic [XLEN-1:0] regs [0:31];
+
+    // Read Logic (combinational logic)
+    // Internal bypass: If reading the same register as currently being written, 
+    // forward the write data immediately.
+    assign rs1_rdata = (rs1_addr == 5'b0) ? '0 : 
+                       ((rs1_addr == rd_addr) && reg_write_en) ? rd_wdata : 
+                       regs[rs1_addr];
+                       
+    assign rs2_rdata = (rs2_addr == 5'b0) ? '0 : 
+                       ((rs2_addr == rd_addr) && reg_write_en) ? rd_wdata : 
+                       regs[rs2_addr];
+
+    // Write Logic (sequential logic)
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            for (int i = 0; i < 32; i++) begin
+                regs[i] <= '0;
+            end
+        end else begin
+            if (reg_write_en && rd_addr != 5'b0) begin
+                regs[rd_addr] <= rd_wdata;
+            end
+        end
+    end
+
+endmodule
diff --git a/rv64cpu/rtl/core/rv64_core.sv b/rv64cpu/rtl/core/rv64_core.sv
new file mode 100644
index 0000000..630fc3d
--- /dev/null
+++ b/rv64cpu/rtl/core/rv64_core.sv
@@ -0,0 +1,1217 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/richard_core.sv
+// Description: Richard CPU core top-level wrapper (Core Top)
+// Assembles a 5-stage pipeline, 4 inter-stage registers, and Hazard Unit
+
+module rv64_core
+    import rv64_pkg::*;
+    import csr_pkg::*;
+(
+    input  logic        clk,
+    input  logic        rst,
+
+    // Interface 1: Instruction Memory (I-Mem)
+    output logic [XLEN-1:0] imem_addr,
+    input  logic [31:0]     imem_rdata,
+
+    // Interface 2: Data Memory (D-Mem)
+    output logic        dmem_read_en,
+    output logic        dmem_write_en,
+    output logic [63:0] dmem_addr,
+    output logic [63:0] dmem_wdata,
+    output logic [2:0]  dmem_size,
+    input  logic [63:0] dmem_rdata,
+
+    // Interface 3: External Interrupt Sources
+    input  logic        m_timer_interrupt,
+    input  logic        m_software_interrupt,
+    input  logic        m_external_interrupt,
+    input  logic        s_timer_interrupt,
+    input  logic        s_software_interrupt,
+    input  logic        s_external_interrupt
+);
+    localparam int CSR_MSTATUS_MIE_BIT = 3;
+    localparam int CSR_MSTATUS_SUM_BIT = 18;
+    localparam int CSR_MSTATUS_MXR_BIT = 19;
+    localparam int CSR_MSTATUS_TVM_BIT = 20;
+    localparam int CSR_MSTATUS_TW_BIT  = 21;
+    localparam int CSR_MSTATUS_TSR_BIT = 22;
+
+    // -----------------------
+    // Internal Wire Declarations
+    // -----------------------
+
+    // (1) Hazard Unit Controls
+    logic [1:0]        hu_forward_a_sel, hu_forward_b_sel;
+    logic              hu_if_stall_en, hu_if_id_stall_en, hu_id_ex_stall_en, hu_ex_mem_stall_en;
+    logic              hu_if_id_flush_en, hu_id_ex_flush_en;
+    logic              hu_pc_redirect_sel, hu_pc_redirect_is_trap;
+    logic [XLEN-1:0]   hu_pc_redirect_target;
+
+    // (2) IF Stage Outputs
+    logic [XLEN-1:0]   if_pc_out;
+    logic [INST_WIDTH-1:0] if_instr_to_decode;
+    logic [INST_WIDTH-1:0] if_compressed_expanded_instr;
+    logic              if_instr_compressed;
+    logic [2:0]        if_instr_len;
+    logic              if_compressed_illegal;
+    // imem interface (ports)
+
+    // (3) IF/ID Pipeline Register Outputs
+    logic [XLEN-1:0]   if_id_pc;
+    logic [31:0]       if_id_instr;
+    logic [2:0]        if_id_instr_len;
+
+    // (4) ID Stage Outputs
+    logic [XLEN-1:0]   id_pc_out;
+    logic [2:0]        id_instr_len;
+    logic [XLEN-1:0]   id_rs1_rdata, id_rs2_rdata;
+    logic [XLEN-1:0]   id_fs1_rdata, id_fs2_rdata, id_fs3_rdata;
+    logic [4:0]        id_rs1_addr, id_rs2_addr;
+    logic [4:0]        id_fs3_addr;
+    logic [XLEN-1:0]   id_imm;
+    alu_op_t           id_alu_op;
+    logic              id_alu_src1_sel, id_alu_src2_sel;
+    logic              id_is_word_op, id_is_branch, id_is_jump;
+    branch_op_t        id_branch_op;
+    logic              id_is_load, id_is_store;
+    logic [2:0]        id_mem_size;
+    logic              id_is_amo;
+    amo_op_t           id_amo_op;
+    logic              id_is_fp, id_fp_reg_write_en, id_fp_store_data_sel;
+    fp_op_t            id_fp_op;
+    fp_fmt_t           id_fp_fmt;
+    logic [2:0]        id_fp_rm;
+    logic              id_reg_write_en;
+    logic [4:0]        id_rd_addr;
+    logic [1:0]        id_wb_sel;
+    logic              id_is_csr;
+    logic [2:0]        id_csr_op;
+    logic              id_illegal_instr, id_is_ecall, id_is_ebreak, id_is_mret, id_is_sret;
+    logic              id_is_wfi, id_is_sfence_vma, id_is_fence, id_is_fence_i;
+
+    // (5) ID/EX Pipeline Register Outputs
+    logic [XLEN-1:0]   ex_pc_in;
+    logic [INST_WIDTH-1:0] ex_instr_in;
+    logic [2:0]        ex_instr_len_in;
+    logic [XLEN-1:0]   ex_rs1_rdata_in, ex_rs2_rdata_in;
+    logic [XLEN-1:0]   ex_fs1_rdata_in, ex_fs2_rdata_in, ex_fs3_rdata_in;
+    logic [4:0]        ex_rs1_addr_in, ex_rs2_addr_in;
+    logic [4:0]        ex_fs3_addr_in;
+    logic [XLEN-1:0]   ex_imm_in;
+    alu_op_t           ex_alu_op_in;
+    logic              ex_alu_src1_sel_in, ex_alu_src2_sel_in;
+    logic              ex_is_word_op_in, ex_is_branch_in, ex_is_jump_in;
+    branch_op_t        ex_branch_op_in;
+    logic              ex_is_load_in, ex_is_store_in;
+    logic [2:0]        ex_mem_size_in;
+    logic              ex_is_amo_in;
+    amo_op_t           ex_amo_op_in;
+    logic              ex_is_fp_in, ex_fp_reg_write_en_in, ex_fp_store_data_sel_in;
+    fp_op_t            ex_fp_op_in;
+    fp_fmt_t           ex_fp_fmt_in;
+    logic [2:0]        ex_fp_rm_in;
+    logic              ex_reg_write_en_in;
+    logic [4:0]        ex_rd_addr_in;
+    logic [1:0]        ex_wb_sel_in;
+    logic              ex_is_csr_in;
+    logic [2:0]        ex_csr_op_in;
+    logic              ex_illegal_instr_in, ex_is_ecall_in, ex_is_ebreak_in, ex_is_mret_in, ex_is_sret_in;
+    logic              ex_is_wfi_in, ex_is_sfence_vma_in, ex_is_fence_in, ex_is_fence_i_in;
+
+    // (6) EX Stage Outputs (and Forwarding Muxes)
+    logic [XLEN-1:0]   ex_rs1_rdata_fwd, ex_rs2_rdata_fwd;
+    logic [XLEN-1:0]   ex_fs1_rdata_fwd, ex_fs2_rdata_fwd, ex_fs3_rdata_fwd;
+    logic [XLEN-1:0]   mem_forward_data;
+    logic [XLEN-1:0]   ex_alu_result_out;
+    logic [XLEN-1:0]   ex_result_to_mem;
+    logic [XLEN-1:0]   ex_fp_result_out;
+    logic [XLEN-1:0]   ex_fp_int_result_out;
+    logic [4:0]        ex_fp_fflags_out;
+    logic              ex_fp_illegal_out;
+    logic              ex_branch_taken_out;
+    logic [XLEN-1:0]   ex_branch_target_out;
+    logic              ex_redirect_taken;
+    logic [XLEN-1:0]   ex_redirect_target;
+
+    // (7) EX/MEM Pipeline Register Outputs
+    logic [XLEN-1:0]   mem_pc_in;
+    logic [INST_WIDTH-1:0] mem_instr_in;
+    logic [2:0]        mem_instr_len_in;
+    logic [XLEN-1:0]   mem_alu_result_in;
+    logic              mem_branch_taken_in;
+    logic [XLEN-1:0]   mem_branch_target_in;
+    logic [XLEN-1:0]   mem_rs1_rdata_in, mem_rs2_rdata_in;
+    logic [XLEN-1:0]   mem_fs1_rdata_in, mem_fs2_rdata_in, mem_fs3_rdata_in;
+    logic [4:0]        mem_rs1_addr_in;
+    logic [4:0]        mem_fs3_addr_in;
+    logic [XLEN-1:0]   mem_imm_in;
+    logic              mem_is_load_in, mem_is_store_in;
+    logic [2:0]        mem_mem_size_in;
+    logic              mem_is_amo_in;
+    amo_op_t           mem_amo_op_in;
+    logic              mem_is_fp_in, mem_fp_reg_write_en_in, mem_fp_store_data_sel_in;
+    fp_op_t            mem_fp_op_in;
+    fp_fmt_t           mem_fp_fmt_in;
+    logic [2:0]        mem_fp_rm_in;
+    logic [XLEN-1:0]   mem_fp_result_in;
+    logic [4:0]        mem_fp_fflags_in;
+    logic              mem_reg_write_en_in;
+    logic [4:0]        mem_rd_addr_in;
+    logic [1:0]        mem_wb_sel_in;
+    logic              mem_is_csr_in;
+    logic [2:0]        mem_csr_op_in;
+    logic              mem_illegal_instr_in, mem_is_ecall_in, mem_is_ebreak_in, mem_is_mret_in, mem_is_sret_in;
+    logic              mem_is_wfi_in, mem_is_sfence_vma_in, mem_is_fence_in, mem_is_fence_i_in;
+
+    // (8) MEM Stage Outputs (to MEM/WB Register)
+    logic [XLEN-1:0]   mw_reg_alu_result_in;
+    logic [XLEN-1:0]   mw_reg_mem_rdata_in;
+    logic [XLEN-1:0]   mw_reg_fp_result_in;
+    logic [4:0]        mw_reg_fp_fflags_in;
+    logic [XLEN-1:0]   mw_reg_pc_in;
+    logic [INST_WIDTH-1:0] mw_reg_instr_in;
+    logic [2:0]        mw_reg_instr_len_in;
+    logic [7:0]        mw_reg_dmem_wstrb_in;
+    logic              mw_reg_reg_write_en_in;
+    logic              mw_reg_fp_reg_write_en_in;
+    logic [4:0]        mw_reg_rd_addr_in;
+    logic [1:0]        mw_reg_wb_sel_in;
+    logic              mw_reg_is_csr_in;
+    logic [2:0]        mw_reg_csr_op_in;
+    logic [XLEN-1:0]   mw_reg_rs1_rdata_in;
+    logic [4:0]        mw_reg_rs1_addr_in;
+    logic [XLEN-1:0]   mw_reg_imm_in;
+    logic              mw_reg_illegal_instr_in, mw_reg_is_ecall_in, mw_reg_is_ebreak_in, mw_reg_is_mret_in, mw_reg_is_sret_in;
+    logic              mw_reg_load_fault_in, mw_reg_store_fault_in;
+    logic              mw_reg_load_page_fault_in, mw_reg_store_page_fault_in;
+    logic [XLEN-1:0]   mw_reg_fault_addr_in;
+    logic              mem_stall_req;
+
+    // (9) MEM/WB Pipeline Register Outputs
+    logic [XLEN-1:0]   wb_pc_in;
+    logic [XLEN-1:0]   wb_alu_result_in;
+    logic [XLEN-1:0]   wb_mem_rdata_in;
+    logic [XLEN-1:0]   wb_fp_result_in;
+    logic [4:0]        wb_fp_fflags_in;
+    logic [INST_WIDTH-1:0] wb_instr_in;
+    logic [2:0]        wb_instr_len_in;
+    logic              wb_reg_write_en_in;
+    logic              wb_fp_reg_write_en_in;
+    logic [4:0]        wb_rd_addr_in;
+    logic [1:0]        wb_wb_sel_in;
+    logic              wb_is_csr_in;
+    logic [2:0]        wb_csr_op_in;
+    logic [XLEN-1:0]   wb_rs1_rdata_in;
+    logic [4:0]        wb_rs1_addr_in;
+    logic [XLEN-1:0]   wb_imm_in;
+    logic              wb_illegal_instr_in, wb_is_ecall_in, wb_is_ebreak_in, wb_is_mret_in, wb_is_sret_in;
+    logic              wb_load_fault, wb_store_fault;
+    logic              wb_load_page_fault, wb_store_page_fault;
+    logic [XLEN-1:0]   wb_fault_addr_in;
+
+    // (10) WB Stage Outputs (to RegFile & CSR Unit)
+    logic [XLEN-1:0]   wb_rd_wdata;
+    logic [4:0]        wb_rd_addr;
+    logic              wb_reg_write_en_out;
+    logic [XLEN-1:0]   wb_fd_wdata;
+    logic [4:0]        wb_fd_addr;
+    logic              wb_fp_write_en;
+    logic [4:0]        wb_fp_fflags;
+    logic              wb_fp_state_dirty;
+    logic              wb_fp_fflags_valid;
+    logic              csr_req;
+    logic [2:0]        csr_op;
+    logic [11:0]       csr_addr;
+    logic [XLEN-1:0]   csr_rs1_data;
+    logic [XLEN-1:0]   csr_rdata; // Result from CSR unit
+    logic [1:0]        csr_priv_mode_cur;
+    logic [XLEN-1:0]   csr_mtvec_base;
+    logic [XLEN-1:0]   csr_stvec_base;
+    logic [XLEN-1:0]   csr_mstatus_value;
+    logic [XLEN-1:0]   csr_sstatus_value;
+    logic [XLEN-1:0]   csr_medeleg_value;
+    logic [XLEN-1:0]   csr_mideleg_value;
+    logic [XLEN-1:0]   csr_mie_value;
+    logic [XLEN-1:0]   csr_sie_value;
+    logic [XLEN-1:0]   csr_mip_value;
+    logic [XLEN-1:0]   csr_sip_value;
+    logic [XLEN-1:0]   csr_mepc_value;
+    logic [XLEN-1:0]   csr_sepc_value;
+    logic [XLEN-1:0]   csr_satp_value;
+    logic [2:0]        csr_frm_value;
+    logic [XLEN-1:0]   csr_trap_vector_value;
+    logic              csr_trap_to_s_mode;
+    logic              csr_illegal_access;
+    pmpcfg_entry_t     csr_pmpcfg_entries [PMP_ENTRY_COUNT];
+    pmpaddr_entry_t    csr_pmpaddr_entries [PMP_ENTRY_COUNT];
+
+    // (11) Exception / Trap bus
+    logic              trap_illegal_instr, trap_is_ecall, trap_is_ebreak, trap_is_mret, trap_is_sret;
+    logic [XLEN-1:0]   trap_epc;
+    logic              trap_instr_access_fault;
+
+    // (12) Trap Controller + interrupt wiring
+    logic              trap_load_fault, trap_store_fault;
+    logic              trap_instr_page_fault, trap_load_page_fault, trap_store_page_fault;
+    logic [XLEN-1:0]   trap_bad_addr;
+    logic [INST_WIDTH-1:0] trap_bad_instr;
+    logic              trap_flush_req;
+    logic              trap_pc_sel;
+    logic [XLEN-1:0]   trap_next_pc;
+    logic              trap_pc_sel_raw;
+    logic [XLEN-1:0]   trap_next_pc_raw;
+    logic              trap_ctrl_trigger;
+    logic [XLEN-1:0]   trap_ctrl_mepc;
+    logic [XLEN-1:0]   trap_ctrl_mcause;
+    logic [XLEN-1:0]   trap_ctrl_mtval;
+    logic              trap_ctrl_to_s_mode;
+    logic              trap_ctrl_mret_en;
+    logic              trap_ctrl_sret_en;
+    logic              csr_mstatus_mie;
+    logic              csr_mstatus_sum;
+    logic              csr_mstatus_mxr;
+    logic              if_instr_access_fault;
+    logic              if_instr_page_fault;
+    logic              if_id_instr_access_fault;
+    logic              if_id_instr_page_fault;
+    logic              ex_instr_access_fault_in;
+    logic              ex_instr_page_fault_in;
+    logic              mem_instr_access_fault_in;
+    logic              mem_instr_page_fault_in;
+    logic              mw_reg_instr_access_fault_in;
+    logic              mw_reg_instr_page_fault_in;
+    logic              wb_instr_access_fault;
+    logic              wb_instr_page_fault;
+    logic              dmem_access_fault;
+    logic              dmem_load_access_fault;
+    logic              dmem_store_access_fault;
+    logic              dmem_load_page_fault;
+    logic              dmem_store_page_fault;
+    logic [3:0]        dmem_access_size;
+
+    logic              mmu_i_req_valid;
+    logic              mmu_i_resp_valid;
+    logic              mmu_i_resp_page_fault;
+    logic [XLEN-1:0]   mmu_i_resp_vaddr;
+    logic [XLEN-1:0]   mmu_i_resp_paddr;
+    logic              mmu_d_req_valid;
+    logic              mmu_d_resp_valid;
+    logic              mmu_d_resp_page_fault;
+    logic [XLEN-1:0]   mmu_d_resp_paddr;
+    logic              ptw_mem_req_valid;
+    logic              ptw_mem_req_ready;
+    logic [XLEN-1:0]   ptw_mem_req_addr;
+    logic              ptw_mem_resp_valid;
+    logic [XLEN-1:0]   ptw_mem_resp_data;
+    logic              ptw_mem_resp_valid_q;
+    logic [XLEN-1:0]   ptw_mem_resp_data_q;
+    logic              raw_dmem_read_en;
+    logic              raw_dmem_write_en;
+    logic [XLEN-1:0]   raw_dmem_addr;
+    logic [XLEN-1:0]   raw_dmem_wdata;
+    logic [7:0]        raw_dmem_wstrb;
+    logic              data_access_needs_translation;
+    logic              data_translation_ready;
+    logic              data_translation_page_fault;
+    logic              data_memory_port_available;
+    logic              if_translation_ready;
+    logic              if_translation_page_fault;
+    logic              mmu_stall_req;
+    logic              dmem_req_is_store;
+
+    assign csr_mstatus_mie        = csr_mstatus_value[CSR_MSTATUS_MIE_BIT];
+    assign csr_mstatus_sum        = csr_mstatus_value[CSR_MSTATUS_SUM_BIT];
+    assign csr_mstatus_mxr        = csr_mstatus_value[CSR_MSTATUS_MXR_BIT];
+    assign ex_redirect_taken       = ex_branch_taken_out | ex_is_fence_i_in;
+    assign ex_redirect_target      = ex_is_fence_i_in ?
+                                     (ex_pc_in + {{(XLEN-3){1'b0}}, ex_instr_len_in}) :
+                                     ex_branch_target_out;
+
+    always_comb begin
+        unique case (mem_mem_size_in)
+            3'b000, 3'b100: dmem_access_size = 4'd1;
+            3'b001, 3'b101: dmem_access_size = 4'd2;
+            3'b010, 3'b110: dmem_access_size = 4'd4;
+            3'b011:         dmem_access_size = 4'd8;
+            default:        dmem_access_size = 4'd1;
+        endcase
+    end
+
+    assign mmu_i_req_valid = !rst;
+    assign dmem_req_is_store = mem_is_store_in | (mem_is_amo_in && (mem_amo_op_in != AMO_LR));
+
+    assign mmu_d_req_valid = (mem_is_load_in | mem_is_store_in | mem_is_amo_in) &
+                              ~mem_instr_access_fault_in & ~mem_instr_page_fault_in;
+
+    assign if_translation_ready      = mmu_i_resp_valid & (mmu_i_resp_vaddr == if_pc_out);
+    assign if_translation_page_fault = if_translation_ready & mmu_i_resp_page_fault;
+    assign if_instr_page_fault       = if_translation_page_fault;
+
+    assign data_access_needs_translation = mmu_d_req_valid;
+    assign data_translation_ready        = !data_access_needs_translation | mmu_d_resp_valid;
+    assign data_translation_page_fault   = data_access_needs_translation & mmu_d_resp_valid &
+                                           mmu_d_resp_page_fault;
+    assign data_memory_port_available    = !ptw_mem_req_valid;
+    assign dmem_load_page_fault          = (mem_is_load_in | (mem_is_amo_in && (mem_amo_op_in == AMO_LR))) &
+                                           data_translation_page_fault;
+    assign dmem_store_page_fault         = dmem_req_is_store & data_translation_page_fault;
+    assign mmu_stall_req                 = !if_translation_ready;
+
+    assign ptw_mem_req_ready  = 1'b1;
+    assign ptw_mem_resp_valid = ptw_mem_resp_valid_q;
+    assign ptw_mem_resp_data  = ptw_mem_resp_data_q;
+
+    always_ff @(posedge clk) begin
+        if (rst) begin
+            ptw_mem_resp_valid_q <= 1'b0;
+            ptw_mem_resp_data_q  <= '0;
+        end else begin
+            ptw_mem_resp_valid_q <= ptw_mem_req_valid & ptw_mem_req_ready;
+            if (ptw_mem_req_valid & ptw_mem_req_ready) begin
+                ptw_mem_resp_data_q <= dmem_rdata;
+            end
+        end
+    end
+
+    mmu u_mmu_top (
+        .clk                  (clk),
+        .rst                  (rst),
+        .satp_value           (csr_satp_value),
+        .priv_mode            (csr_priv_mode_cur),
+        .sum                  (csr_mstatus_sum),
+        .mxr                  (csr_mstatus_mxr),
+        .i_req_valid          (mmu_i_req_valid),
+        .i_req_ready          (),
+        .i_req_vaddr          (if_pc_out),
+        .i_resp_valid         (mmu_i_resp_valid),
+        .i_resp_hit           (),
+        .i_resp_page_fault    (mmu_i_resp_page_fault),
+        .i_resp_vaddr         (mmu_i_resp_vaddr),
+        .i_resp_paddr         (mmu_i_resp_paddr),
+        .d_req_valid          (mmu_d_req_valid),
+        .d_req_ready          (),
+        .d_req_vaddr          (mem_alu_result_in),
+        .d_req_is_store       (dmem_req_is_store),
+        .d_resp_valid         (mmu_d_resp_valid),
+        .d_resp_hit           (),
+        .d_resp_page_fault    (mmu_d_resp_page_fault),
+        .d_resp_paddr         (mmu_d_resp_paddr),
+        .flush_all            (mem_is_sfence_vma_in),
+        .flush_asid_valid     (1'b0),
+        .flush_asid           ('0),
+        .flush_vpn_valid      (1'b0),
+        .flush_vpn            ('0),
+        .ptw_mem_req_valid    (ptw_mem_req_valid),
+        .ptw_mem_req_ready    (ptw_mem_req_ready),
+        .ptw_mem_req_addr     (ptw_mem_req_addr),
+        .ptw_mem_resp_valid   (ptw_mem_resp_valid),
+        .ptw_mem_resp_data    (ptw_mem_resp_data)
+    );
+
+    // ---------------------------------------------------------
+    // (A) Hazard Unit Implementation
+    // ---------------------------------------------------------
+    hazard_unit u_hazard_unit (
+        .id_rs1_addr        (id_rs1_addr),
+        .id_rs2_addr        (id_rs2_addr),
+        .ex_rs1_addr        (ex_rs1_addr_in),
+        .ex_rs2_addr        (ex_rs2_addr_in),
+        .ex_rd_addr         (ex_rd_addr_in),
+        .ex_reg_write_en    (ex_reg_write_en_in),
+        .ex_is_load         (ex_is_load_in | ex_is_amo_in),
+        .ex_is_csr          (ex_is_csr_in),
+        .ex_branch_taken    (ex_redirect_taken),
+        .ex_branch_target   (ex_redirect_target),
+        .mem_rd_addr        (mem_rd_addr_in),
+        .mem_reg_write_en   (mem_reg_write_en_in),
+        .mem_result_late    (mem_wb_sel_in == 2'b01),
+        .mem_is_csr         (mem_is_csr_in),
+        .mem_stall_req      (mem_stall_req | mmu_stall_req),
+        .trap_flush_req     (trap_flush_req),
+        .trap_pc_sel        (trap_pc_sel),
+        .trap_target_pc     (trap_next_pc),
+        .wb_rd_addr         (wb_rd_addr_in),
+        .wb_reg_write_en    (wb_reg_write_en_out),
+        .forward_a_sel      (hu_forward_a_sel),
+        .forward_b_sel      (hu_forward_b_sel),
+        .if_stall_en        (hu_if_stall_en),
+        .if_id_stall_en     (hu_if_id_stall_en),
+        .id_ex_stall_en     (hu_id_ex_stall_en),
+        .ex_mem_stall_en    (hu_ex_mem_stall_en),
+        .if_id_flush_en     (hu_if_id_flush_en),
+        .id_ex_flush_en     (hu_id_ex_flush_en),
+        .pc_redirect_sel    (hu_pc_redirect_sel),
+        .pc_redirect_target (hu_pc_redirect_target),
+        .pc_redirect_is_trap(hu_pc_redirect_is_trap)
+    );
+
+    // ---------------------------------------------------------
+    // (B) IF Stage - Instruction Fetch
+    // ---------------------------------------------------------
+    if_stage u_if_stage (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_if_stall_en),
+        .pc_redirect_sel    (hu_pc_redirect_sel),
+        .pc_redirect_target (hu_pc_redirect_target),
+        .pc_redirect_is_trap(hu_pc_redirect_is_trap),
+        .if_instr_compressed(if_instr_compressed),
+        .if_pc              (if_pc_out)
+    );
+
+    assign imem_addr = (if_translation_ready && !mmu_i_resp_page_fault) ? mmu_i_resp_paddr : if_pc_out;
+
+    assign if_instr_compressed = (imem_rdata[1:0] != 2'b11);
+    assign if_instr_len        = if_instr_compressed ? 3'd2 : 3'd4;
+
+    compressed_decoder u_compressed_decoder (
+        .compressed_instr (imem_rdata[15:0]),
+        .expanded_instr   (if_compressed_expanded_instr),
+        .illegal_instr    (if_compressed_illegal)
+    );
+
+    always_comb begin
+        if_instr_to_decode = imem_rdata;
+        if (if_instr_compressed) begin
+            if_instr_to_decode = if_compressed_illegal ? 32'h0000_0000 : if_compressed_expanded_instr;
+        end
+    end
+
+    pmp_checker u_pmp_checker_if (
+        .req_valid       (if_translation_ready && !mmu_i_resp_page_fault),
+        .req_paddr       (mmu_i_resp_paddr),
+        .req_size        (4'd4),
+        .req_is_fetch    (1'b1),
+        .req_is_store    (1'b0),
+        .priv_mode       (csr_priv_mode_cur),
+        .pmpcfg_entries  (csr_pmpcfg_entries),
+        .pmpaddr_entries (csr_pmpaddr_entries),
+        .access_fault    (if_instr_access_fault)
+    );
+
+    if_id_reg u_if_id_reg (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_if_id_stall_en),
+        .flush_en           (hu_if_id_flush_en),
+        .if_pc                  (if_pc_out),
+        .if_instr               (if_instr_to_decode),
+        .if_instr_len           (if_instr_len),
+        .if_instr_access_fault  (if_instr_access_fault),
+        .if_instr_page_fault    (if_instr_page_fault),
+        .id_pc                  (if_id_pc),
+        .id_instr               (if_id_instr),
+        .id_instr_len           (if_id_instr_len),
+        .id_instr_access_fault  (if_id_instr_access_fault),
+        .id_instr_page_fault    (if_id_instr_page_fault)
+    );
+
+    // ---------------------------------------------------------
+    // (C) ID Stage - Instruction Decode & RegFile
+    // ---------------------------------------------------------
+    id_stage u_id_stage (
+        .clk                (clk),
+        .rst                (rst),
+        .if_pc              (if_id_pc),
+        .if_instr           (if_id_instr),
+        .if_instr_len       (if_id_instr_len),
+        .csr_priv_mode      (csr_priv_mode_cur),
+        .csr_mstatus_tsr    (csr_mstatus_value[CSR_MSTATUS_TSR_BIT]),
+        .csr_mstatus_tw     (csr_mstatus_value[CSR_MSTATUS_TW_BIT]),
+        .csr_mstatus_tvm    (csr_mstatus_value[CSR_MSTATUS_TVM_BIT]),
+        .wb_reg_write_en    (wb_reg_write_en_out),
+        .wb_rd_addr         (wb_rd_addr),
+        .wb_rd_wdata        (wb_rd_wdata),
+        .wb_fp_write_en     (wb_fp_write_en),
+        .wb_fd_addr         (wb_fd_addr),
+        .wb_fd_wdata        (wb_fd_wdata),
+        .id_pc              (id_pc_out),
+        .id_instr_len       (id_instr_len),
+        .id_rs1_rdata       (id_rs1_rdata),
+        .id_rs2_rdata       (id_rs2_rdata),
+        .id_fs1_rdata       (id_fs1_rdata),
+        .id_fs2_rdata       (id_fs2_rdata),
+        .id_fs3_rdata       (id_fs3_rdata),
+        .id_rs1_addr        (id_rs1_addr),
+        .id_rs2_addr        (id_rs2_addr),
+        .id_fs3_addr        (id_fs3_addr),
+        .id_imm             (id_imm),
+        .id_alu_op          (id_alu_op),
+        .id_alu_src1_sel    (id_alu_src1_sel),
+        .id_alu_src2_sel    (id_alu_src2_sel),
+        .id_is_word_op      (id_is_word_op),
+        .id_is_branch       (id_is_branch),
+        .id_branch_op       (id_branch_op),
+        .id_is_jump         (id_is_jump),
+        .id_is_load         (id_is_load),
+        .id_is_store        (id_is_store),
+        .id_mem_size        (id_mem_size),
+        .id_is_amo          (id_is_amo),
+        .id_amo_op          (id_amo_op),
+        .id_is_fp           (id_is_fp),
+        .id_fp_reg_write_en (id_fp_reg_write_en),
+        .id_fp_store_data_sel(id_fp_store_data_sel),
+        .id_fp_op           (id_fp_op),
+        .id_fp_fmt          (id_fp_fmt),
+        .id_fp_rm           (id_fp_rm),
+        .id_reg_write_en    (id_reg_write_en),
+        .id_rd_addr         (id_rd_addr),
+        .id_wb_sel          (id_wb_sel),
+        .id_is_csr          (id_is_csr),
+        .id_csr_op          (id_csr_op),
+        .id_illegal_instr   (id_illegal_instr),
+        .id_is_ecall        (id_is_ecall),
+        .id_is_ebreak       (id_is_ebreak),
+        .id_is_mret         (id_is_mret),
+        .id_is_sret         (id_is_sret),
+        .id_is_wfi          (id_is_wfi),
+        .id_is_sfence_vma   (id_is_sfence_vma),
+        .id_is_fence        (id_is_fence),
+        .id_is_fence_i      (id_is_fence_i)
+    );
+
+    id_ex_reg u_id_ex_reg (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_id_ex_stall_en),
+        .flush_en           (hu_id_ex_flush_en),
+        .id_instr           (if_id_instr),
+        .id_instr_len       (id_instr_len),
+        .id_pc              (id_pc_out),
+        .id_rs1_rdata       (id_rs1_rdata),
+        .id_rs2_rdata       (id_rs2_rdata),
+        .id_fs1_rdata       (id_fs1_rdata),
+        .id_fs2_rdata       (id_fs2_rdata),
+        .id_fs3_rdata       (id_fs3_rdata),
+        .id_rs1_addr        (id_rs1_addr),
+        .id_rs2_addr        (id_rs2_addr),
+        .id_fs3_addr        (id_fs3_addr),
+        .id_imm             (id_imm),
+        .id_alu_op          (id_alu_op),
+        .id_alu_src1_sel    (id_alu_src1_sel),
+        .id_alu_src2_sel    (id_alu_src2_sel),
+        .id_is_word_op      (id_is_word_op),
+        .id_is_branch       (id_is_branch),
+        .id_branch_op       (id_branch_op),
+        .id_is_jump         (id_is_jump),
+        .id_is_load         (id_is_load),
+        .id_is_store        (id_is_store),
+        .id_mem_size        (id_mem_size),
+        .id_is_amo          (id_is_amo),
+        .id_amo_op          (id_amo_op),
+        .id_is_fp           (id_is_fp),
+        .id_fp_reg_write_en (id_fp_reg_write_en),
+        .id_fp_store_data_sel(id_fp_store_data_sel),
+        .id_fp_op           (id_fp_op),
+        .id_fp_fmt          (id_fp_fmt),
+        .id_fp_rm           (id_fp_rm),
+        .id_reg_write_en    (id_reg_write_en),
+        .id_rd_addr         (id_rd_addr),
+        .id_wb_sel          (id_wb_sel),
+        .id_is_csr              (id_is_csr),
+        .id_csr_op              (id_csr_op),
+        .id_illegal_instr       (id_illegal_instr),
+        .id_instr_access_fault  (if_id_instr_access_fault),
+        .id_instr_page_fault    (if_id_instr_page_fault),
+        .id_is_ecall            (id_is_ecall),
+        .id_is_ebreak       (id_is_ebreak),
+        .id_is_mret         (id_is_mret),
+        .id_is_sret         (id_is_sret),
+        .id_is_wfi          (id_is_wfi),
+        .id_is_sfence_vma   (id_is_sfence_vma),
+        .id_is_fence        (id_is_fence),
+        .id_is_fence_i      (id_is_fence_i),
+        .ex_pc              (ex_pc_in),
+        .ex_instr           (ex_instr_in),
+        .ex_instr_len       (ex_instr_len_in),
+        .ex_rs1_rdata       (ex_rs1_rdata_in),
+        .ex_rs2_rdata       (ex_rs2_rdata_in),
+        .ex_fs1_rdata       (ex_fs1_rdata_in),
+        .ex_fs2_rdata       (ex_fs2_rdata_in),
+        .ex_fs3_rdata       (ex_fs3_rdata_in),
+        .ex_rs1_addr        (ex_rs1_addr_in),
+        .ex_rs2_addr        (ex_rs2_addr_in),
+        .ex_fs3_addr        (ex_fs3_addr_in),
+        .ex_imm             (ex_imm_in),
+        .ex_alu_op          (ex_alu_op_in),
+        .ex_alu_src1_sel    (ex_alu_src1_sel_in),
+        .ex_alu_src2_sel    (ex_alu_src2_sel_in),
+        .ex_is_word_op      (ex_is_word_op_in),
+        .ex_is_branch       (ex_is_branch_in),
+        .ex_branch_op       (ex_branch_op_in),
+        .ex_is_jump         (ex_is_jump_in),
+        .ex_is_load         (ex_is_load_in),
+        .ex_is_store        (ex_is_store_in),
+        .ex_mem_size        (ex_mem_size_in),
+        .ex_is_amo          (ex_is_amo_in),
+        .ex_amo_op          (ex_amo_op_in),
+        .ex_is_fp           (ex_is_fp_in),
+        .ex_fp_reg_write_en (ex_fp_reg_write_en_in),
+        .ex_fp_store_data_sel(ex_fp_store_data_sel_in),
+        .ex_fp_op           (ex_fp_op_in),
+        .ex_fp_fmt          (ex_fp_fmt_in),
+        .ex_fp_rm           (ex_fp_rm_in),
+        .ex_reg_write_en    (ex_reg_write_en_in),
+        .ex_rd_addr         (ex_rd_addr_in),
+        .ex_wb_sel          (ex_wb_sel_in),
+        .ex_is_csr              (ex_is_csr_in),
+        .ex_csr_op              (ex_csr_op_in),
+        .ex_illegal_instr       (ex_illegal_instr_in),
+        .ex_instr_access_fault  (ex_instr_access_fault_in),
+        .ex_instr_page_fault    (ex_instr_page_fault_in),
+        .ex_is_ecall            (ex_is_ecall_in),
+        .ex_is_ebreak       (ex_is_ebreak_in),
+        .ex_is_mret         (ex_is_mret_in),
+        .ex_is_sret         (ex_is_sret_in),
+        .ex_is_wfi          (ex_is_wfi_in),
+        .ex_is_sfence_vma   (ex_is_sfence_vma_in),
+        .ex_is_fence        (ex_is_fence_in),
+        .ex_is_fence_i      (ex_is_fence_i_in)
+    );
+
+    // ---------------------------------------------------------
+    // (D) EX Stage - Execution & Forwarding
+    // ---------------------------------------------------------
+
+    always_comb begin
+        unique case (mem_wb_sel_in)
+            2'b00:   mem_forward_data = mem_alu_result_in;
+            2'b01:   mem_forward_data = mw_reg_mem_rdata_in;
+            2'b10:   mem_forward_data = mem_pc_in + 64'd4;
+            default: mem_forward_data = '0;
+        endcase
+    end
+    
+    // Operand A Forwarding Mux
+    always_comb begin
+        case (hu_forward_a_sel)
+            2'b01:   ex_rs1_rdata_fwd = mem_forward_data;  // From MEM
+            2'b10:   ex_rs1_rdata_fwd = wb_rd_wdata;       // From WB
+            default: ex_rs1_rdata_fwd = ex_rs1_rdata_in;   // From RegFile
+        endcase
+    end
+
+    // Operand B Forwarding Mux
+    always_comb begin
+        case (hu_forward_b_sel)
+            2'b01:   ex_rs2_rdata_fwd = mem_forward_data;  // From MEM
+            2'b10:   ex_rs2_rdata_fwd = wb_rd_wdata;       // From WB
+            default: ex_rs2_rdata_fwd = ex_rs2_rdata_in;   // From RegFile
+        endcase
+    end
+
+    always_comb begin
+        if (mem_fp_reg_write_en_in && (mem_rd_addr_in == ex_rs1_addr_in)) begin
+            ex_fs1_rdata_fwd = mw_reg_fp_result_in;
+        end else if (wb_fp_write_en && (wb_fd_addr == ex_rs1_addr_in)) begin
+            ex_fs1_rdata_fwd = wb_fd_wdata;
+        end else begin
+            ex_fs1_rdata_fwd = ex_fs1_rdata_in;
+        end
+    end
+
+    always_comb begin
+        if (mem_fp_reg_write_en_in && (mem_rd_addr_in == ex_rs2_addr_in)) begin
+            ex_fs2_rdata_fwd = mw_reg_fp_result_in;
+        end else if (wb_fp_write_en && (wb_fd_addr == ex_rs2_addr_in)) begin
+            ex_fs2_rdata_fwd = wb_fd_wdata;
+        end else begin
+            ex_fs2_rdata_fwd = ex_fs2_rdata_in;
+        end
+    end
+
+    always_comb begin
+        if (mem_fp_reg_write_en_in && (mem_rd_addr_in == ex_fs3_addr_in)) begin
+            ex_fs3_rdata_fwd = mw_reg_fp_result_in;
+        end else if (wb_fp_write_en && (wb_fd_addr == ex_fs3_addr_in)) begin
+            ex_fs3_rdata_fwd = wb_fd_wdata;
+        end else begin
+            ex_fs3_rdata_fwd = ex_fs3_rdata_in;
+        end
+    end
+
+    ex_stage u_ex_stage (
+        .ex_pc              (ex_pc_in),
+        .ex_rs1_rdata       (ex_rs1_rdata_fwd),
+        .ex_rs2_rdata       (ex_rs2_rdata_fwd),
+        .ex_imm             (ex_imm_in),
+        .ex_alu_op          (ex_alu_op_in),
+        .ex_alu_src1_sel    (ex_alu_src1_sel_in),
+        .ex_alu_src2_sel    (ex_alu_src2_sel_in),
+        .ex_is_branch       (ex_is_branch_in),
+        .ex_branch_op       (ex_branch_op_in),
+        .ex_is_jump         (ex_is_jump_in),
+        .ex_is_word_op      (ex_is_word_op_in),
+        .ex_alu_result      (ex_alu_result_out),
+        .ex_branch_taken    (ex_branch_taken_out),
+        .ex_branch_target   (ex_branch_target_out)
+    );
+
+    fpu u_fpu_unit (
+        .fp_op          (ex_fp_op_in),
+        .fp_fmt         (ex_fp_fmt_in),
+        .rm             ((ex_fp_rm_in == 3'b111) ? csr_frm_value : ex_fp_rm_in),
+        .funct3         (ex_instr_in[14:12]),
+        .rs2_addr       (ex_rs2_addr_in),
+        .fs1_data       (ex_fs1_rdata_fwd),
+        .fs2_data       (ex_fs2_rdata_fwd),
+        .fs3_data       (ex_fs3_rdata_fwd),
+        .int_rs1_data   (ex_rs1_rdata_fwd),
+        .fp_result      (ex_fp_result_out),
+        .int_result     (ex_fp_int_result_out),
+        .fflags         (ex_fp_fflags_out),
+        .illegal_instr  (ex_fp_illegal_out)
+    );
+
+    assign ex_result_to_mem = (ex_is_fp_in && ex_reg_write_en_in) ?
+                              ex_fp_int_result_out : ex_alu_result_out;
+
+    ex_mem_reg u_ex_mem_reg (
+        .clk                (clk),
+        .rst                (rst),
+        .stall_en           (hu_ex_mem_stall_en),
+        .flush_en           (trap_flush_req),
+        .ex_alu_result      (ex_result_to_mem),
+        .ex_branch_taken    (ex_branch_taken_out),
+        .ex_branch_target   (ex_branch_target_out),
+        .ex_rs1_rdata       (ex_rs1_rdata_fwd),
+        .ex_rs2_rdata       (ex_rs2_rdata_fwd),
+        .ex_fs1_rdata       (ex_fs1_rdata_fwd),
+        .ex_fs2_rdata       (ex_fs2_rdata_fwd),
+        .ex_fs3_rdata       (ex_fs3_rdata_fwd),
+        .ex_rs1_addr        (ex_rs1_addr_in),
+        .ex_fs3_addr        (ex_fs3_addr_in),
+        .ex_pc              (ex_pc_in),
+        .ex_imm             (ex_imm_in),
+        .ex_instr           (ex_instr_in),
+        .ex_instr_len       (ex_instr_len_in),
+        .ex_is_load         (ex_is_load_in),
+        .ex_is_store        (ex_is_store_in),
+        .ex_mem_size        (ex_mem_size_in),
+        .ex_is_amo          (ex_is_amo_in),
+        .ex_amo_op          (ex_amo_op_in),
+        .ex_is_fp           (ex_is_fp_in),
+        .ex_fp_reg_write_en (ex_fp_reg_write_en_in),
+        .ex_fp_store_data_sel(ex_fp_store_data_sel_in),
+        .ex_fp_op           (ex_fp_op_in),
+        .ex_fp_fmt          (ex_fp_fmt_in),
+        .ex_fp_rm           (ex_fp_rm_in),
+        .ex_fp_result       (ex_fp_result_out),
+        .ex_fp_fflags       (ex_fp_fflags_out),
+        .ex_reg_write_en    (ex_reg_write_en_in),
+        .ex_rd_addr         (ex_rd_addr_in),
+        .ex_wb_sel          (ex_wb_sel_in),
+        .ex_is_csr          (ex_is_csr_in),
+        .ex_csr_op          (ex_csr_op_in),
+        .ex_illegal_instr   (ex_illegal_instr_in | (ex_is_fp_in & ex_fp_illegal_out)),
+        .ex_instr_access_fault(ex_instr_access_fault_in),
+        .ex_instr_page_fault(ex_instr_page_fault_in),
+        .ex_is_ecall        (ex_is_ecall_in),
+        .ex_is_ebreak       (ex_is_ebreak_in),
+        .ex_is_mret         (ex_is_mret_in),
+        .ex_is_sret         (ex_is_sret_in),
+        .ex_is_wfi          (ex_is_wfi_in),
+        .ex_is_sfence_vma   (ex_is_sfence_vma_in),
+        .ex_is_fence        (ex_is_fence_in),
+        .ex_is_fence_i      (ex_is_fence_i_in),
+        .mem_alu_result      (mem_alu_result_in),
+        .mem_branch_taken    (mem_branch_taken_in),
+        .mem_branch_target   (mem_branch_target_in),
+        .mem_rs1_rdata       (mem_rs1_rdata_in),
+        .mem_rs2_rdata       (mem_rs2_rdata_in),
+        .mem_fs1_rdata       (mem_fs1_rdata_in),
+        .mem_fs2_rdata       (mem_fs2_rdata_in),
+        .mem_fs3_rdata       (mem_fs3_rdata_in),
+        .mem_rs1_addr        (mem_rs1_addr_in),
+        .mem_fs3_addr        (mem_fs3_addr_in),
+        .mem_pc              (mem_pc_in),
+        .mem_imm             (mem_imm_in),
+        .mem_instr           (mem_instr_in),
+        .mem_instr_len       (mem_instr_len_in),
+        .mem_is_load         (mem_is_load_in),
+        .mem_is_store        (mem_is_store_in),
+        .mem_mem_size        (mem_mem_size_in),
+        .mem_is_amo          (mem_is_amo_in),
+        .mem_amo_op          (mem_amo_op_in),
+        .mem_is_fp           (mem_is_fp_in),
+        .mem_fp_reg_write_en (mem_fp_reg_write_en_in),
+        .mem_fp_store_data_sel(mem_fp_store_data_sel_in),
+        .mem_fp_op           (mem_fp_op_in),
+        .mem_fp_fmt          (mem_fp_fmt_in),
+        .mem_fp_rm           (mem_fp_rm_in),
+        .mem_fp_result       (mem_fp_result_in),
+        .mem_fp_fflags       (mem_fp_fflags_in),
+        .mem_reg_write_en    (mem_reg_write_en_in),
+        .mem_rd_addr         (mem_rd_addr_in),
+        .mem_wb_sel          (mem_wb_sel_in),
+        .mem_is_csr          (mem_is_csr_in),
+        .mem_csr_op          (mem_csr_op_in),
+        .mem_illegal_instr   (mem_illegal_instr_in),
+        .mem_instr_access_fault(mem_instr_access_fault_in),
+        .mem_instr_page_fault(mem_instr_page_fault_in),
+        .mem_is_ecall        (mem_is_ecall_in),
+        .mem_is_ebreak       (mem_is_ebreak_in),
+        .mem_is_mret         (mem_is_mret_in),
+        .mem_is_sret         (mem_is_sret_in),
+        .mem_is_wfi          (mem_is_wfi_in),
+        .mem_is_sfence_vma   (mem_is_sfence_vma_in),
+        .mem_is_fence        (mem_is_fence_in),
+        .mem_is_fence_i      (mem_is_fence_i_in)
+    );
+
+    // ---------------------------------------------------------
+    // (E) MEM Stage - Memory Access
+    // ---------------------------------------------------------
+    pmp_checker u_pmp_checker_dmem (
+        .req_valid       (data_access_needs_translation && mmu_d_resp_valid && !mmu_d_resp_page_fault),
+        .req_paddr       (mmu_d_resp_paddr),
+        .req_size        (dmem_access_size),
+        .req_is_fetch    (1'b0),
+        .req_is_store    (dmem_req_is_store),
+        .priv_mode       (csr_priv_mode_cur),
+        .pmpcfg_entries  (csr_pmpcfg_entries),
+        .pmpaddr_entries (csr_pmpaddr_entries),
+        .access_fault    (dmem_access_fault)
+    );
+
+    assign dmem_load_access_fault  = dmem_access_fault &
+                                     (mem_is_load_in | (mem_is_amo_in && (mem_amo_op_in == AMO_LR)));
+    assign dmem_store_access_fault = dmem_access_fault & dmem_req_is_store;
+
+    mem_stage u_mem_stage (
+        .clk                (clk),
+        .rst                (rst),
+        .mem_alu_result      (mem_alu_result_in),
+        .mem_branch_taken    (mem_branch_taken_in),
+        .mem_branch_target   (mem_branch_target_in),
+        .mem_rs1_rdata       (mem_rs1_rdata_in),
+        .mem_rs2_rdata       (mem_rs2_rdata_in),
+        .mem_fs2_rdata       (mem_fs2_rdata_in),
+        .mem_rs1_addr        (mem_rs1_addr_in),
+        .mem_pc              (mem_pc_in),
+        .mem_imm             (mem_imm_in),
+        .mem_instr           (mem_instr_in),
+        .mem_instr_len       (mem_instr_len_in),
+        .mem_is_load         (mem_is_load_in),
+        .mem_is_store        (mem_is_store_in),
+        .mem_mem_size        (mem_mem_size_in),
+        .mem_is_amo          (mem_is_amo_in),
+        .mem_amo_op          (mem_amo_op_in),
+        .mem_is_fp           (mem_is_fp_in),
+        .mem_fp_reg_write_en (mem_fp_reg_write_en_in),
+        .mem_fp_store_data_sel(mem_fp_store_data_sel_in),
+        .mem_fp_result       (mem_fp_result_in),
+        .mem_fp_fflags       (mem_fp_fflags_in),
+        .mem_reg_write_en    (mem_reg_write_en_in),
+        .mem_rd_addr         (mem_rd_addr_in),
+        .mem_wb_sel          (mem_wb_sel_in),
+        .mem_is_csr          (mem_is_csr_in),
+        .mem_csr_op          (mem_csr_op_in),
+        .mem_illegal_instr   (mem_illegal_instr_in),
+        .mem_instr_access_fault(mem_instr_access_fault_in),
+        .mem_instr_page_fault(mem_instr_page_fault_in),
+        .mem_is_ecall        (mem_is_ecall_in),
+        .mem_is_ebreak       (mem_is_ebreak_in),
+        .mem_is_mret         (mem_is_mret_in),
+        .mem_is_sret         (mem_is_sret_in),
+        .mem_is_wfi          (mem_is_wfi_in),
+        .mem_is_sfence_vma   (mem_is_sfence_vma_in),
+        .mem_is_fence        (mem_is_fence_in),
+        .mem_is_fence_i      (mem_is_fence_i_in),
+        .dmem_read_req       (raw_dmem_read_en),
+        .dmem_write_req      (raw_dmem_write_en),
+        .dmem_addr           (raw_dmem_addr),
+        .dmem_wdata          (raw_dmem_wdata),
+        .dmem_wstrb          (raw_dmem_wstrb),
+        .dmem_req_ready      (data_translation_ready && !data_translation_page_fault &&
+                              data_memory_port_available),
+        .dmem_rsp_valid      (data_translation_ready && !data_translation_page_fault &&
+                              data_memory_port_available),
+        .dmem_rdata          (dmem_rdata),
+        .dmem_load_access_fault(dmem_load_access_fault),
+        .dmem_store_access_fault(dmem_store_access_fault),
+        .dmem_load_page_fault(dmem_load_page_fault),
+        .dmem_store_page_fault(dmem_store_page_fault),
+        .mem_wb_alu_result   (mw_reg_alu_result_in),
+        .mem_wb_mem_rdata    (mw_reg_mem_rdata_in),
+        .mem_wb_fp_result    (mw_reg_fp_result_in),
+        .mem_wb_fp_fflags    (mw_reg_fp_fflags_in),
+        .mem_wb_pc           (mw_reg_pc_in),
+        .mem_wb_instr        (mw_reg_instr_in),
+        .mem_wb_instr_len    (mw_reg_instr_len_in),
+        .mem_wb_reg_write_en (mw_reg_reg_write_en_in),
+        .mem_wb_fp_reg_write_en(mw_reg_fp_reg_write_en_in),
+        .mem_wb_rd_addr      (mw_reg_rd_addr_in),
+        .mem_wb_wb_sel       (mw_reg_wb_sel_in),
+        .mem_wb_is_csr       (mw_reg_is_csr_in),
+        .mem_wb_csr_op       (mw_reg_csr_op_in),
+        .mem_wb_rs1_rdata    (mw_reg_rs1_rdata_in),
+        .mem_wb_rs1_addr     (mw_reg_rs1_addr_in),
+        .mem_wb_imm          (mw_reg_imm_in),
+        .mem_wb_illegal_instr(mw_reg_illegal_instr_in),
+        .mem_wb_instr_access_fault(mw_reg_instr_access_fault_in),
+        .mem_wb_instr_page_fault(mw_reg_instr_page_fault_in),
+        .mem_wb_is_ecall     (mw_reg_is_ecall_in),
+        .mem_wb_is_ebreak    (mw_reg_is_ebreak_in),
+        .mem_wb_is_mret      (mw_reg_is_mret_in),
+        .mem_wb_is_sret      (mw_reg_is_sret_in),
+        .mem_wb_load_fault   (mw_reg_load_fault_in),
+        .mem_wb_store_fault  (mw_reg_store_fault_in),
+        .mem_wb_load_page_fault(mw_reg_load_page_fault_in),
+        .mem_wb_store_page_fault(mw_reg_store_page_fault_in),
+        .mem_wb_fault_addr   (mw_reg_fault_addr_in),
+        .mem_stall_req       (mem_stall_req)
+    );
+
+    always_comb begin
+        if (ptw_mem_req_valid) begin
+            dmem_read_en  = 1'b1;
+            dmem_write_en = 1'b0;
+            dmem_addr     = ptw_mem_req_addr;
+            dmem_wdata    = '0;
+            dmem_size     = 3'b011;
+        end else begin
+            dmem_read_en  = raw_dmem_read_en && data_translation_ready &&
+                            !data_translation_page_fault;
+            dmem_write_en = raw_dmem_write_en && data_translation_ready &&
+                            !data_translation_page_fault;
+            dmem_addr     = data_access_needs_translation ? mmu_d_resp_paddr : raw_dmem_addr;
+            dmem_wdata    = raw_dmem_wdata;
+            dmem_size     = mem_mem_size_in;
+        end
+    end
+
+    mem_wb_reg u_mem_wb_reg (
+        .clk                 (clk),
+        .rst                 (rst),
+        .stall_en            (1'b0),
+        .flush_en            (trap_flush_req | mem_stall_req),
+        .mem_pc              (mw_reg_pc_in),
+        .mem_alu_result      (mw_reg_alu_result_in),
+        .mem_mem_rdata       (mw_reg_mem_rdata_in),
+        .mem_fp_result       (mw_reg_fp_result_in),
+        .mem_fp_fflags       (mw_reg_fp_fflags_in),
+        .mem_instr           (mw_reg_instr_in),
+        .mem_instr_len       (mw_reg_instr_len_in),
+        .mem_reg_write_en    (mw_reg_reg_write_en_in),
+        .mem_fp_reg_write_en (mw_reg_fp_reg_write_en_in),
+        .mem_rd_addr         (mw_reg_rd_addr_in),
+        .mem_wb_sel          (mw_reg_wb_sel_in),
+        .mem_is_csr          (mw_reg_is_csr_in),
+        .mem_csr_op          (mw_reg_csr_op_in),
+        .mem_rs1_rdata       (mw_reg_rs1_rdata_in),
+        .mem_rs1_addr        (mw_reg_rs1_addr_in),
+        .mem_imm             (mw_reg_imm_in),
+        .mem_illegal_instr   (mw_reg_illegal_instr_in),
+        .mem_instr_access_fault(mw_reg_instr_access_fault_in),
+        .mem_instr_page_fault(mw_reg_instr_page_fault_in),
+        .mem_is_ecall        (mw_reg_is_ecall_in),
+        .mem_is_ebreak       (mw_reg_is_ebreak_in),
+        .mem_is_mret         (mw_reg_is_mret_in),
+        .mem_is_sret         (mw_reg_is_sret_in),
+        .mem_load_fault      (mw_reg_load_fault_in),
+        .mem_store_fault     (mw_reg_store_fault_in),
+        .mem_load_page_fault (mw_reg_load_page_fault_in),
+        .mem_store_page_fault(mw_reg_store_page_fault_in),
+        .mem_fault_addr      (mw_reg_fault_addr_in),
+        .wb_pc               (wb_pc_in),
+        .wb_alu_result       (wb_alu_result_in),
+        .wb_mem_rdata        (wb_mem_rdata_in),
+        .wb_fp_result        (wb_fp_result_in),
+        .wb_fp_fflags        (wb_fp_fflags_in),
+        .wb_instr            (wb_instr_in),
+        .wb_instr_len        (wb_instr_len_in),
+        .wb_reg_write_en     (wb_reg_write_en_in),
+        .wb_fp_reg_write_en  (wb_fp_reg_write_en_in),
+        .wb_rd_addr          (wb_rd_addr_in),
+        .wb_wb_sel           (wb_wb_sel_in),
+        .wb_is_csr           (wb_is_csr_in),
+        .wb_csr_op           (wb_csr_op_in),
+        .wb_rs1_rdata        (wb_rs1_rdata_in),
+        .wb_rs1_addr         (wb_rs1_addr_in),
+        .wb_imm              (wb_imm_in),
+        .wb_illegal_instr    (wb_illegal_instr_in),
+        .wb_instr_access_fault(wb_instr_access_fault),
+        .wb_instr_page_fault (wb_instr_page_fault),
+        .wb_is_ecall         (wb_is_ecall_in),
+        .wb_is_ebreak        (wb_is_ebreak_in),
+        .wb_is_mret          (wb_is_mret_in),
+        .wb_is_sret          (wb_is_sret_in),
+        .wb_load_fault       (wb_load_fault),
+        .wb_store_fault      (wb_store_fault),
+        .wb_load_page_fault  (wb_load_page_fault),
+        .wb_store_page_fault (wb_store_page_fault),
+        .wb_fault_addr       (wb_fault_addr_in)
+    );
+
+    // ---------------------------------------------------------
+    // (F) WB Stage - Write Back & System Interaction
+    // ---------------------------------------------------------
+    wb_stage u_wb_stage (
+        .mem_wb_alu_result  (wb_alu_result_in),
+        .mem_wb_mem_rdata   (wb_mem_rdata_in),
+        .mem_wb_fp_result   (wb_fp_result_in),
+        .mem_wb_fp_fflags   (wb_fp_fflags_in),
+        .mem_wb_pc          (wb_pc_in),
+        .mem_wb_pc_plus_4   (wb_pc_in + {{(XLEN-3){1'b0}}, wb_instr_len_in}),
+        .mem_wb_reg_write_en(wb_reg_write_en_in),
+        .mem_wb_fp_reg_write_en(wb_fp_reg_write_en_in),
+        .mem_wb_rd_addr     (wb_rd_addr_in),
+        .mem_wb_wb_sel      (wb_wb_sel_in),
+        .mem_wb_is_csr      (wb_is_csr_in),
+        .mem_wb_csr_op      (wb_csr_op_in),
+        .mem_wb_rs1_rdata   (wb_rs1_rdata_in),
+        .mem_wb_rs1_addr    (wb_rs1_addr_in),
+        .mem_wb_imm         (wb_imm_in),
+        .mem_wb_instr       (wb_instr_in),
+        .mem_wb_illegal_instr(wb_illegal_instr_in),
+        .mem_wb_instr_access_fault(wb_instr_access_fault),
+        .mem_wb_instr_page_fault(wb_instr_page_fault),
+        .mem_wb_is_ecall    (wb_is_ecall_in),
+        .mem_wb_is_ebreak   (wb_is_ebreak_in),
+        .mem_wb_is_mret     (wb_is_mret_in),
+        .mem_wb_is_sret     (wb_is_sret_in),
+        .mem_wb_load_fault  (wb_load_fault),
+        .mem_wb_store_fault (wb_store_fault),
+        .mem_wb_load_page_fault(wb_load_page_fault),
+        .mem_wb_store_page_fault(wb_store_page_fault),
+        .mem_wb_fault_addr  (wb_fault_addr_in),
+        .csr_rdata          (csr_rdata),
+        .csr_illegal_access (csr_illegal_access),
+        .wb_rd_wdata        (wb_rd_wdata),
+        .wb_rd_addr         (wb_rd_addr),
+        .wb_reg_write_en    (wb_reg_write_en_out),
+        .wb_fd_wdata        (wb_fd_wdata),
+        .wb_fd_addr         (wb_fd_addr),
+        .wb_fp_write_en     (wb_fp_write_en),
+        .wb_fp_fflags       (wb_fp_fflags),
+        .wb_fp_state_dirty  (wb_fp_state_dirty),
+        .wb_fp_fflags_valid (wb_fp_fflags_valid),
+        .csr_req            (csr_req),
+        .csr_op             (csr_op),
+        .csr_addr           (csr_addr),
+        .csr_rs1_data       (csr_rs1_data),
+        .trap_illegal_instr (trap_illegal_instr),
+        .trap_is_ecall      (trap_is_ecall),
+        .trap_is_ebreak     (trap_is_ebreak),
+        .trap_is_mret       (trap_is_mret),
+        .trap_is_sret       (trap_is_sret),
+        .trap_epc           (trap_epc),
+        .trap_instr_access_fault(trap_instr_access_fault),
+        .trap_instr_page_fault(trap_instr_page_fault),
+        .trap_load_fault    (trap_load_fault),
+        .trap_store_fault   (trap_store_fault),
+        .trap_load_page_fault(trap_load_page_fault),
+        .trap_store_page_fault(trap_store_page_fault),
+        .trap_bad_addr      (trap_bad_addr),
+        .trap_bad_instr     (trap_bad_instr)
+    );
+
+    csr_unit u_csr_unit (
+        .clk                (clk),
+        .rst                (rst),
+        .csr_req            (csr_req),
+        .csr_op             (csr_op),
+        .csr_addr           (csr_addr),
+        .csr_rs1_data       (csr_rs1_data),
+        .trap_ctrl_trigger  (trap_ctrl_trigger),
+        .trap_ctrl_to_s_mode(trap_ctrl_to_s_mode),
+        .trap_ctrl_mepc     (trap_ctrl_mepc),
+        .trap_ctrl_mcause   (trap_ctrl_mcause),
+        .trap_ctrl_mtval    (trap_ctrl_mtval),
+        .trap_ctrl_mret_en  (trap_ctrl_mret_en),
+        .trap_ctrl_sret_en  (trap_ctrl_sret_en),
+        .m_timer_interrupt(m_timer_interrupt),
+        .m_software_interrupt(m_software_interrupt),
+        .m_external_interrupt(m_external_interrupt),
+        .s_timer_interrupt  (s_timer_interrupt),
+        .s_software_interrupt(s_software_interrupt),
+        .s_external_interrupt(s_external_interrupt),
+        .fp_state_dirty     (wb_fp_state_dirty),
+        .fp_fflags_valid    (wb_fp_fflags_valid),
+        .fp_fflags          (wb_fp_fflags),
+        .csr_rdata          (csr_rdata),
+        .csr_mstatus        (csr_mstatus_value),
+        .csr_sstatus        (csr_sstatus_value),
+        .csr_medeleg        (csr_medeleg_value),
+        .csr_mideleg        (csr_mideleg_value),
+        .csr_mie            (csr_mie_value),
+        .csr_sie            (csr_sie_value),
+        .csr_mip            (csr_mip_value),
+        .csr_sip            (csr_sip_value),
+        .csr_mtvec          (csr_mtvec_base),
+        .csr_stvec          (csr_stvec_base),
+        .csr_mepc           (csr_mepc_value),
+        .csr_sepc           (csr_sepc_value),
+        .csr_satp           (csr_satp_value),
+        .csr_frm            (csr_frm_value),
+        .csr_priv_mode          (csr_priv_mode_cur),
+        .csr_trap_vector        (),
+        .csr_trap_to_s_mode     (),
+        .csr_trap_vector_next   (csr_trap_vector_value),
+        .csr_trap_to_s_mode_next(csr_trap_to_s_mode),
+        .csr_pmpcfg_entries     (csr_pmpcfg_entries),
+        .csr_pmpaddr_entries    (csr_pmpaddr_entries),
+        .csr_illegal_access     (csr_illegal_access)
+    );
+
+    trap_controller u_trap_ctrl (
+        .wb_pc               (wb_pc_in),
+        .trap_illegal_instr  (trap_illegal_instr),
+        .trap_is_ecall       (trap_is_ecall),
+        .trap_is_ebreak      (trap_is_ebreak),
+        .trap_is_mret        (trap_is_mret),
+        .trap_is_sret        (trap_is_sret),
+        .trap_instr_access_fault(trap_instr_access_fault),
+        .trap_load_fault     (trap_load_fault),
+        .trap_store_fault    (trap_store_fault),
+        .trap_instr_page_fault(trap_instr_page_fault),
+        .trap_load_page_fault(trap_load_page_fault),
+        .trap_store_page_fault(trap_store_page_fault),
+        .trap_bad_addr       (trap_bad_addr),
+        .trap_bad_instr      (trap_bad_instr),
+        .csr_mstatus_mie     (csr_mstatus_mie),
+        .csr_mie             (csr_mie_value),
+        .csr_sie             (csr_sie_value),
+        .csr_mip             (csr_mip_value),
+        .csr_sip             (csr_sip_value),
+        .csr_mstatus         (csr_mstatus_value),
+        .csr_mtvec           (csr_mtvec_base),
+        .csr_stvec           (csr_stvec_base),
+        .csr_mepc            (csr_mepc_value),
+        .csr_sepc            (csr_sepc_value),
+        .csr_medeleg         (csr_medeleg_value),
+        .csr_mideleg         (csr_mideleg_value),
+        .csr_priv_mode       (csr_priv_mode_cur),
+        .trap_trigger        (trap_ctrl_trigger),
+        .trap_mepc           (trap_ctrl_mepc),
+        .trap_mcause         (trap_ctrl_mcause),
+        .trap_mtval          (trap_ctrl_mtval),
+        .trap_to_s_mode      (trap_ctrl_to_s_mode),
+        .mret_en             (trap_ctrl_mret_en),
+        .sret_en             (trap_ctrl_sret_en),
+        .trap_flush_req      (trap_flush_req),
+        .trap_pc_sel         (trap_pc_sel_raw),
+        .trap_next_pc        (trap_next_pc_raw)
+    );
+
+    assign trap_pc_sel = trap_pc_sel_raw;
+    assign trap_next_pc = trap_next_pc_raw;
+
+endmodule
diff --git a/rv64cpu/rtl/packages/csr_pkg.sv b/rv64cpu/rtl/packages/csr_pkg.sv
new file mode 100644
index 0000000..d1f9416
--- /dev/null
+++ b/rv64cpu/rtl/packages/csr_pkg.sv
@@ -0,0 +1,232 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/include/csr_pkg.sv
+// Description: Global enumerations, address definitions, and exception reason constants related to CSR and privilege-level architecture
+
+package csr_pkg;
+    import rv64_pkg::*;
+
+    typedef enum logic [2:0] { 
+        CSR_OP_NONE  = 3'b000,
+        CSR_OP_WRITE = 3'b001,
+        CSR_OP_SET   = 3'b010,
+        CSR_OP_CLEAR = 3'b011
+    } csr_op_t;
+
+    // ---------------------------------
+    // Floating-Point CSRs
+    // ---------------------------------
+    localparam logic [11:0]     CSR_FFLAGS   = 12'h001;    // Floating-Point accrued exception flags
+    localparam logic [11:0]     CSR_FRM      = 12'h002;    // Floating-Point dynamic rounding mode
+    localparam logic [11:0]     CSR_FCSR     = 12'h003;    // Combined fflags/frm register
+
+    // ---------------------------------
+    // Machine Mode CSRs (M-Mode)
+    // ---------------------------------
+    localparam logic [11:0]     CSR_MVENDORID = 12'hF11;    // Vendor ID
+    localparam logic [11:0]     CSR_MARCHID   = 12'hF12;    // Architecture ID
+    localparam logic [11:0]     CSR_MIMPID    = 12'hF13;    // Implementation ID
+    localparam logic [11:0]     CSR_MHARTID   = 12'hF14;    // Hart ID
+    localparam logic [11:0]     CSR_MCONFIGPTR= 12'hF15;    // Machine Configuration Pointer
+
+    localparam logic [11:0]     CSR_MSTATUS   = 12'h300;    // Machine Status Register
+    localparam logic [11:0]     CSR_MISA      = 12'h301;    // Machine ISA Register
+    localparam logic [11:0]     CSR_MEDELEG   = 12'h302;    // Exception Delegation
+    localparam logic [11:0]     CSR_MIDELEG   = 12'h303;    // Interrupt Delegation
+    localparam logic [11:0]     CSR_MIE       = 12'h304;    // Machine Interrupt Enable Register
+    localparam logic [11:0]     CSR_MTVEC     = 12'h305;    // Machine Trap Vector Register
+    localparam logic [11:0]     CSR_MCOUNTEREN= 12'h306;    // Machine Counter Enable Register
+
+    localparam logic [11:0]     CSR_MENVCFG   = 12'h30A;    // Machine Environment Configuration
+    localparam logic [11:0]     CSR_MSTATEEN0 = 12'h30C;    // Machine State Enable 0
+    localparam logic [11:0]     CSR_MSTATEEN1 = 12'h30D;    // Machine State Enable 1
+    localparam logic [11:0]     CSR_MSTATEEN2 = 12'h30E;    // Machine State Enable 2
+    localparam logic [11:0]     CSR_MSTATEEN3 = 12'h30F;    // Machine State Enable 3
+
+    localparam logic [11:0]     CSR_PMPCFG0   = 12'h3A0;    // PMP Configuration Register 0
+    localparam logic [11:0]     CSR_PMPCFG15  = 12'h3AF;    // PMP Configuration Register 15
+    localparam logic [11:0]     CSR_PMPADDR0  = 12'h3B0;    // PMP Address Register 0
+    localparam logic [11:0]     CSR_PMPADDR63 = 12'h3EF;    // PMP Address Register 63
+
+    localparam logic [11:0]     CSR_MSCRATCH  = 12'h340;    // Machine Scratch Register
+    localparam logic [11:0]     CSR_MEPC      = 12'h341;    // Machine Exception Program Counter
+    localparam logic [11:0]     CSR_MCAUSE    = 12'h342;    // Machine Cause Register
+    localparam logic [11:0]     CSR_MTVAL     = 12'h343;    // Machine Trap Value Register
+    localparam logic [11:0]     CSR_MIP       = 12'h344;    // Machine Interrupt Pending Register
+    localparam logic [11:0]     CSR_MTINST    = 12'h34A;    // Machine Trap Instruction
+    localparam logic [11:0]     CSR_MTVAL2    = 12'h34B;    // Machine Second Trap Value
+
+    localparam logic [11:0]     CSR_MCOUNTINHIBIT = 12'h320; // Machine Counter-Inhibit Register
+    localparam logic [11:0]     CSR_MCYCLE    = 12'hB00;    // Machine Cycle Counter
+    localparam logic [11:0]     CSR_MINSTRET  = 12'hB02;    // Machine Instruction Retired Counter
+    localparam logic [11:0]     CSR_MHPMCOUNTER3  = 12'hB03; // Machine HPM Counter 3
+    localparam logic [11:0]     CSR_MHPMCOUNTER31 = 12'hB1F; // Machine HPM Counter 31
+    localparam logic [11:0]     CSR_MHPMEVENT3    = 12'h323; // Machine HPM Event Selector 3
+    localparam logic [11:0]     CSR_MHPMEVENT31   = 12'h33F; // Machine HPM Event Selector 31
+
+    // ---------------------------------
+    // Supervisor Mode CSRs (S-Mode)
+    // ---------------------------------
+    localparam logic [11:0]     CSR_SSTATUS   = 12'h100;    // Supervisor Status Register
+    localparam logic [11:0]     CSR_SIE       = 12'h104;    // Supervisor Interrupt Enable Register
+    localparam logic [11:0]     CSR_STVEC     = 12'h105;    // Supervisor Trap Vector Register
+    localparam logic [11:0]     CSR_SCOUNTEREN= 12'h106;    // Supervisor Counter Enable Register
+    localparam logic [11:0]     CSR_SENVCFG   = 12'h10A;    // Supervisor Environment Configuration
+    localparam logic [11:0]     CSR_SSTATEEN0 = 12'h10C;    // Supervisor State Enable 0
+    localparam logic [11:0]     CSR_SSTATEEN1 = 12'h10D;    // Supervisor State Enable 1
+    localparam logic [11:0]     CSR_SSTATEEN2 = 12'h10E;    // Supervisor State Enable 2
+    localparam logic [11:0]     CSR_SSTATEEN3 = 12'h10F;    // Supervisor State Enable 3
+    localparam logic [11:0]     CSR_SSCRATCH  = 12'h140;    // Supervisor Scratch Register
+    localparam logic [11:0]     CSR_SEPC      = 12'h141;    // Supervisor Exception Program Counter
+    localparam logic [11:0]     CSR_SCAUSE    = 12'h142;    // Supervisor Cause Register
+    localparam logic [11:0]     CSR_STVAL     = 12'h143;    // Supervisor Trap Value Register
+    localparam logic [11:0]     CSR_SIP       = 12'h144;    // Supervisor Interrupt Pending Register
+    localparam logic [11:0]     CSR_SATP      = 12'h180;    // Supervisor Address Translation and Protection Register
+
+    // ---------------------------------
+    // Hypervisor CSRs (H-Extension)
+    // ---------------------------------
+    localparam logic [11:0]     CSR_HSTATUS     = 12'h600;  // Hypervisor Status
+    localparam logic [11:0]     CSR_HEDELEG     = 12'h602;  // Hypervisor Exception Delegation
+    localparam logic [11:0]     CSR_HIDELEG     = 12'h603;  // Hypervisor Interrupt Delegation
+    localparam logic [11:0]     CSR_HIE         = 12'h604;  // Hypervisor Interrupt Enable
+    localparam logic [11:0]     CSR_HTIMEDELTA  = 12'h605;  // Hypervisor Timer Delta
+    localparam logic [11:0]     CSR_HCOUNTEREN  = 12'h606;  // Hypervisor Counter Enable
+    localparam logic [11:0]     CSR_HGEIE       = 12'h607;  // Hypervisor Guest External Interrupt Enable
+    localparam logic [11:0]     CSR_HENVCFG     = 12'h60A;  // Hypervisor Environment Configuration
+    localparam logic [11:0]     CSR_HSTATEEN0   = 12'h60C;  // Hypervisor State Enable 0
+    localparam logic [11:0]     CSR_HSTATEEN1   = 12'h60D;  // Hypervisor State Enable 1
+    localparam logic [11:0]     CSR_HSTATEEN2   = 12'h60E;  // Hypervisor State Enable 2
+    localparam logic [11:0]     CSR_HSTATEEN3   = 12'h60F;  // Hypervisor State Enable 3
+    localparam logic [11:0]     CSR_HTVAL       = 12'h643;  // Hypervisor Trap Value
+    localparam logic [11:0]     CSR_HIP         = 12'h644;  // Hypervisor Interrupt Pending
+    localparam logic [11:0]     CSR_HVIP        = 12'h645;  // Hypervisor Virtual Interrupt Pending
+    localparam logic [11:0]     CSR_HTINST      = 12'h64A;  // Hypervisor Trap Instruction
+    localparam logic [11:0]     CSR_HGATP       = 12'h680;  // Hypervisor Guest Address Translation and Protection
+    localparam logic [11:0]     CSR_HGEIP       = 12'hE12;  // Hypervisor Guest External Interrupt Pending
+
+    localparam logic [11:0]     CSR_VSSTATUS    = 12'h200;  // Virtual Supervisor Status
+    localparam logic [11:0]     CSR_VSIE        = 12'h204;  // Virtual Supervisor Interrupt Enable
+    localparam logic [11:0]     CSR_VSTVEC      = 12'h205;  // Virtual Supervisor Trap Vector
+    localparam logic [11:0]     CSR_VSSCRATCH   = 12'h240;  // Virtual Supervisor Scratch
+    localparam logic [11:0]     CSR_VSEPC       = 12'h241;  // Virtual Supervisor Exception PC
+    localparam logic [11:0]     CSR_VSCAUSE     = 12'h242;  // Virtual Supervisor Cause
+    localparam logic [11:0]     CSR_VSTVAL      = 12'h243;  // Virtual Supervisor Trap Value
+    localparam logic [11:0]     CSR_VSIP        = 12'h244;  // Virtual Supervisor Interrupt Pending
+    localparam logic [11:0]     CSR_VSATP       = 12'h280;  // Virtual Supervisor Address Translation and Protection
+
+    // ---------------------------------
+    // User Mode CSRs (U-Mode / URW)
+    // ---------------------------------
+    localparam logic [11:0]     CSR_CYCLE     = 12'hC00;    // Cycle Counter
+    localparam logic [11:0]     CSR_TIME      = 12'hC01;    // Timer Counter
+    localparam logic [11:0]     CSR_INSTRET   = 12'hC02;    // Instruction Retired Counter
+    localparam logic [11:0]     CSR_HPMCOUNTER3  = 12'hC03; // HPM Counter 3
+    localparam logic [11:0]     CSR_HPMCOUNTER31 = 12'hC1F; // HPM Counter 31
+
+    // ---------------------------------
+    // Exception Cause Codes (mcause.Interrupt == 0)
+    // ---------------------------------
+    localparam logic [XLEN-1:0] EXC_INST_ADDR_MISALIGNED        = 64'd0;    // Instruction Address Misaligned
+    localparam logic [XLEN-1:0] EXC_INST_FAULT                  = 64'd1;    // Instruction Access Fault
+    localparam logic [XLEN-1:0] EXC_INST_ILLEGAL                = 64'd2;    // Illegal Instruction
+    localparam logic [XLEN-1:0] EXC_BREAKPOINT                  = 64'd3;    // Breakpoint
+    localparam logic [XLEN-1:0] EXC_LOAD_ADDR_MISALIGNED        = 64'd4;    // Load Address Misaligned
+    localparam logic [XLEN-1:0] EXC_LOAD_FAULT                  = 64'd5;    // Load Access Fault
+    localparam logic [XLEN-1:0] EXC_STORE_ADDR_MISALIGNED       = 64'd6;    // Store Address Misaligned
+    localparam logic [XLEN-1:0] EXC_STORE_FAULT                 = 64'd7;    // Store Access Fault
+    localparam logic [XLEN-1:0] EXC_ECALL_FROM_U                = 64'd8;    // Environment Call from U-Mode
+    localparam logic [XLEN-1:0] EXC_ECALL_FROM_S                = 64'd9;    // Environment Call from S-Mode
+    localparam logic [XLEN-1:0] EXC_ECALL_FROM_M                = 64'd11;   // Environment Call from M-Mode
+    localparam logic [XLEN-1:0] EXC_INSTR_PAGE_FAULT            = 64'd12;   // Instruction Page Fault
+    localparam logic [XLEN-1:0] EXC_LOAD_PAGE_FAULT             = 64'd13;   // Load Page Fault
+    localparam logic [XLEN-1:0] EXC_STORE_PAGE_FAULT            = 64'd15;   // Store Page Fault
+
+    localparam logic [XLEN-1:0] MEDELEG_WRITABLE_MASK =
+        (64'h1 << EXC_INST_ADDR_MISALIGNED) |
+        (64'h1 << EXC_INST_FAULT) |
+        (64'h1 << EXC_INST_ILLEGAL) |
+        (64'h1 << EXC_BREAKPOINT) |
+        (64'h1 << EXC_LOAD_ADDR_MISALIGNED) |
+        (64'h1 << EXC_LOAD_FAULT) |
+        (64'h1 << EXC_STORE_ADDR_MISALIGNED) |
+        (64'h1 << EXC_STORE_FAULT) |
+        (64'h1 << EXC_ECALL_FROM_U) |
+        (64'h1 << EXC_ECALL_FROM_S) |
+        (64'h1 << EXC_INSTR_PAGE_FAULT) |
+        (64'h1 << EXC_LOAD_PAGE_FAULT) |
+        (64'h1 << EXC_STORE_PAGE_FAULT);
+
+    localparam logic [XLEN-1:0] MEDELEG_READ_ONLY_ONE_MASK = '0;
+
+    // ---------------------------------
+    // Interrupt Cause Codes (mcause.Interrupt == 1)
+    // ---------------------------------
+    // User Mode Interrupts
+    localparam logic [XLEN-1:0] INTERRUPT_U_SOFTWARE            = 64'd0;    // User Mode Software Interrupt
+    localparam logic [XLEN-1:0] INTERRUPT_U_TIMER               = 64'd4;    // User Mode Timer Interrupt
+    localparam logic [XLEN-1:0] INTERRUPT_U_EXTERNAL            = 64'd8;    // User Mode External Interrupt
+    // Supervisor Mode Interrupts
+    localparam logic [XLEN-1:0] INTERRUPT_S_SOFTWARE            = 64'd1;    // Supervisor Mode Software Interrupt
+    localparam logic [XLEN-1:0] INTERRUPT_S_TIMER               = 64'd5;    // Supervisor Mode Timer Interrupt
+    localparam logic [XLEN-1:0] INTERRUPT_S_EXTERNAL            = 64'd9;    // Supervisor Mode External Interrupt
+    // Machine Mode Interrupts
+    localparam logic [XLEN-1:0] INTERRUPT_M_SOFTWARE            = 64'd3;    // Machine Mode Software Interrupt
+    localparam logic [XLEN-1:0] INTERRUPT_M_TIMER               = 64'd7;    // Machine Mode Timer Interrupt
+    localparam logic [XLEN-1:0] INTERRUPT_M_EXTERNAL            = 64'd11;   // Machine Mode External Interrupt
+
+    localparam logic [XLEN-1:0] MIDELEG_WRITABLE_MASK =
+        (64'h1 << INTERRUPT_S_SOFTWARE) |
+        (64'h1 << INTERRUPT_S_TIMER) |
+        (64'h1 << INTERRUPT_S_EXTERNAL);
+
+    // ---------------------------------
+    // Physical Memory Protection (PMP)
+    // ---------------------------------
+    localparam int PMP_ENTRY_COUNT = 64;
+    localparam int PMP_CFG_CSR_COUNT = 16;
+    localparam int PMP_CFG_ENTRIES_PER_RV64_CSR = 8;
+
+    typedef logic [7:0]      pmpcfg_entry_t;
+    typedef logic [XLEN-1:0] pmpaddr_entry_t;
+
+    localparam int PMPCFG_R_BIT = 0;
+    localparam int PMPCFG_W_BIT = 1;
+    localparam int PMPCFG_X_BIT = 2;
+    localparam int PMPCFG_A_LO  = 3;
+    localparam int PMPCFG_A_HI  = 4;
+    localparam int PMPCFG_L_BIT = 7;
+
+    localparam logic [1:0] PMP_MODE_OFF   = 2'b00;
+    localparam logic [1:0] PMP_MODE_TOR   = 2'b01;
+    localparam logic [1:0] PMP_MODE_NA4   = 2'b10;
+    localparam logic [1:0] PMP_MODE_NAPOT = 2'b11;
+
+    // ---------------------------------
+    // Environment Configuration / State Enable
+    // ---------------------------------
+    localparam int ENVCFG_FIOM_BIT     = 0;
+    localparam int ENVCFG_CBIE_LO      = 4;
+    localparam int ENVCFG_CBIE_HI      = 5;
+    localparam int ENVCFG_CBCFE_BIT    = 6;
+    localparam int ENVCFG_CBZE_BIT     = 7;
+    localparam int ENVCFG_PMM_LO       = 32;
+    localparam int ENVCFG_PMM_HI       = 33;
+    localparam int STATEEN0_ENVCFG_BIT = 62;
+    localparam int STATEEN_SE0_BIT     = 63;
+
+endpackage
diff --git a/rv64cpu/rtl/packages/rv64_pkg.sv b/rv64cpu/rtl/packages/rv64_pkg.sv
new file mode 100644
index 0000000..afa7979
--- /dev/null
+++ b/rv64cpu/rtl/packages/rv64_pkg.sv
@@ -0,0 +1,162 @@
+//     Richard CPU
+//     Copyright (C) 2026  Richard Qin
+
+//     This program is free software: you can redistribute it and/or modify
+//     it under the terms of the GNU General Public License as published by
+//     the Free Software Foundation, either version 3 of the License, or
+//     (at your option) any later version.
+
+//     This program is distributed in the hope that it will be useful,
+//     but WITHOUT ANY WARRANTY; without even the implied warranty of
+//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//     GNU General Public License for more details.
+
+//     You should have received a copy of the GNU General Public License
+//     along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+// File path: rtl/core/include/rv64_pkg.sv
+// Description: RISC-V 64-bit base instruction set global definition package
+
+package rv64_pkg;
+
+    localparam XLEN       = 64;
+    localparam INST_WIDTH = 32;
+
+    // RISC-V Base Opcode Map (inst[6:0])
+    // Reference: RISC-V Unprivileged ISA V20240411, Table 34
+    typedef enum logic [6:0] {
+        // Loads
+        OPCODE_LOAD      = 7'b000_0011,  // LB/LH/LW/LD/LBU/LHU/LWU
+        OPCODE_LOAD_FP   = 7'b000_0111,  // FLW/FLD
+
+        // Misc-Mem
+        OPCODE_MISC_MEM  = 7'b000_1111,  // FENCE / FENCE.I (Zifencei)
+
+        // Immediate ALU
+        OPCODE_OP_IMM    = 7'b001_0011,  // ADDI/SLTI/ANDI/ORI/XORI/SLLI/SRLI/SRAI...
+        OPCODE_OP_IMM_32 = 7'b001_1011,  // ADDIW/SLLIW/SRLIW/SRAIW (RV64 only)
+
+        // Upper Immediate
+        OPCODE_AUIPC     = 7'b001_0111,  // AUIPC
+        OPCODE_LUI       = 7'b011_0111,  // LUI
+
+        // Stores
+        OPCODE_STORE     = 7'b010_0011,  // SB/SH/SW/SD
+        OPCODE_STORE_FP  = 7'b010_0111,  // FSW/FSD
+
+        // Atomic (A extension)
+        OPCODE_AMO       = 7'b010_1111,  // LR/SC/AMOSWAP/AMOADD/...
+
+        // Register ALU
+        OPCODE_OP        = 7'b011_0011,  // ADD/SUB/AND/OR/XOR/SLL/SRL/SRA/SLT/SLTU/MUL/DIV...
+        OPCODE_OP_32     = 7'b011_1011,  // ADDW/SUBW/SLLW/SRLW/SRAW/MULW/DIVW... (RV64 only)
+
+        // Floating-point fused multiply-add
+        OPCODE_MADD      = 7'b100_0011,  // FMADD.S/D
+        OPCODE_MSUB      = 7'b100_0111,  // FMSUB.S/D
+        OPCODE_NMSUB     = 7'b100_1011,  // FNMSUB.S/D
+        OPCODE_NMADD     = 7'b100_1111,  // FNMADD.S/D
+
+        // Floating-point arithmetic
+        OPCODE_OP_FP     = 7'b101_0011,  // FADD/FSUB/FMUL/FDIV/FSQRT/FCMP/FCVT/FMV/FCLASS...
+
+        // Branches
+        OPCODE_BRANCH    = 7'b110_0011,  // BEQ/BNE/BLT/BGE/BLTU/BGEU
+
+        // Jumps
+        OPCODE_JALR      = 7'b110_0111,  // JALR
+        OPCODE_JAL       = 7'b110_1111,  // JAL
+
+        // System
+        OPCODE_SYSTEM    = 7'b111_0011,  // ECALL/EBREAK/CSR*/MRET/SRET/WFI/SFENCE.VMA
+
+        // Custom (reserved for extensions)
+        OPCODE_CUSTOM_0  = 7'b000_1011,
+        OPCODE_CUSTOM_1  = 7'b010_1011,
+        OPCODE_CUSTOM_2  = 7'b101_1011,
+        OPCODE_CUSTOM_3  = 7'b111_1011
+    } opcode_t;
+
+    // ALU Operation Encoding
+    // Derived from funct3 + funct7[5] of R-type / I-type instructions
+    typedef enum logic [4:0] {
+        ALU_ADD    = 5'd0,   // Add
+        ALU_SUB    = 5'd8,   // Subtract
+        ALU_SLL    = 5'd1,   // Shift Left Logical
+        ALU_SLT    = 5'd2,   // Set Less Than (signed)
+        ALU_SLTU   = 5'd3,   // Set Less Than (unsigned)
+        ALU_XOR    = 5'd4,   // Exclusive Or
+        ALU_SRL    = 5'd5,   // Shift Right Logical
+        ALU_SRA    = 5'd13,  // Shift Right Arithmetic
+        ALU_OR     = 5'd6,   // Or
+        ALU_AND    = 5'd7,   // And
+        ALU_MUL    = 5'd16,  // Low XLEN bits of signed multiplication
+        ALU_MULH   = 5'd17,  // High XLEN bits of signed x signed multiplication
+        ALU_MULHSU = 5'd18,  // High XLEN bits of signed x unsigned multiplication
+        ALU_MULHU  = 5'd19,  // High XLEN bits of unsigned x unsigned multiplication
+        ALU_DIV    = 5'd20,  // Signed division
+        ALU_DIVU   = 5'd21,  // Unsigned division
+        ALU_REM    = 5'd22,  // Signed remainder
+        ALU_REMU   = 5'd23   // Unsigned remainder
+    } alu_op_t;
+
+
+    // Branch Operation Encoding
+    // Derived from funct3 of B-type instructions
+    typedef enum logic [2:0] {
+        BRANCH_EQ  = 3'b000,  // BEQ:  Branch if Equal
+        BRANCH_NE  = 3'b001,  // BNE:  Branch if Not Equal
+        BRANCH_LT  = 3'b100,  // BLT:  Branch if Less Than (signed)
+        BRANCH_GE  = 3'b101,  // BGE:  Branch if Greater Than or Equal (signed)
+        BRANCH_LTU = 3'b110,  // BLTU: Branch if Less Than (unsigned)
+        BRANCH_GEU = 3'b111   // BGEU: Branch if Greater Than or Equal (unsigned)
+    } branch_op_t;
+
+    typedef enum logic [3:0] {
+        AMO_NONE = 4'd0,
+        AMO_LR   = 4'd1,
+        AMO_SC   = 4'd2,
+        AMO_SWAP = 4'd3,
+        AMO_ADD  = 4'd4,
+        AMO_XOR  = 4'd5,
+        AMO_AND  = 4'd6,
+        AMO_OR   = 4'd7,
+        AMO_MIN  = 4'd8,
+        AMO_MAX  = 4'd9,
+        AMO_MINU = 4'd10,
+        AMO_MAXU = 4'd11
+    } amo_op_t;
+
+    typedef enum logic [4:0] {
+        FP_OP_NONE     = 5'd0,
+        FP_OP_ADD      = 5'd1,
+        FP_OP_SUB      = 5'd2,
+        FP_OP_MUL      = 5'd3,
+        FP_OP_DIV      = 5'd4,
+        FP_OP_SQRT     = 5'd5,
+        FP_OP_SGNJ     = 5'd6,
+        FP_OP_SGNJN    = 5'd7,
+        FP_OP_SGNJX    = 5'd8,
+        FP_OP_MIN      = 5'd9,
+        FP_OP_MAX      = 5'd10,
+        FP_OP_CVT_F2I  = 5'd11,
+        FP_OP_CVT_I2F  = 5'd12,
+        FP_OP_CVT_F2F  = 5'd13,
+        FP_OP_MV_F2X   = 5'd14,
+        FP_OP_MV_X2F   = 5'd15,
+        FP_OP_EQ       = 5'd16,
+        FP_OP_LT       = 5'd17,
+        FP_OP_LE       = 5'd18,
+        FP_OP_CLASS    = 5'd19,
+        FP_OP_MADD     = 5'd20,
+        FP_OP_MSUB     = 5'd21,
+        FP_OP_NMSUB    = 5'd22,
+        FP_OP_NMADD    = 5'd23
+    } fp_op_t;
+
+    typedef enum logic [1:0] {
+        FP_FMT_S = 2'd0,
+        FP_FMT_D = 2'd1
+    } fp_fmt_t;
+
+endpackage
diff --git a/rv64cpu/scripts/gen_hex.sh b/rv64cpu/scripts/gen_hex.sh
new file mode 100755
index 0000000..8ba4c94
--- /dev/null
+++ b/rv64cpu/scripts/gen_hex.sh
@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+# ─── gen_hex.sh ──────────────────────────────────────────────────────────────────
+# Convert an ELF file to Verilog hex format for $readmemh
+#
+# Usage: ./scripts/gen_hex.sh <input.elf> [output.hex]
+# ─────────────────────────────────────────────────────────────────────────────────
+set -euo pipefail
+
+CROSS_PREFIX="${CROSS_PREFIX:-riscv64-unknown-elf-}"
+
+if [ $# -lt 1 ]; then
+    echo "Usage: $0 <input.elf> [output.hex]"
+    exit 1
+fi
+
+INPUT="$1"
+OUTPUT="${2:-${INPUT%.elf}.hex}"
+
+${CROSS_PREFIX}objcopy -O verilog "$INPUT" "$OUTPUT"
+echo "[gen_hex] $INPUT → $OUTPUT"
+
+# Also generate .bin and disassembly for inspection
+${CROSS_PREFIX}objcopy -O binary "$INPUT" "${INPUT%.elf}.bin"
+${CROSS_PREFIX}objdump -d -M no-aliases "$INPUT" > "${INPUT%.elf}.dump"
+echo "[gen_hex] Also generated .bin and .dump"
diff --git a/rv64cpu/scripts/wave_view.sh b/rv64cpu/scripts/wave_view.sh
new file mode 100755
index 0000000..5a3a6e4
--- /dev/null
+++ b/rv64cpu/scripts/wave_view.sh
@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+# ─── wave_view.sh ────────────────────────────────────────────────────────────────
+# Open a VCD/FST waveform file in GTKWave
+#
+# Usage: ./scripts/wave_view.sh [waveform_file]
+#        ./scripts/wave_view.sh                    # opens latest .vcd in waves/
+# ─────────────────────────────────────────────────────────────────────────────────
+set -euo pipefail
+
+WAVES_DIR="$(dirname "$0")/../waves"
+VIEWER="${WAVE_VIEWER:-gtkwave}"
+
+if [ $# -ge 1 ]; then
+    WAVE_FILE="$1"
+else
+    # Open the most recently modified waveform file
+    WAVE_FILE=$(ls -t "$WAVES_DIR"/*.vcd "$WAVES_DIR"/*.fst 2>/dev/null | head -1)
+    if [ -z "$WAVE_FILE" ]; then
+        echo "No waveform files found in $WAVES_DIR"
+        exit 1
+    fi
+fi
+
+echo "[wave_view] Opening: $WAVE_FILE"
+$VIEWER "$WAVE_FILE" &