prerelease: 0.92: fix uninitialized usrdata->cpu_topology_list

Change-Id: Ia12970bda1225898823a67c2d0461144fc62ebb9

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.6)
+cmake_minimum_required(VERSION 3.11)
 
 if (NOT CMAKE_BUILD_TYPE)
 	set (CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type: Debug Release..." FORCE)
@@ -10,7 +10,7 @@ project(mckernel C ASM)
 set(MCKERNEL_VERSION "1.7.1")
 
 # See "Fedora Packaging Guidelines -- Versioning"
-set(MCKERNEL_RELEASE "0.3")
+set(MCKERNEL_RELEASE "0.92")
 
 set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules)
 # for rpmbuild
@@ -64,6 +64,50 @@ if(ENABLE_TOFU)
 	set(KBUILD_C_FLAGS "${KBUILD_C_FLAGS} -DENABLE_TOFU")
 endif()
 
+# when compiling on a compute-node
+execute_process(COMMAND bash -c "grep $(hostname) /etc/opt/FJSVfefs/config/fefs_node1.csv 2>/dev/null | cut -d, -f2"
+  OUTPUT_VARIABLE FUGAKU_NODE_TYPE OUTPUT_STRIP_TRAILING_WHITESPACE)
+if(FUGAKU_NODE_TYPE STREQUAL "CN")
+	option(ENABLE_FUGAKU_HACKS "Fugaku hacks" ON)
+else()
+	option(ENABLE_FUGAKU_HACKS "Fugaku hacks" OFF)
+endif()
+
+if(ENABLE_FUGAKU_HACKS)
+	add_definitions(-DENABLE_FUGAKU_HACKS)
+	set(KBUILD_C_FLAGS "${KBUILD_C_FLAGS} -DENABLE_FUGAKU_HACKS")
+endif()
+
+# krm that mandates reserved memory amount >= available at boot time?
+execute_process(COMMAND bash -c "rpm -qi FJSVpxkrm-plugin-mckernel | awk '$1 == \"Version\" && $2 == \":\" { print $3 }'"
+  OUTPUT_VARIABLE KRM_VERSION OUTPUT_STRIP_TRAILING_WHITESPACE)
+message("KRM_VERSION: ${KRM_VERSION}")
+
+if(NOT "${KRM_VERSION}" STREQUAL "" AND "${KRM_VERSION}" VERSION_LESS_EQUAL 4.0.1)
+	option(ENABLE_KRM_WORKAROUND "krm workaround" ON)
+else()
+	option(ENABLE_KRM_WORKAROUND "krm workaround" OFF)
+endif()
+
+if(ENABLE_KRM_WORKAROUND)
+	add_definitions(-DENABLE_KRM_WORKAROUND)
+	set(KBUILD_C_FLAGS "${KBUILD_C_FLAGS} -DENABLE_KRM_WORKAROUND")
+endif()
+
+# SIGSTOP instead of SIGSEGV, additional IHK Linux kmsg
+option(ENABLE_FUGAKU_DEBUG "Fugaku debug instrumentation" OFF)
+
+if(ENABLE_FUGAKU_DEBUG)
+	add_definitions(-DENABLE_FUGAKU_DEBUG)
+	set(KBUILD_C_FLAGS "${KBUILD_C_FLAGS} -DENABLE_FUGAKU_DEBUG")
+endif()
+
+option(PROFILE_ENABLE "System call profile" ON)
+if(PROFILE_ENABLE)
+	add_definitions(-DPROFILE_ENABLE)
+	set(KBUILD_C_FLAGS "${KBUILD_C_FLAGS} -DPROFILE_ENABLE")
+endif()
+
 option(ENABLE_LINUX_WORK_IRQ_FOR_IKC "Use Linux work IRQ for IKC IPI" ON)
 if (ENABLE_LINUX_WORK_IRQ_FOR_IKC)
 	set(KBUILD_C_FLAGS "${KBUILD_C_FLAGS} -DIHK_IKC_USE_LINUX_WORK_IRQ")
@@ -267,6 +311,10 @@ message("MAP_KERNEL_START: ${MAP_KERNEL_START}")
 message("ENABLE_MEMDUMP: ${ENABLE_MEMDUMP}")
 message("ENABLE_PERF: ${ENABLE_PERF}")
 message("ENABLE_TOFU: ${ENABLE_TOFU}")
+message("ENABLE_FUGAKU_HACKS: ${ENABLE_FUGAKU_HACKS}")
+message("ENABLE_FUGAKU_DEBUG: ${ENABLE_FUGAKU_DEBUG}")
+message("ENABLE_KRM_WORKAROUND: ${ENABLE_KRM_WORKAROUND}")
+message("PROFILE_ENABLE: ${PROFILE_ENABLE}")
 message("ENABLE_RUSAGE: ${ENABLE_RUSAGE}")
 message("ENABLE_QLMPI: ${ENABLE_QLMPI}")
 message("ENABLE_UTI: ${ENABLE_UTI}")

arch/arm64/kernel/cpu.c

@@ -730,6 +730,49 @@ static void show_context_stack(struct pt_regs *regs)
 	}
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+void __show_context_stack(struct thread *thread,
+        unsigned long pc, uintptr_t sp, int kprintf_locked)
+{
+    uintptr_t stack_top;
+    unsigned long irqflags = 0;
+
+    stack_top = ALIGN_UP(sp, (uintptr_t)KERNEL_STACK_SIZE);
+
+    if (!kprintf_locked)
+        irqflags = kprintf_lock();
+
+    __kprintf("TID: %d, call stack (most recent first):\n",
+        thread->tid);
+    __kprintf("PC: %016lx, SP: %016lx\n", pc, sp);
+    for (;;) {
+        extern char _head[], _end[];
+        uintptr_t *fp, *lr;
+        fp = (uintptr_t *)sp;
+        lr = (uintptr_t *)(sp + 8);
+
+        if ((*fp <= sp)) {
+            break;
+        }
+
+        if ((*fp > stack_top)) {
+            break;
+        }
+
+        if ((*lr < (unsigned long)_head) ||
+            (*lr > (unsigned long)_end)) {
+            break;
+        }
+
+        __kprintf("PC: %016lx, SP: %016lx, FP: %016lx\n", *lr - 4, sp, *fp);
+        sp = *fp;
+    }
+
+    if (!kprintf_locked)
+        kprintf_unlock(irqflags);
+}
+#endif
+
 void handle_IPI(unsigned int vector, struct pt_regs *regs)
 {
 	struct ihk_mc_interrupt_handler *h;
@@ -791,6 +834,19 @@ void cpu_safe_halt(void)
 	cpu_enable_interrupt();
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+/*@
+  @ assigns \nothing;
+  @ ensures \interrupt_disabled == 0;
+  @*/
+void cpu_halt_panic(void)
+{
+	extern void __cpu_do_idle(void);
+	cpu_enable_interrupt();
+	__cpu_do_idle();
+}
+#endif
+
 #if defined(CONFIG_HAS_NMI)
 #include <arm-gic-v3.h>
 
@@ -856,6 +912,21 @@ unsigned long cpu_enable_interrupt_save(void)
 	return flags;
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+int cpu_interrupt_disabled(void)
+{
+	unsigned long flags;
+	unsigned long masked = ICC_PMR_EL1_MASKED;
+
+	asm volatile(
+		"mrs_s  %0, " __stringify(ICC_PMR_EL1)
+		: "=&r" (flags)
+		:
+		: "memory");
+	return (flags == masked);
+}
+#endif
+
 #else /* defined(CONFIG_HAS_NMI) */
 
 /* @ref.impl arch/arm64/include/asm/irqflags.h::arch_local_irq_enable */
@@ -1377,6 +1448,14 @@ void arch_print_stack(void)
 {
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+unsigned long arch_get_instruction_address(const void *reg)
+{
+	const struct pt_regs *regs = (struct pt_regs *)reg;
+	return regs->pc;
+}
+#endif
+
 void arch_show_interrupt_context(const void *reg)
 {
 	const struct pt_regs *regs = (struct pt_regs *)reg;

arch/arm64/kernel/include/arch-lock.h

@@ -9,6 +9,9 @@
 #include "affinity.h"
 #include <lwk/compiler.h>
 #include "config.h"
+#ifdef ENABLE_FUGAKU_HACKS
+#include <ihk/debug.h>
+#endif
 
 //#define DEBUG_SPINLOCK
 //#define DEBUG_MCS_RWLOCK
@@ -31,6 +34,10 @@ typedef struct {
 #endif /* __AARCH64EB__ */
 } __attribute__((aligned(4))) ihk_spinlock_t;
 
+#ifdef ENABLE_FUGAKU_HACKS
+extern ihk_spinlock_t *get_this_cpu_runq_lock(void);
+#endif
+
 extern void preempt_enable(void);
 extern void preempt_disable(void);
 
@@ -98,6 +105,18 @@ static int __ihk_mc_spinlock_trylock_noirq(ihk_spinlock_t *lock)
 	: "memory");
 
 	success = !tmp;
+
+#ifdef ENABLE_FUGAKU_HACKS
+#if 0
+	if (success) {
+		if (get_this_cpu_runq_lock() == lock &&
+				!cpu_interrupt_disabled()) {
+			kprintf("%s: WARNING: runq lock held without IRQs disabled?\n", __func__); \
+		}
+	}
+#endif
+#endif
+
 	if (!success) {
 		preempt_enable();
 	}
@@ -182,6 +201,14 @@ static void __ihk_mc_spinlock_lock_noirq(ihk_spinlock_t *lock)
 	: "=&r" (lockval), "=&r" (newval), "=&r" (tmp), "+Q" (*lock)
 	: "Q" (lock->owner), "I" (1 << TICKET_SHIFT)
 	: "memory");
+#ifdef ENABLE_FUGAKU_HACKS
+#if 0
+	if (get_this_cpu_runq_lock() == lock &&
+			!cpu_interrupt_disabled()) {
+		kprintf("%s: WARNING: runq lock held without IRQs disabled?\n", __func__); \
+	}
+#endif
+#endif
 }
 
 #ifdef DEBUG_SPINLOCK

arch/arm64/kernel/include/syscall_list.h

@@ -124,7 +124,7 @@ SYSCALL_HANDLED(271, process_vm_writev)
 SYSCALL_HANDLED(281, execveat)
 SYSCALL_HANDLED(700, get_cpu_id)
 #ifdef PROFILE_ENABLE
-SYSCALL_HANDLED(__NR_profile, profile)
+SYSCALL_HANDLED(PROFILE_EVENT_MAX, profile)
 #endif // PROFILE_ENABLE
 SYSCALL_HANDLED(730, util_migrate_inter_kernel)
 SYSCALL_HANDLED(731, util_indicate_clone)

arch/arm64/kernel/irq-gic-v3.c

@@ -7,6 +7,9 @@
 #include <process.h>
 #include <syscall.h>
 #include <ihk/debug.h>
+#ifdef ENABLE_FUGAKU_HACKS
+#include <ihk/monitor.h>
+#endif
 #include <arch-timer.h>
 #include <cls.h>
 
@@ -313,14 +316,27 @@ void handle_interrupt_gicv3(struct pt_regs *regs)
 	struct cpu_local_var *v = get_this_cpu_local_var();
 	//unsigned long irqflags;
 	int do_check = 0;
+#ifdef ENABLE_FUGAKU_HACKS
+	struct ihk_os_cpu_monitor *monitor = cpu_local_var(monitor);
 
+	++v->in_interrupt;
+#endif
 	irqnr = gic_read_iar();
 	cpu_enable_nmi();
 	set_cputime(from_user ? CPUTIME_MODE_U2K : CPUTIME_MODE_K2K_IN);
 	while (irqnr != ICC_IAR1_EL1_SPURIOUS) {
 		if ((irqnr < 1020) || (irqnr >= 8192)) {
 			gic_write_eoir(irqnr);
+#ifndef ENABLE_FUGAKU_HACKS
 			handle_IPI(irqnr, regs);
+#else
+			/* Once paniced, only allow CPU stop and NMI IRQs */
+			if (monitor->status != IHK_OS_MONITOR_PANIC ||
+					irqnr == INTRID_CPU_STOP ||
+					irqnr == INTRID_MULTI_NMI) {
+				handle_IPI(irqnr, regs);
+			}
+#endif
 		}
 		irqnr = gic_read_iar();
 	}
@@ -335,7 +351,12 @@ void handle_interrupt_gicv3(struct pt_regs *regs)
 	}
 	//ihk_mc_spinlock_unlock(&v->runq_lock, irqflags);
 
+#ifndef ENABLE_FUGAKU_HACKS
 	if (do_check) {
+#else
+	--v->in_interrupt;
+	if (monitor->status != IHK_OS_MONITOR_PANIC && do_check) {
+#endif
 		check_signal(0, regs, 0);
 		schedule();
 	}

arch/arm64/kernel/syscall.c

@@ -1071,6 +1071,9 @@ static int setup_rt_frame(int usig, unsigned long rc, int to_restart,
 
 	if (k->sa.sa_flags & SA_RESTORER){
 		regs->regs[30] = (unsigned long)k->sa.sa_restorer;
+#ifdef ENABLE_FUGAKU_HACKS
+		kprintf("%s: SA_RESTORER: 0x%lx\n", __func__, regs->regs[30]);
+#endif
 	} else {
 		regs->regs[30] = (unsigned long)VDSO_SYMBOL(thread->vm->vdso_addr, sigtramp);
 	}
@@ -1723,6 +1726,7 @@ SYSCALL_DECLARE(mmap)
 
 	/* check arguments */
 	pgsize = PAGE_SIZE;
+#ifndef ENABLE_FUGAKU_HACKS
 	if (flags & MAP_HUGETLB) {
 		int hugeshift = flags & (0x3F << MAP_HUGE_SHIFT);
 
@@ -1763,6 +1767,11 @@ SYSCALL_DECLARE(mmap)
 			goto out;
 		}
 	}
+#else
+	if (flags & MAP_HUGETLB) {
+		flags &= ~(MAP_HUGETLB);
+	}
+#endif
 
 #define	VALID_DUMMY_ADDR	((region->user_start + PTL3_SIZE - 1) & ~(PTL3_SIZE - 1))
 	addr = (flags & MAP_FIXED)? addr0: VALID_DUMMY_ADDR;

arch/x86_64/kernel/cpu.c

@@ -868,6 +868,49 @@ void show_context_stack(uintptr_t *rbp) {
 	return;
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+void __show_context_stack(struct thread *thread,
+        unsigned long pc, uintptr_t sp, int kprintf_locked)
+{
+    uintptr_t stack_top;
+    unsigned long irqflags = 0;
+
+    stack_top = ALIGN_UP(sp, (uintptr_t)KERNEL_STACK_SIZE);
+
+    if (!kprintf_locked)
+        irqflags = kprintf_lock();
+
+    __kprintf("TID: %d, call stack (most recent first):\n",
+        thread->tid);
+    __kprintf("PC: %016lx, SP: %016lx\n", pc, sp);
+    for (;;) {
+        extern char _head[], _end[];
+        uintptr_t *fp, *lr;
+        fp = (uintptr_t *)sp;
+        lr = (uintptr_t *)(sp + 8);
+
+        if ((*fp <= sp)) {
+            break;
+        }
+
+        if ((*fp > stack_top)) {
+            break;
+        }
+
+        if ((*lr < (unsigned long)_head) ||
+            (*lr > (unsigned long)_end)) {
+            break;
+        }
+
+        __kprintf("PC: %016lx, SP: %016lx, FP: %016lx\n", *lr - 4, sp, *fp);
+        sp = *fp;
+    }
+
+    if (!kprintf_locked)
+        kprintf_unlock(irqflags);
+}
+#endif
+
 void interrupt_exit(struct x86_user_context *regs)
 {
 	if (interrupt_from_user(regs)) {
@@ -1137,6 +1180,17 @@ void cpu_halt(void)
 	asm volatile("hlt");
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+/*@
+  @ assigns \nothing;
+  @ ensures \interrupt_disabled == 0;
+  @*/
+void cpu_halt_panic(void)
+{
+    cpu_halt();
+}
+#endif
+
 /*@
   @ assigns \nothing;
   @ ensures \interrupt_disabled == 0;
@@ -1521,6 +1575,16 @@ void arch_print_stack(void)
 	__print_stack(rbp, 0);
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+unsigned long arch_get_instruction_address(const void *reg)
+{
+	const struct x86_user_context *uctx = reg;
+	const struct x86_basic_regs *regs = &uctx->gpr;
+
+	return regs->rip;
+}
+#endif
+
 /*@
   @ requires \valid(reg);
   @ assigns \nothing;

arch/x86_64/kernel/include/arch-memory.h

@@ -451,4 +451,12 @@ extern unsigned long ap_trampoline;
 
 /* Local is cachable */
 #define IHK_IKC_QUEUE_PT_ATTR (PTATTR_NO_EXECUTE | PTATTR_WRITABLE)
+
+#ifdef ENABLE_FUGAKU_HACKS
+#ifndef __ASSEMBLY__
+# define ALIGN_UP(x, align)     ALIGN_DOWN((x) + (align) - 1, align)
+# define ALIGN_DOWN(x, align)   ((x) & ~((align) - 1))
+#endif /* !__ASSEMBLY__ */
+#endif
+
 #endif

arch/x86_64/kernel/include/cpulocal.h

@@ -53,5 +53,9 @@ struct x86_cpu_local_variables *get_x86_this_cpu_local(void);
 void *get_x86_cpu_local_kstack(int id);
 void *get_x86_this_cpu_kstack(void);
 
+#ifdef ENABLE_FUGAKU_HACKS
+#define LOCALS_SPAN (4 * PAGE_SIZE)
+#define KERNEL_STACK_SIZE LOCALS_SPAN
+#endif
 
 #endif

arch/x86_64/kernel/include/syscall_list.h

@@ -168,7 +168,7 @@ SYSCALL_HANDLED(311, process_vm_writev)
 SYSCALL_HANDLED(322, execveat)
 SYSCALL_HANDLED(700, get_cpu_id)
 #ifdef PROFILE_ENABLE
-SYSCALL_HANDLED(__NR_profile, profile)
+SYSCALL_HANDLED(PROFILE_EVENT_MAX, profile)
 #endif // PROFILE_ENABLE
 SYSCALL_HANDLED(730, util_migrate_inter_kernel)
 SYSCALL_HANDLED(731, util_indicate_clone)

arch/x86_64/kernel/local.c

@@ -21,7 +21,9 @@
 #include <registers.h>
 #include <string.h>
 
+#ifndef ENABLE_FUGAKU_HACKS
 #define LOCALS_SPAN (4 * PAGE_SIZE)
+#endif
 
 struct x86_cpu_local_variables *locals;
 size_t x86_cpu_local_variables_span = LOCALS_SPAN;	/* for debugger */

docs/spec/ihk.rst

@@ -2,17 +2,6 @@
    :suffix: .
    :depth: 3
 
-External Specs
+Specifications
 ==============
-
+The specifications pdf is :download:`here <ihk.pdf>`
-Overview
---------
-
-Function Specs
---------------
-
-Command / Daemon Specs
-----------------------
-
-Booting LWK
-===========

docs/spec/mckernel.rst

@@ -2,8 +2,6 @@
    :suffix: .
    :depth: 3
 
-Interfaces
+Specifications
-==========
+==============
-
+The specifications pdf is :download:`here <mckernel.pdf>`
-Interface details
-=================

docs/users.rst

@@ -34,6 +34,19 @@ For example, with Fujitsu Technical Computing Suite (TCS), you need to specify `
 
    #PJM -L jobenv=mck1
 
+(Optional, Fujitsu TCS only) Specify boot parameters
+----------------------------------------------------
+
+You can specify the boot parameters by defining environmental variables and pass them to Fujitsu TCS.
+The parameters include the resource reservation settings, resource reservation amount, kernel arguments and routing of message channels between McKernel CPUs and Linux CPUs.
+See `IHK Specifications - ihk_create_os_str() <spec/ihk.html>`__ for the parameter names and allowed values.
+The example of setting the memory amount is shown below.
+
+.. code-block:: none
+
+   export IHK_MEM="7G@4,7G@5,7G@6,7G@7"
+   pjsub -X run.sh
+
 Insert ``mcexec`` into the command line
 ---------------------------------------
 
@@ -183,3 +196,9 @@ Limitations
 
 26. mmap() allows unlimited overcommit. Note that it corresponds to
     setting sysctl ``vm.overcommit_memory`` to 1.
+
+27. mlockall() is not supported and returns -EPERM.
+
+28. munlockall() is not supported and returns zero.
+
+29. scheduling behavior is not Linux compatible. For example, sometimes one of the two processes on the same CPU continues to run after yielding.

executer/kernel/mcctrl/arch/arm64/archdeps.c

@@ -8,6 +8,10 @@
 #endif /* LINUX_VERSION_CODE >= KERNEL_VERSION(4, 11, 0) */
 #include <linux/ptrace.h>
 #include <linux/uaccess.h>
+#include <linux/mmu_notifier.h>
+#include <linux/kref.h>
+#include <linux/file.h>
+#include <linux/proc_fs.h>
 #include <asm/vdso.h>
 #include "config.h"
 #include "../../mcctrl.h"
@@ -36,6 +40,29 @@ int (*mcctrl_tof_utofu_release_cq)(struct inode *inode,
 struct file_operations *mcctrl_tof_utofu_procfs_ops_bch;
 int (*mcctrl_tof_utofu_release_bch)(struct inode *inode,
 		struct file *filp);
+int (*mcctrl_tof_core_cq_cacheflush)(int tni, int cqid);
+int (*mcctrl_tof_core_disable_bch)(int tni, int bgid);
+int (*mcctrl_tof_core_unset_bg)(int tni, int bgid);
+typedef void (*tof_core_signal_handler)(int, int, uint64_t, uint64_t);
+void (*mcctrl_tof_core_register_signal_bg)(int tni, int bgid,
+			tof_core_signal_handler handler);
+struct tof_utofu_bg;
+struct tof_utofu_bg *mcctrl_tof_utofu_bg;
+
+
+/* Tofu MMU notifier */
+struct mmu_notifier_ops *mcctrl_tof_utofu_mn_ops;
+struct mmu_notifier_ops __mcctrl_tof_utofu_mn_ops;
+static void (*mcctrl_tof_utofu_mn_invalidate_range_end)(
+	struct mmu_notifier *mn,
+	struct mm_struct *mm,
+	unsigned long start,
+	unsigned long end);
+void __mcctrl_tof_utofu_mn_invalidate_range_end(
+	struct mmu_notifier *mn,
+	struct mm_struct *mm,
+	unsigned long start,
+	unsigned long end);
 #endif
 
 int arch_symbols_init(void)
@@ -74,6 +101,49 @@ int arch_symbols_init(void)
 		(void *)kallsyms_lookup_name("tof_utofu_release_bch");
 	if (WARN_ON(!mcctrl_tof_utofu_release_bch))
 		return -EFAULT;
+
+	mcctrl_tof_core_cq_cacheflush =
+		(void *)kallsyms_lookup_name("tof_core_cq_cacheflush");
+	if (WARN_ON(!mcctrl_tof_core_cq_cacheflush))
+		return -EFAULT;
+
+	mcctrl_tof_core_disable_bch =
+		(void *)kallsyms_lookup_name("tof_core_disable_bch");
+	if (WARN_ON(!mcctrl_tof_core_disable_bch))
+		return -EFAULT;
+
+	mcctrl_tof_core_unset_bg =
+		(void *)kallsyms_lookup_name("tof_core_unset_bg");
+	if (WARN_ON(!mcctrl_tof_core_unset_bg))
+		return -EFAULT;
+
+	mcctrl_tof_core_register_signal_bg =
+		(void *)kallsyms_lookup_name("tof_core_register_signal_bg");
+	if (WARN_ON(!mcctrl_tof_core_register_signal_bg))
+		return -EFAULT;
+
+	mcctrl_tof_utofu_bg =
+		(void *)kallsyms_lookup_name("tof_utofu_bg");
+	if (WARN_ON(!mcctrl_tof_utofu_bg))
+		return -EFAULT;
+
+	mcctrl_tof_utofu_mn_ops =
+		(void *)kallsyms_lookup_name("tof_utofu_mn_ops");
+	if (WARN_ON(!mcctrl_tof_utofu_mn_ops))
+		return -EFAULT;
+	/*
+	 * Copy original content and update redirected function,
+	 * CQ will be pointed to this structure after init ioctl()
+	 */
+	memcpy(&__mcctrl_tof_utofu_mn_ops, mcctrl_tof_utofu_mn_ops,
+			sizeof(*mcctrl_tof_utofu_mn_ops));
+	__mcctrl_tof_utofu_mn_ops.invalidate_range =
+		__mcctrl_tof_utofu_mn_invalidate_range_end;
+
+	mcctrl_tof_utofu_mn_invalidate_range_end =
+		(void *)kallsyms_lookup_name("tof_utofu_mn_invalidate_range_end");
+	if (WARN_ON(!mcctrl_tof_utofu_mn_invalidate_range_end))
+		return -EFAULT;
 #endif
 
 	return 0;
@@ -364,6 +434,15 @@ int translate_rva_to_rpa(ihk_os_t os, unsigned long rpt, unsigned long rva,
 
 	// page table to translation_table.
 	phys = ihk_device_map_memory(ihk_os_to_dev(os), rpt, PAGE_SIZE);
+#ifdef ENABLE_FUGAKU_HACKS
+	if (!phys) {
+		pr_err("%s(): ERROR: VA: 0x%lx, rpt is NULL for PID %d\n",
+			__func__, rva, task_tgid_vnr(current));
+		error = -EFAULT;
+		goto out;
+	}
+#endif
+
 	tbl = ihk_device_map_virtual(ihk_os_to_dev(os), phys, PAGE_SIZE, NULL, 0);
 	rpa = (unsigned long)tbl->tt_pa;
 
@@ -465,6 +544,7 @@ void mcctrl_tofu_hijack_release_handlers(void)
 		__mcctrl_tof_utofu_release_cq;
 	mcctrl_tof_utofu_procfs_ops_bch->release =
 		__mcctrl_tof_utofu_release_bch;
+	wmb();
 }
 
 void mcctrl_tofu_restore_release_handlers(void)
@@ -473,8 +553,327 @@ void mcctrl_tofu_restore_release_handlers(void)
 		mcctrl_tof_utofu_release_cq;
 	mcctrl_tof_utofu_procfs_ops_bch->release =
 		mcctrl_tof_utofu_release_bch;
+	wmb();
 }
 
+/*
+ * Tofu cleanup functions
+ */
+#include <tofu/tof_uapi.h>
+#include <tofu/tof_icc.h>
+#include <tofu/tofu_generated-tof_core_cq.h>
+#include <tofu/tofu_generated-tof_utofu_device.h>
+#include <tofu/tofu_generated-tof_utofu_cq.h>
+#include <tofu/tofu_generated-tof_utofu_mbpt.h>
+#include <tofu/tofu_generated-tof_utofu_bg.h>
+
+#define TOF_UTOFU_VERSION TOF_UAPI_VERSION
+#define TOF_UTOFU_NUM_STAG_NTYPES 3
+#define TOF_UTOFU_NUM_STAG_BITS(size) ((size) + 13)
+#define TOF_UTOFU_NUM_STAG(size) ((uint64_t)1 << TOF_UTOFU_NUM_STAG_BITS(size))
+#define TOF_UTOFU_STAG_TRANS_BITS 3
+#define TOF_UTOFU_STAG_TRANS_SIZE ((uint64_t)1 << TOF_UTOFU_STAG_TRANS_BITS)
+#define TOF_UTOFU_STAG_TRANS_TABLE_LEN(size) (TOF_UTOFU_NUM_STAG(size) * TOF_UTOFU_STAG_TRANS_SIZE)
+#define TOF_UTOFU_STEERING_TABLE_LEN(size) (TOF_UTOFU_NUM_STAG(size) * TOF_ICC_STEERING_SIZE)
+#define TOF_UTOFU_MB_TABLE_LEN(size) (TOF_UTOFU_NUM_STAG(size) * TOF_ICC_MB_SIZE)
+#define TOF_UTOFU_STAG_MEM_LEN(size) (TOF_UTOFU_STEERING_TABLE_LEN(size) * 4)
+#define TOF_UTOFU_SPECIAL_STAG 4096
+
+#define TOF_UTOFU_ICC_COMMON_REGISTER (tof_icc_reg_pa + 0x0B000000)
+#define TOF_UTOFU_REG_START tof_icc_reg_pa
+#define TOF_UTOFU_REG_END (TOF_UTOFU_ICC_COMMON_REGISTER + 0x000FFFFF)
+
+#define TOF_UTOFU_SET_SUBNET_TNI	0	/* This number is kernel TNIs number in setting subnet */
+#define TOF_UTOFU_KCQ			11
+#define TOF_UTOFU_LINKDOWN_PORT_MASK	0x000003FF
+
+#define TOF_UTOFU_ALLOC_STAG_LPG	0x2
+#define TOF_UTOFU_BLANK_MBVA (-1)
+
+#define TOF_UTOFU_MRU_EMPTY (-1)
+
+struct tof_utofu_trans_list {
+	int16_t prev;
+	int16_t next;
+	uint8_t pgszbits;
+	struct tof_utofu_mbpt *mbpt;
+};
+
+
+/*
+ * Bit 30 marks a kref as McKernel internal.
+ * This can be used to distinguish krefs from Linux and
+ * it also ensures that a non deallocated kref will not
+ * crash the Linux allocator.
+ */
+#define MCKERNEL_KREF_MARK	(1U << 30)
+static inline unsigned int mcctrl_kref_is_mckernel(const struct kref *kref)
+{
+	return (refcount_read(&kref->refcount) & (MCKERNEL_KREF_MARK));
+}
+
+/**
+ * kref_put - decrement refcount for object.
+ * @kref: object.
+ * @release: pointer to the function that will clean up the object when the
+ *	     last reference to the object is released.
+ *	     This pointer is required, and it is not acceptable to pass kfree
+ *	     in as this function.  If the caller does pass kfree to this
+ *	     function, you will be publicly mocked mercilessly by the kref
+ *	     maintainer, and anyone else who happens to notice it.  You have
+ *	     been warned.
+ *
+ * Decrement the refcount, and if 0, call release().
+ * Return 1 if the object was removed, otherwise return 0.  Beware, if this
+ * function returns 0, you still can not count on the kref from remaining in
+ * memory.  Only use the return value if you want to see if the kref is now
+ * gone, not present.
+ */
+static inline int mcctrl_kref_put(struct kref *kref, void (*release)(struct kref *kref))
+{
+	if (atomic_dec_return(&kref->refcount.refs) == MCKERNEL_KREF_MARK) {
+		release(kref);
+		return 1;
+	}
+	return 0;
+}
+
+static int tof_utofu_cq_cacheflush(struct tof_utofu_cq *ucq){
+	return mcctrl_tof_core_cq_cacheflush(ucq->tni, ucq->cqid);
+}
+
+static void tof_utofu_trans_mru_delete(struct tof_utofu_cq *ucq, int stag){
+	struct tof_utofu_trans_list *mru = ucq->trans.mru;
+	int prev = mru[stag].prev;
+	int next = mru[stag].next;
+	if(prev == TOF_UTOFU_MRU_EMPTY || next == TOF_UTOFU_MRU_EMPTY){ /* already deleted */
+		return;
+	}
+	if(prev == stag){  /* a single entry */
+		ucq->trans.mruhead = TOF_UTOFU_MRU_EMPTY;
+	}else{
+		if(ucq->trans.mruhead == stag){
+			ucq->trans.mruhead = next;
+		}
+		mru[prev].next = next;
+		mru[next].prev = prev;
+	}
+	mru[stag].prev = TOF_UTOFU_MRU_EMPTY;
+	mru[stag].next = TOF_UTOFU_MRU_EMPTY;
+}
+
+static void tof_utofu_trans_disable(struct tof_utofu_cq *ucq, int stag){
+	struct tof_trans_table *table = ucq->trans.table;
+	atomic64_set((atomic64_t *)&table[stag], 0);
+	tof_utofu_trans_mru_delete(ucq, stag);
+}
+
+/* McKernel scatterlist is simply a contiguous buffer. */
+struct scatterlist {
+	void *pages;
+	unsigned int	offset;
+	unsigned int	length;
+	unsigned long	dma_address;
+	unsigned int	dma_length;
+};
+
+static uintptr_t tof_utofu_disable_mbpt(struct tof_utofu_mbpt *mbpt, int idx){
+	int i0, i1;
+	struct tof_icc_mbpt_entry *ent;
+	uintptr_t ipa;
+	i0 = idx / (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
+	i1 = idx - i0 * (PAGE_SIZE / TOF_ICC_MBPT_SIZE);
+	//ent = sg_virt(&mbpt->sg[i0]);
+	ent = mbpt->sg->pages + (i0 * PAGE_SIZE);
+	if(!ent[i1].enable){
+		return 0;
+	}
+	ent[i1].enable = 0;
+	ipa = (uint64_t)ent[i1].ipa << 12;
+	ent[i1].ipa = 0;
+	return ipa;
+}
+
+static void tof_utofu_free_mbpt(struct tof_utofu_cq *ucq, struct tof_utofu_mbpt *mbpt){
+	int i;
+
+	for(i = 0; i < mbpt->nsgents * PAGE_SIZE / sizeof(struct tof_icc_mbpt_entry); i++){
+		uintptr_t iova;
+		iova = tof_utofu_disable_mbpt(mbpt, i);
+#if 0
+		/*
+		 * NOTE: Not performed for McKernel managed stags.
+		 */
+		if(iova){
+			tof_smmu_release_ipa_cq(ucq->tni, ucq->cqid, iova, mbpt->pgsz);
+		}
+#endif
+	}
+
+#if 0
+	/*
+	 * NOTE: Everyhing below has been allocated in McKernel, do nothing here!!
+	 * This leaks memory in McKernel, but it doesn't crash Linux.
+	 * Memory will be released once McKernel is unbooted.
+	 */
+	tof_smmu_iova_unmap_sg(ucq->tni, ucq->cqid, mbpt->sg, mbpt->nsgents);
+
+	for(i = 0; i < mbpt->nsgents; i++){
+		tof_util_free_pages((unsigned long)sg_virt(&mbpt->sg[i]), 0);
+	}
+	tof_util_free(mbpt->sg);
+	tof_util_free(mbpt);
+#endif
+}
+
+static void tof_utofu_mbpt_release(struct kref *kref)
+{
+	struct tof_utofu_mbpt *mbpt = container_of(kref, struct tof_utofu_mbpt, kref);
+	//atomic64_inc((atomic64_t *)&kref_free_count);
+	tof_utofu_free_mbpt(mbpt->ucq, mbpt);
+}
+
+static int tof_utofu_free_stag(struct tof_utofu_cq *ucq, int stag){
+	if(stag < 0 || stag >= TOF_UTOFU_NUM_STAG(ucq->num_stag) ||
+	   ucq->steering == NULL){
+		return -EINVAL;
+	}
+	if(!(ucq->steering[stag].enable)){
+		return -ENOENT;
+	}
+	if (!mcctrl_kref_is_mckernel(&ucq->trans.mru[stag].mbpt->kref)) {
+		printk("%s: stag: %d is not an McKernel kref\n", __func__, stag);
+		return -EINVAL;
+	}
+	ucq->steering[stag].enable = 0;
+	ucq->mb[stag].enable = 0;
+	tof_utofu_trans_disable(ucq, stag);
+	dma_wmb();
+	tof_utofu_cq_cacheflush(ucq);
+	mcctrl_kref_put(&ucq->trans.mru[stag].mbpt->kref, tof_utofu_mbpt_release);
+	ucq->trans.mru[stag].mbpt = NULL;
+	dprintk("%s: TNI: %d, CQ: %d: stag %d deallocated\n",
+			__func__, ucq->tni, ucq->cqid, stag);
+	return 0;
+}
+
+void mcctrl_mckernel_tof_utofu_release_cq(void *pde_data)
+{
+	struct tof_utofu_cq *ucq;
+	struct tof_utofu_device *dev;
+	unsigned long irqflags;
+	int stag;
+
+	dev = (struct tof_utofu_device *)pde_data;
+	ucq = container_of(dev, struct tof_utofu_cq, common);
+
+	if (!ucq->common.enabled) {
+		return;
+	}
+
+	dprintk("%s: UCQ (PDE: 0x%lx) TNI %d CQ %d\n",
+		__func__, (unsigned long)pde_data, ucq->tni, ucq->cqid);
+
+	/*
+	 * Only release stags here, actual cleanup is still performed
+	 * in the Tofu driver
+	 */
+	for (stag = 0; stag < TOF_UTOFU_NUM_STAG(ucq->num_stag); stag++) {
+		spin_lock_irqsave(&ucq->trans.mru_lock, irqflags);
+		tof_utofu_free_stag(ucq, stag);
+		spin_unlock_irqrestore(&ucq->trans.mru_lock, irqflags);
+	}
+}
+
+static inline void tof_core_unregister_signal_bg(int tni, int bgid)
+{
+	return mcctrl_tof_core_register_signal_bg(tni, bgid, NULL);
+}
+
+static struct tof_utofu_bg *tof_utofu_bg_get(int tni, int bgid){
+	if((unsigned int)tni >= TOF_ICC_NTNIS ||
+	   (unsigned int)bgid >= TOF_ICC_NBGS){
+		return NULL;
+	}
+	//return &tof_utofu_bg[tni][bgid];
+
+	// Convert [][] notion into pointer aritmethic
+	return mcctrl_tof_utofu_bg + (tni * TOF_ICC_NBGS) + bgid;
+}
+
+static int __tof_utofu_unset_bg(struct tof_utofu_bg *ubg){
+	if(ubg->common.enabled){
+		mcctrl_tof_core_unset_bg(ubg->tni, ubg->bgid);
+		ubg->common.enabled = false;
+		tof_core_unregister_signal_bg(ubg->tni, ubg->bgid);
+	}
+	return 0;
+}
+
+static int mcctrl_tof_utofu_disable_bch(struct tof_utofu_bg *ubg){
+	int ret;
+	int tni, bgid;
+
+	if(!ubg->bch.enabled){
+		return -EPERM;
+	}
+
+	ret = mcctrl_tof_core_disable_bch(ubg->tni, ubg->bgid);
+	if(ret < 0){
+		return ret;
+	}
+
+	for(tni = 0; tni < TOF_ICC_NTNIS; tni++){
+		uint64_t mask = ubg->bch.bgmask[tni];
+		for(bgid = 0; bgid < TOF_ICC_NBGS; bgid++){
+			if((mask >> bgid) & 1){
+				ret = __tof_utofu_unset_bg(tof_utofu_bg_get(tni, bgid));
+				if(ret < 0){
+					/* OK? */
+					//BUG();
+					return ret;
+				}
+			}
+		}
+	}
+
+	/* Not performed in McKernel handler */
+	//tof_smmu_release_ipa_bg(ubg->tni, ubg->bgid, ubg->bch.iova, TOF_ICC_BCH_DMA_ALIGN);
+	//put_page(ubg->bch.page);
+	ubg->bch.enabled = false;
+	smp_mb();
+	dprintk("%s: tni=%d bgid=%d\n", __func__, ubg->tni, ubg->bgid);
+	return 0;
+}
+
+void mcctrl_mckernel_tof_utofu_release_bch(void *pde_data)
+{
+	struct tof_utofu_bg *ubg;
+	struct tof_utofu_device *dev = (struct tof_utofu_device *)pde_data;
+
+	ubg = container_of(dev, struct tof_utofu_bg, common);
+	//tof_log_if("tni=%d bgid=%d\n", ubg->tni, ubg->bgid);
+	dprintk("%s: tni=%d bgid=%d\n", __func__, ubg->tni, ubg->bgid);
+	mcctrl_tof_utofu_disable_bch(ubg);
+}
+
+void mcctrl_tofu_cleanup_file(struct mcctrl_file_to_pidfd *f2pfd)
+{
+	/* Figure out whether CQ or BCH */
+	if (strstr(f2pfd->tofu_dev_path, "cq")) {
+		dprintk("%s: PID: %d, fd: %d (%s) -> release CQ\n",
+			__func__, f2pfd->pid, f2pfd->fd, f2pfd->tofu_dev_path);
+		mcctrl_mckernel_tof_utofu_release_cq(f2pfd->pde_data);
+	}
+
+	else if (strstr(f2pfd->tofu_dev_path, "bch")) {
+		dprintk("%s: PID: %d, fd: %d (%s) -> release BCH\n",
+			__func__, f2pfd->pid, f2pfd->fd, f2pfd->tofu_dev_path);
+		mcctrl_mckernel_tof_utofu_release_bch(f2pfd->pde_data);
+	}
+}
+
+
 int __mcctrl_tof_utofu_release_handler(struct inode *inode, struct file *filp,
 		int (*__release_func)(struct inode *inode, struct file *filp))
 {
@@ -519,28 +918,22 @@ int __mcctrl_tof_utofu_release_handler(struct inode *inode, struct file *filp,
 	ret = mcctrl_ikc_send_wait(f2pfd->os, ppd->ikc_target_cpu,
 			&isp, -20, NULL, NULL, 0);
 	if (ret != 0) {
-		dprintk("%s: WARNING: failed to send IKC msg: %d\n",
+		pr_err("%s: WARNING: IKC req for PID: %d, fd: %d failed\n",
-				__func__, ret);
+				__func__, f2pfd->pid, f2pfd->fd);
 	}
 
+	/* Disable any remaining STAGs/BCH in mcctrl anyway */
+	mcctrl_tofu_cleanup_file(f2pfd);
+
 	mcctrl_file_to_pidfd_hash_remove(filp, f2pfd->os,
 			current->group_leader, f2pfd->fd);
 
 	mcctrl_put_per_proc_data(ppd);
 
-	/* Do not call into Linux driver if timed out in SIGKILL.. */
-	if (ret == -ETIME && __fatal_signal_pending(current)) {
-		pr_err("%s: WARNING: failed to send IKC msg in SIGKILL: %d\n",
-				__func__, ret);
-		goto out_no_release;
-	}
 out:
 	dprintk("%s: current PID: %d, comm: %s -> calling release\n",
 			__func__, task_tgid_vnr(current), current->comm);
 	return __release_func(inode, filp);
-
-out_no_release:
-	return ret;
 }
 
 int __mcctrl_tof_utofu_release_cq(struct inode *inode, struct file *filp)
@@ -554,4 +947,70 @@ int __mcctrl_tof_utofu_release_bch(struct inode *inode, struct file *filp)
 	return __mcctrl_tof_utofu_release_handler(inode, filp,
 			mcctrl_tof_utofu_release_bch);
 }
+
+/*
+ * Tofu MMU notifier functions
+ */
+void __mcctrl_tof_utofu_mn_invalidate_range_end(
+	struct mmu_notifier *mn,
+	struct mm_struct *mm,
+	unsigned long start,
+	unsigned long end)
+{
+	char tmpname[TASK_COMM_LEN];
+
+	/* Not an offloaded syscall? */
+	if (current->mm != mm) {
+		goto out_call_real;
+	}
+
+	/* Not mcexec? Just in case.. */
+	get_task_comm(tmpname, current);
+	if (strncmp(tmpname, "mcexec", TASK_COMM_LEN)) {
+		goto out_call_real;
+	}
+
+	/* This is only called for Tofu enabled mcexec processes */
+	dprintk("%s: skipping tof_utofu_mn_invalidate_range_end() "
+			"for mcexec PID %d\n",
+			__func__, task_tgid_vnr(current));
+	return;
+
+out_call_real:
+	return mcctrl_tof_utofu_mn_invalidate_range_end(mn, mm, start, end);
+}
+
+int __mcctrl_tof_utofu_ioctl_init_cq(struct tof_utofu_device *dev,
+		unsigned long arg) {
+	struct tof_utofu_cq *ucq;
+
+	ucq = container_of(dev, struct tof_utofu_cq, common);
+	if (!ucq->common.enabled) {
+		return -EINVAL;
+	}
+
+	dprintk("%s: Tofu TNI %d CQ %d (PDE: 0x%lx) MMU notifier to be hijacked\n",
+		__func__, ucq->tni, ucq->cqid, (unsigned long)dev);
+	/* Override the MMU notifier */
+	ucq->mn.ops = &__mcctrl_tof_utofu_mn_ops;
+
+	return 0;
+}
+
+long __mcctrl_tof_utofu_unlocked_ioctl_cq(void *pde_data, unsigned int cmd,
+					unsigned long arg) {
+	struct tof_utofu_device *dev = (struct tof_utofu_device *)pde_data;
+	int ret;
+
+	switch (cmd) {
+		/* We only care about init, where we hijack the MMU notifier */
+		case TOF_IOCTL_INIT_CQ:
+			ret = __mcctrl_tof_utofu_ioctl_init_cq(dev, arg);
+			break;
+		default:
+			ret = 0;
+	}
+
+	return ret;
+}
 #endif

executer/kernel/mcctrl/arch/arm64/include/tofu/generate_headers.sh

@@ -0,0 +1,41 @@
+#!/bin/bash
+
+SCRIPT="`readlink -f ${BASH_SOURCE[0]:-}`"
+SCRIPT_DIR=$(dirname ${SCRIPT})
+CURRENT_DIR=`pwd`
+
+cd ${SCRIPT_DIR}
+
+DWARF_TOOL=${SCRIPT_DIR}/../../../../../../../tools/dwarf-extract-struct/dwarf-extract-struct
+if [ ! -x ${DWARF_TOOL} ]; then
+	echo "error: couldn't find DWARF extractor executable (${DWARF_TOOL}), have you compiled it?"
+	cd -
+	exit 1
+fi
+
+echo "Looking for Tofu driver debug symbols..."
+if [ "`find /lib/modules/ -name "tof_module.tar.gz" | xargs -r ls -t | head -n 1 | wc -l`" == "0" ]; then
+	echo "error: couldn't find Tofu modules with debug symbols"
+	cd -
+	exit 1
+fi
+
+MODULE_TAR_GZ=`find /lib/modules/ -name "tof_module.tar.gz" | xargs ls -t | head -n 1`
+echo "Using Tofu driver debug symbols: ${MODULE_TAR_GZ}"
+
+KMODULE=tof_utofu.ko
+if ! tar zxvf ${MODULE_TAR_GZ} ${KMODULE} 2>&1 > /dev/null; then
+	echo "error: uncompressing kernel module with debug symbols"
+	cd -
+	exit 1
+fi
+
+${DWARF_TOOL} ${KMODULE} tof_utofu_device enabled subnet gpid > tofu_generated-tof_utofu_device.h
+${DWARF_TOOL} ${KMODULE} tof_utofu_cq common tni cqid mn trans steering mb num_stag | sed "s/struct FILL_IN_MANUALLY trans;/#include \"tof_utofu_cq_trans.h\"/g" > tofu_generated-tof_utofu_cq.h
+${DWARF_TOOL} ${KMODULE} tof_utofu_mbpt ucq iova sg nsgents mbptstart pgsz kref > tofu_generated-tof_utofu_mbpt.h
+${DWARF_TOOL} ${KMODULE} tof_utofu_bg common tni bgid bch | sed "s/struct FILL_IN_MANUALLY bch;/#include \"tof_utofu_bg_bch.h\"/g" > tofu_generated-tof_utofu_bg.h
+rm ${KMODULE}
+
+
+#cat tofu_generated*.h
+cd - > /dev/null

executer/kernel/mcctrl/arch/arm64/include/tofu/tof_icc.h

@@ -0,0 +1,831 @@
+#ifndef _TOF_ICC_H_
+#define _TOF_ICC_H_
+
+#include <linux/types.h>
+#ifdef __KERNEL__
+#include <linux/bitops.h>
+#else
+#include <stdint.h>
+typedef uint64_t phys_addr_t;
+#endif
+
+/* constants related to the Tofu Interconnect D */
+
+#define TOF_ICC_NTNIS 6
+#define TOF_ICC_NCQS 12
+#define TOF_ICC_NBGS 48
+#define TOF_ICC_NBCHS 16
+#define TOF_ICC_NPORTS 10
+#define TOF_ICC_NVMSIDS 16
+
+#define TOF_ICC_RH_LEN 8
+#define TOF_ICC_ECRC_LEN 4
+#define TOF_ICC_FRAME_ALIGN 32
+#define TOF_ICC_TLP_LEN(len) (((len) + 1) * TOF_ICC_FRAME_ALIGN)
+#define TOF_ICC_TLP_PAYLOAD_MAX (TOF_ICC_TLP_LEN(61) - TOF_ICC_ECRC_LEN)
+#define TOF_ICC_FRAME_LEN(len) (TOF_ICC_RH_LEN + TOF_ICC_TLP_LEN(len))
+#define TOF_ICC_FRAME_LEN_MIN TOF_ICC_FRAME_LEN(2)
+#define TOF_ICC_FRAME_LEN_MAX TOF_ICC_FRAME_LEN(61)
+#define TOF_ICC_FRAME_BUF_SIZE_BITS 11
+#define TOF_ICC_FRAME_BUF_SIZE (1 << TOF_ICC_FRAME_BUF_SIZE_BITS)
+#define TOF_ICC_FRAME_BUF_ALIGN_BITS 8
+#define TOF_ICC_FRAME_BUF_ALIGN (1 << TOF_ICC_FRAME_BUF_ALIGN_BITS)
+#define TOF_ICC_PB_SIZE_BITS 11
+#define TOF_ICC_PB_SIZE (1 << TOF_ICC_PB_SIZE_BITS)
+#define TOF_ICC_PB_ALIGN_BITS 11
+#define TOF_ICC_PB_ALIGN (1 << TOF_ICC_PB_ALIGN_BITS)
+
+#define TOF_ICC_ST_ALIGN_BITS 8
+#define TOF_ICC_ST_ALIGN (1 << TOF_ICC_ST_ALIGN_BITS)
+
+#define TOF_ICC_MBT_ALIGN_BITS 8
+#define TOF_ICC_MBT_ALIGN (1 << TOF_ICC_MBT_ALIGN_BITS)
+
+#define TOF_ICC_MBPT_ALIGN_BITS 8
+#define TOF_ICC_MBPT_ALIGN (1 << TOF_ICC_MBPT_ALIGN_BITS)
+
+#define TOF_ICC_BG_BSEQ_SIZE_BITS 24
+#define TOF_ICC_BG_BSEQ_SIZE (1 << TOF_ICC_BG_BSEQ_SIZE_BITS)
+
+#define TOF_ICC_BCH_DMA_ALIGN_BITS 8
+#define TOF_ICC_BCH_DMA_ALIGN (1 << TOF_ICC_BCH_DMA_ALIGN_BITS)
+
+/* this is a CPU-specific constant, but referred in the ICC spec. */
+#define TOF_ICC_CACHE_LINE_SIZE_BITS 8
+#define TOF_ICC_CACHE_LINE_SIZE (1 << TOF_ICC_CACHE_LINE_SIZE_BITS)
+
+#define TOF_ICC_TOQ_DESC_SIZE_BITS 5
+#define TOF_ICC_TOQ_DESC_SIZE (1 << TOF_ICC_TOQ_DESC_SIZE_BITS)
+#define TOF_ICC_TCQ_DESC_SIZE_BITS 3
+#define TOF_ICC_TCQ_DESC_SIZE (1 << TOF_ICC_TCQ_DESC_SIZE_BITS)
+#define TOF_ICC_TCQ_NLINE_BITS (TOF_ICC_CACHE_LINE_SIZE_BITS - TOF_ICC_TCQ_DESC_SIZE_BITS)
+#define TOF_ICC_MRQ_DESC_SIZE_BITS 5
+#define TOF_ICC_MRQ_DESC_SIZE (1 << TOF_ICC_MRQ_DESC_SIZE_BITS)
+#define TOF_ICC_PBQ_DESC_SIZE_BITS 3
+#define TOF_ICC_PBQ_DESC_SIZE (1 << TOF_ICC_PBQ_DESC_SIZE_BITS)
+#define TOF_ICC_PRQ_DESC_SIZE_BITS 3
+#define TOF_ICC_PRQ_DESC_SIZE (1 << TOF_ICC_PRQ_DESC_SIZE_BITS)
+#define TOF_ICC_PRQ_NLINE_BITS (TOF_ICC_CACHE_LINE_SIZE_BITS - TOF_ICC_PBQ_DESC_SIZE_BITS)
+
+#define TOF_ICC_TOQ_SIZE_NTYPES 6
+#define TOF_ICC_TOQ_SIZE_BITS(size) ((size) * 2 + 11)
+#define TOF_ICC_TOQ_SIZE(size) (1 << TOF_ICC_TOQ_SIZE_BITS(size))
+#define TOF_ICC_TOQ_LEN(size) (TOF_ICC_TOQ_SIZE(size) * TOF_ICC_TOQ_DESC_SIZE)
+#define TOF_ICC_TCQ_LEN(size) (TOF_ICC_TOQ_SIZE(size) * TOF_ICC_TCQ_DESC_SIZE)
+
+#define TOF_ICC_MRQ_SIZE_NTYPES 6
+#define TOF_ICC_MRQ_SIZE_BITS(size) ((size) * 2 + 11)
+#define TOF_ICC_MRQ_SIZE(size) (1 << TOF_ICC_MRQ_SIZE_BITS(size))
+#define TOF_ICC_MRQ_LEN(size) (TOF_ICC_MRQ_SIZE(size) * TOF_ICC_MRQ_DESC_SIZE)
+
+#define TOF_ICC_PBQ_SIZE_NTYPES 6
+#define TOF_ICC_PBQ_SIZE_BITS(size) ((size) * 2 + 11)
+#define TOF_ICC_PBQ_SIZE(size) (1 << TOF_ICC_PBQ_SIZE_BITS(size))
+#define TOF_ICC_PBQ_LEN(size) (TOF_ICC_PBQ_SIZE(size) * TOF_ICC_PBQ_DESC_SIZE)
+
+#define TOF_ICC_PRQ_SIZE_NTYPES 6
+#define TOF_ICC_PRQ_SIZE_BITS(size) ((size) * 2 + 11)
+#define TOF_ICC_PRQ_SIZE(size) (1 << TOF_ICC_PRQ_SIZE_BITS(size))
+#define TOF_ICC_PRQ_LEN(size) (TOF_ICC_PRQ_SIZE(size) * TOF_ICC_PRQ_DESC_SIZE)
+
+#define TOF_ICC_STEERING_TABLE_ALIGN_BITS 8
+#define TOF_ICC_STEERING_TABLE_ALIGN (1 << TOF_ICC_STEERING_TABLE_ALIGN_BITS)
+#define TOF_ICC_STEERING_SIZE_BITS 4
+#define TOF_ICC_STEERING_SIZE (1 << TOF_ICC_STEERING_SIZE_BITS)
+
+#define TOF_ICC_MB_TABLE_ALIGN_BITS 8
+#define TOF_ICC_MB_TABLE_ALIGN (1 << TOF_ICC_MB_TABLE_ALIGN_BITS)
+#define TOF_ICC_MB_SIZE_BITS 4
+#define TOF_ICC_MB_SIZE (1 << TOF_ICC_MB_SIZE_BITS)
+#define TOF_ICC_MB_PS_ENCODE(bits) ((bits) % 9 == 3 ? (bits) / 9 - 1 : (bits) / 13 + 3)
+
+#define TOF_ICC_MBPT_ALIGN_BITS 8
+#define TOF_ICC_MBPT_ALIGN (1 << TOF_ICC_MBPT_ALIGN_BITS)
+#define TOF_ICC_MBPT_SIZE_BITS 3
+#define TOF_ICC_MBPT_SIZE (1 << TOF_ICC_MBPT_SIZE_BITS)
+
+#define TOF_ICC_X_BITS 5
+#define TOF_ICC_Y_BITS 5
+#define TOF_ICC_Z_BITS 5
+#define TOF_ICC_A_BITS 1
+#define TOF_ICC_B_BITS 2
+#define TOF_ICC_C_BITS 1
+#define TOF_ICC_MAX_X_SIZE (1 << TOF_ICC_X_BITS)
+#define TOF_ICC_MAX_Y_SIZE (1 << TOF_ICC_Y_BITS)
+#define TOF_ICC_MAX_Z_SIZE (1 << TOF_ICC_Z_BITS)
+#define TOF_ICC_A_SIZE 2
+#define TOF_ICC_B_SIZE 3
+#define TOF_ICC_C_SIZE 2
+#define TOF_ICC_X_MASK ((1 << TOF_ICC_X_BITS) - 1)
+#define TOF_ICC_Y_MASK ((1 << TOF_ICC_Y_BITS) - 1)
+#define TOF_ICC_Z_MASK ((1 << TOF_ICC_Z_BITS) - 1)
+#define TOF_ICC_A_MASK ((1 << TOF_ICC_A_BITS) - 1)
+#define TOF_ICC_B_MASK ((1 << TOF_ICC_B_BITS) - 1)
+#define TOF_ICC_C_MASK ((1 << TOF_ICC_C_BITS) - 1)
+#define TOF_ICC_ABC_SIZE (TOF_ICC_A_SIZE * TOF_ICC_B_SIZE * TOF_ICC_C_SIZE)
+
+static inline int tof_icc_get_framelen(int len){
+	len = TOF_ICC_RH_LEN + round_up(len + TOF_ICC_ECRC_LEN, TOF_ICC_FRAME_ALIGN);
+	if(len < TOF_ICC_FRAME_LEN_MIN){
+		len = TOF_ICC_FRAME_LEN_MIN;
+	}
+	return len;
+}
+
+/** Descriptors **/
+/** commands and rcodes **/
+enum {
+	TOF_ICC_TOQ_NOP,
+	TOF_ICC_TOQ_PUT,
+	TOF_ICC_TOQ_WRITE_PIGGYBACK_BUFFER,
+	TOF_ICC_TOQ_PUT_PIGGYBACK,
+	TOF_ICC_TOQ_GET,
+	TOF_ICC_TOQ_GETL,
+	TOF_ICC_TOQ_ATOMIC_READ_MODIFY_WRITE = 0xe,
+	TOF_ICC_TOQ_TRANSMIT_RAW_PACKET1 = 0x10,
+	TOF_ICC_TOQ_TRANSMIT_RAW_PACKET2,
+	TOF_ICC_TOQ_TRANSMIT_SYSTEM_PACKET1,
+	TOF_ICC_TOQ_TRANSMIT_SYSTEM_PACKET2,
+
+	TOF_ICC_TOQ_NCOMMANDS,
+};
+
+enum {
+	TOF_ICC_MRQ_ATOMIC_READ_MODIFY_WRITE_HALFWAY_NOTICE = 0x1,
+	TOF_ICC_MRQ_ATOMIC_READ_MODIFY_WRITE_NOTICE,
+	TOF_ICC_MRQ_ATOMIC_READ_MODIFY_WRITE_REMOTE_ERROR,
+	TOF_ICC_MRQ_PUT_HALFWAY_NOTICE,
+	TOF_ICC_MRQ_PUT_LAST_HALFWAY_NOTICE,
+	TOF_ICC_MRQ_GET_HALFWAY_NOTICE,
+	TOF_ICC_MRQ_GET_LAST_HALFWAY_NOTICE,
+	TOF_ICC_MRQ_PUT_NOTICE,
+	TOF_ICC_MRQ_PUT_LAST_NOTICE,
+	TOF_ICC_MRQ_GET_NOTICE,
+	TOF_ICC_MRQ_GET_LAST_NOTICE,
+	TOF_ICC_MRQ_PUT_REMOTE_ERROR,
+	TOF_ICC_MRQ_PUT_LAST_REMOTE_ERROR,
+	TOF_ICC_MRQ_GET_REMOTE_ERROR,
+	TOF_ICC_MRQ_GET_LAST_REMOTE_ERROR,
+
+	TOF_ICC_MRQ_NCOMMANDS,
+};
+
+enum {
+	TOF_ICC_PRQ_UNKNOWN_TLP,
+	TOF_ICC_PRQ_SYSTEM_TLP,
+	TOF_ICC_PRQ_ADDRESS_RANGE_EXCEPTION = 0x6,
+	TOF_ICC_PRQ_CQ_EXCEPTION = 0x8,
+	TOF_ICC_PRQ_ILLEGAL_TLP_FLAGS,
+	TOF_ICC_PRQ_ILLEGAL_TLP_LENGTH,
+	TOF_ICC_PRQ_CQ_ERROR = 0xc,
+};
+
+/** structures **/
+struct tof_icc_steering_entry {
+	uint64_t res1:6;
+	uint64_t readonly:1;
+	uint64_t enable:1;
+	uint64_t mbva:32;
+	uint64_t res2:8;
+	uint64_t mbid:16;
+	uint64_t length;  /* for optimization */
+};
+
+struct tof_icc_mb_entry {
+	uint64_t ps:3;
+	uint64_t res1:4;
+	uint64_t enable:1;
+	uint64_t ipa:32;
+	uint64_t res2:24;
+	uint64_t npage;  /* for optimization */
+};
+
+struct tof_icc_mbpt_entry {
+	uint64_t res1:7;
+	uint64_t enable:1;
+	uint64_t res2:4;
+	uint64_t ipa:28;
+	uint64_t res3:24;
+};
+
+struct tof_icc_cq_stag_offset {
+	uint64_t offset:40;
+	uint64_t stag:18;
+	uint64_t cqid:6;
+};
+
+struct tof_icc_toq_common_header1 {
+	uint8_t interrupt:1;
+	uint8_t res1:4;
+	uint8_t source_type:2;
+	uint8_t flip:1;
+	uint8_t command;
+	union {
+		uint8_t mtu;
+		struct {
+			uint8_t res:4;
+			uint8_t op:4;
+		} armw;
+	} mtuop;
+	uint8_t sps:4;
+	uint8_t pa:1;
+	uint8_t pb:2;
+	uint8_t pc:1;
+	uint8_t rx;
+	uint8_t ry;
+	uint8_t rz;
+	uint8_t ra:1;
+	uint8_t rb:2;
+	uint8_t rc:1;
+	uint8_t res3:1;
+	uint8_t ri:3;
+};
+
+struct tof_icc_toq_common_header2 {
+	uint8_t gap;
+	uint8_t s:1;
+	uint8_t r:1;
+	uint8_t q:1;
+	uint8_t p:1;
+	uint8_t res1:1;
+	uint8_t j:1;
+	uint8_t res2:2;
+	uint16_t edata;
+	union{
+		struct {
+			uint32_t length:24;
+			uint32_t res:8;
+		} normal;
+		struct {
+			uint32_t length:6;
+			uint32_t res:26;
+		} piggyback;
+	} len;
+};
+
+struct tof_icc_toq_descriptor {
+	struct tof_icc_toq_common_header1 head1;
+	uint64_t res[3];
+};
+
+struct tof_icc_toq_nop {
+	struct tof_icc_toq_common_header1 head1;
+	uint64_t res[3];
+};
+
+struct tof_icc_toq_put {
+	struct tof_icc_toq_common_header1 head1;
+	struct tof_icc_toq_common_header2 head2;
+	struct tof_icc_cq_stag_offset remote;
+	struct tof_icc_cq_stag_offset local;
+};
+
+struct tof_icc_toq_write_piggyback_buffer {
+	struct tof_icc_toq_common_header1 head1;
+	uint64_t data[3];
+};
+
+struct tof_icc_toq_put_piggyback {
+	struct tof_icc_toq_common_header1 head1;
+	struct tof_icc_toq_common_header2 head2;
+	struct tof_icc_cq_stag_offset remote;
+	uint64_t data;
+};
+
+struct tof_icc_toq_get {
+	struct tof_icc_toq_common_header1 head1;
+	struct tof_icc_toq_common_header2 head2;
+	struct tof_icc_cq_stag_offset remote;
+	struct tof_icc_cq_stag_offset local;
+};
+
+struct tof_icc_toq_atomic_read_modify_write {
+	struct tof_icc_toq_common_header1 head1;
+	struct tof_icc_toq_common_header2 head2;
+	struct tof_icc_cq_stag_offset remote;
+	uint64_t data;
+};
+
+struct tof_icc_toq_transmit_raw_packet1 {
+	struct tof_icc_toq_common_header1 head1;
+	uint8_t gap;
+	uint8_t res4[3];
+	uint32_t length:12;
+	uint32_t res5:20;
+	uint64_t res6;
+	uint64_t pa:48;  /* for optimization */
+	uint64_t res7:16;
+};
+
+struct tof_icc_toq_transmit_raw_packet2 {
+	uint8_t interrupt:1;
+	uint8_t res1:4;
+	uint8_t source_type:2;
+	uint8_t flip:1;
+	uint8_t command;
+	uint8_t res2:7;
+	uint8_t e:1;
+	uint8_t res3[4];
+	uint8_t port:5;
+	uint8_t res4:1;
+	uint8_t vc:2;
+	uint8_t gap;
+	uint8_t res5[3];
+	uint32_t length:12;
+	uint32_t res6:20;
+	uint64_t res7;
+	uint64_t pa:48;  /* for optimization */
+	uint64_t res8:16;
+};
+
+struct tof_icc_toq_transmit_system_packet {
+	struct tof_icc_toq_common_header1 head1;  /* rx, ry, rz should be rdx, rdy, rdz */
+	uint8_t gap;
+	uint8_t res4[3];
+	uint32_t length:12;
+	uint32_t res5:20;
+	uint64_t res6;
+	uint64_t pa:48;  /* for optimization */
+	uint64_t res7:16;
+};
+
+struct tof_icc_tcq_descriptor {
+	uint8_t res1:5;
+	uint8_t counter_unmatch:1;
+	uint8_t res2:1;
+	uint8_t flip:1;
+	uint8_t rcode;
+	uint8_t res3[2];
+	union{
+		struct {
+			uint32_t length:24;
+			uint32_t res:8;
+		} normal;
+		struct {
+			uint32_t length:6;
+			uint32_t res:26;
+		} piggyback;
+	} len;
+};
+
+struct tof_icc_mrq_common_header1 {
+	uint8_t res1:7;
+	uint8_t flip:1;
+	uint8_t id;
+	uint8_t rcode;
+	uint8_t res2:4;
+	uint8_t pa:1;
+	uint8_t pb:2;
+	uint8_t pc:1;
+	uint8_t x;
+	uint8_t y;
+	uint8_t z;
+	uint8_t a:1;
+	uint8_t b:2;
+	uint8_t c:1;
+	uint8_t res3:1;
+	uint8_t i:3;
+};
+
+struct tof_icc_mrq_common_header2 {
+	uint8_t res1;
+	uint8_t res2:4;
+	uint8_t initial:1;
+	uint8_t res3:3;
+	uint16_t edata;
+	union {
+		struct {
+			uint32_t length:11;
+			uint32_t res:21;
+		} normal;
+		struct {
+			uint32_t op:4;
+			uint32_t res:28;
+		} armw;
+	} lenop;
+};
+
+struct tof_icc_mrq_atomic_read_modify_write_halfway_notice {
+	struct tof_icc_mrq_common_header1 head1;
+	struct tof_icc_mrq_common_header2 head2;
+	struct tof_icc_cq_stag_offset local;
+	struct tof_icc_cq_stag_offset remote;
+};
+
+struct tof_icc_mrq_descriptor {
+	struct tof_icc_mrq_common_header1 head1;
+	struct tof_icc_mrq_common_header2 head2;
+	struct tof_icc_cq_stag_offset cso1;
+	struct tof_icc_cq_stag_offset cso2;
+};
+
+struct tof_icc_pbq_descriptor {
+	uint64_t res1:7;
+	uint64_t f:1;
+	uint64_t res2:3;
+	uint64_t pa:29;
+	uint64_t res3:24;
+};
+
+struct tof_icc_prq_descriptor {
+	uint64_t rcode:7;
+	uint64_t f:1;
+	uint64_t res1:3;
+	uint64_t pa:29;
+	uint64_t res2:8;
+	uint64_t w:1;
+	uint64_t res3:5;
+	uint64_t l:1;
+	uint64_t e:1;
+	uint64_t res4:8;
+};
+
+
+/** Registers **/
+/* useful packed structures */
+struct tof_icc_reg_subnet {
+	uint64_t lz:6;
+	uint64_t sz:6;
+	uint64_t nz:6;
+	uint64_t ly:6;
+	uint64_t sy:6;
+	uint64_t ny:6;
+	uint64_t lx:6;
+	uint64_t sx:6;
+	uint64_t nx:6;
+	uint64_t res:10;
+};
+
+struct tof_icc_reg_bg_address {
+	uint32_t bgid:6;
+	uint32_t tni:3;
+	uint32_t c:1;
+	uint32_t b:2;
+	uint32_t a:1;
+	uint32_t z:5;
+	uint32_t y:5;
+	uint32_t x:5;
+	uint32_t pc:1;
+	uint32_t pb:2;
+	uint32_t pa:1;
+};
+
+/* relative offset of interrupt controller registers */
+#define TOF_ICC_IRQREG_IRR 0x0
+#define TOF_ICC_IRQREG_IMR 0x8
+#define TOF_ICC_IRQREG_IRC 0x10
+#define TOF_ICC_IRQREG_IMC 0x18
+#define TOF_ICC_IRQREG_ICL 0x20
+
+/* TOFU REGISTERS */
+#define  tof_icc_reg_pa 0x40000000
+
+/* CQ */
+#define TOF_ICC_REG_CQ_PA(tni, cqid) (tof_icc_reg_pa + 0 + (tni) * 0x1000000 + (cqid) * 0x10000)
+#define TOF_ICC_REG_CQ_TOQ_DIRECT_DESCRIPTOR 0x0
+#define TOF_ICC_REG_CQ_TOQ_FETCH_START 0x40
+#define TOF_ICC_REG_CQ_MRQ_FULL_POINTER 0x48
+#define TOF_ICC_REG_CQ_TOQ_PIGGYBACK_BUFFER0 0x50
+#define TOF_ICC_REG_CQ_TOQ_PIGGYBACK_BUFFER1 0x58
+#define TOF_ICC_REG_CQ_TOQ_PIGGYBACK_BUFFER2 0x60
+#define TOF_ICC_REG_CQ_TCQ_NUM_NOTICE 0x68
+#define TOF_ICC_REG_CQ_MRQ_NUM_NOTICE 0x70
+#define TOF_ICC_REG_CQ_TX_PAYLOAD_BYTE 0x78
+#define TOF_ICC_REG_CQ_RX_PAYLOAD_BYTE 0x80
+#define TOF_ICC_REG_CQ_DUMP_START 0x0
+#define TOF_ICC_REG_CQ_DUMP_END 0x88
+
+/* BCH */
+#define TOF_ICC_REG_BCH_PA(tni, bgid) (tof_icc_reg_pa + 0x0000e00000 + (tni) * 0x1000000 + (bgid) * 0x10000)
+#define TOF_ICC_REG_BCH_IDATA 0x800
+#define TOF_ICC_REG_BCH_READY 0x840
+#define TOF_ICC_REG_BCH_READY_STATE BIT(63)
+#define TOF_ICC_REG_BCH_IGNORED_SIGNAL_COUNT 0x848
+#define TOF_ICC_REG_BCH_DUMP_START 0x800
+#define TOF_ICC_REG_BCH_DUMP_END 0x850
+
+/* CQS */
+#define TOF_ICC_REG_CQS_PA(tni, cqid) (tof_icc_reg_pa + 0x0000400000 + (tni) * 0x1000000 + (cqid) * 0x10000)
+#define TOF_ICC_REG_CQS_STATUS 0x0
+#define TOF_ICC_REG_CQS_STATUS_DESCRIPTOR_PROCESS_STOP BIT(63)
+#define TOF_ICC_REG_CQS_STATUS_DESCRIPTOR_FETCH_STOP BIT(62)
+#define TOF_ICC_REG_CQS_STATUS_BLANK_ENTRY_FLIP_BIT BIT(61)
+#define TOF_ICC_REG_CQS_STATUS_CACHE_FLUSH_BUSY BIT(60)
+#define TOF_ICC_REG_CQS_STATUS_CQ_ENABLE BIT(59)
+#define TOF_ICC_REG_CQS_STATUS_SESSION_DEAD BIT(58)
+#define TOF_ICC_REG_CQS_STATUS_SESSION_OFFSET_OVERFLOW BIT(57)
+#define TOF_ICC_REG_CQS_STATUS_SESSION_OFFSET GENMASK(56, 32)
+#define TOF_ICC_REG_CQS_STATUS_NEXT_DESCRIPTOR_OFFSET GENMASK(29, 5)
+#define TOF_ICC_REG_CQS_ENABLE 0x8
+#define TOF_ICC_REG_CQS_CACHE_FLUSH 0x10
+#define TOF_ICC_REG_CQS_FETCH_STOP 0x18
+#define TOF_ICC_REG_CQS_MODE 0x20
+#define TOF_ICC_REG_CQS_MODE_SYSTEM BIT(63)
+#define TOF_ICC_REG_CQS_MODE_TRP2_ENABLE BIT(62)
+#define TOF_ICC_REG_CQS_MODE_TRP1_ENABLE BIT(61)
+#define TOF_ICC_REG_CQS_MODE_SESSION BIT(60)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_NX GENMASK(53, 48)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_SX GENMASK(47, 42)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_LX GENMASK(41, 36)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_NY GENMASK(35, 30)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_SY GENMASK(29, 24)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_LY GENMASK(23, 18)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_NZ GENMASK(17, 12)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_SZ GENMASK(11, 6)
+#define TOF_ICC_REG_CQS_MODE_SUBNET_LZ GENMASK(5, 0)
+#define TOF_ICC_REG_CQS_GPID 0x28
+#define TOF_ICC_REG_CQS_TOQ_IPA 0x30
+#define TOF_ICC_REG_CQS_TOQ_SIZE 0x38
+#define TOF_ICC_REG_CQS_TCQ_IPA 0x40
+#define TOF_ICC_REG_CQS_TCQ_IPA_CACHE_INJECTION BIT(63)
+#define TOF_ICC_REG_CQS_MRQ_IPA 0x48
+#define TOF_ICC_REG_CQS_MRQ_IPA_CACHE_INJECTION BIT(63)
+#define TOF_ICC_REG_CQS_MRQ_SIZE 0x50
+#define TOF_ICC_REG_CQS_MRQ_MASK 0x58
+#define TOF_ICC_REG_CQS_TCQ_DESCRIPTOR_COALESCING_TIMER 0x60
+#define TOF_ICC_REG_CQS_MRQ_DESCRIPTOR_COALESCING_TIMER 0x68
+#define TOF_ICC_REG_CQS_MRQ_INTERRUPT_COALESCING_TIMER 0x70
+#define TOF_ICC_REG_CQS_MRQ_INTERRUPT_COALESCING_COUNT 0x78
+#define TOF_ICC_REG_CQS_TOQ_DIRECT_SOURCE_COUNT 0x80
+#define TOF_ICC_REG_CQS_TOQ_DIRECT_DESCRIPTOR_COUNT 0x88
+#define TOF_ICC_REG_CQS_MEMORY_BLOCK_TABLE_ENABLE 0x90
+#define TOF_ICC_REG_CQS_MEMORY_BLOCK_TABLE_IPA 0x98
+#define TOF_ICC_REG_CQS_MEMORY_BLOCK_TABLE_SIZE 0xa0
+#define TOF_ICC_REG_CQS_STEERING_TABLE_ENABLE 0xa8
+#define TOF_ICC_REG_CQS_STEERING_TABLE_IPA 0xb0
+#define TOF_ICC_REG_CQS_STEERING_TABLE_SIZE 0xb8
+#define TOF_ICC_REG_CQS_MRQ_INTERRUPT_MASK 0xc0
+#define TOF_ICC_REG_CQS_IRR 0xc8
+#define TOF_ICC_REG_CQS_IMR 0xd0
+#define TOF_ICC_REG_CQS_IRC 0xd8
+#define TOF_ICC_REG_CQS_IMC 0xe0
+#define TOF_ICC_REG_CQS_ICL 0xe8
+#define TOF_ICC_REG_CQS_DUMP_START 0x0
+#define TOF_ICC_REG_CQS_DUMP_END 0xf0
+
+/* BGS */
+#define TOF_ICC_REG_BGS_PA(tni, bgid) (tof_icc_reg_pa + 0x0000800000 + (tni) * 0x1000000 + (bgid) * 0x10000)
+#define TOF_ICC_REG_BGS_ENABLE 0x0
+#define TOF_ICC_REG_BGS_IRR 0x8
+#define TOF_ICC_REG_BGS_IMR 0x10
+#define TOF_ICC_REG_BGS_IRC 0x18
+#define TOF_ICC_REG_BGS_IMC 0x20
+#define TOF_ICC_REG_BGS_ICL 0x28
+#define TOF_ICC_REG_BGS_STATE 0x30
+#define TOF_ICC_REG_BGS_STATE_ENABLE BIT(0)
+#define TOF_ICC_REG_BGS_EXCEPTION_INFO_GPID_UNMATCH 0x38
+#define TOF_ICC_REG_BGS_EXCEPTION_INFO_GPID_UNMATCH_BG_ADDRESS GENMASK(27, 0)
+#define TOF_ICC_REG_BGS_EXCEPTION_INFO_ADDRESS_UNMATCH 0x40
+#define TOF_ICC_REG_BGS_EXCEPTION_INFO_ADDRESS_UNMATCH_BG_ADDRESS GENMASK(27, 0)
+#define TOF_ICC_REG_BGS_SIGNAL_A 0x48
+#define TOF_ICC_REG_BGS_SIGNAL_A_SIG_RECV BIT(63)
+#define TOF_ICC_REG_BGS_SIGNAL_A_TLP_RECV BIT(62)
+#define TOF_ICC_REG_BGS_SIGNAL_A_SIG_SEND BIT(61)
+#define TOF_ICC_REG_BGS_SIGNAL_A_OP_TYPE GENMASK(3, 0)
+#define TOF_ICC_REG_BGS_SIGNAL_B 0x50
+#define TOF_ICC_REG_BGS_SIGNAL_B_SIG_RECV BIT(63)
+#define TOF_ICC_REG_BGS_SIGNAL_B_TLP_RECV BIT(62)
+#define TOF_ICC_REG_BGS_SIGNAL_B_SIG_SEND BIT(61)
+#define TOF_ICC_REG_BGS_SIGNAL_B_OP_TYPE GENMASK(3, 0)
+#define TOF_ICC_REG_BGS_SIGNAL_MASK 0x58
+#define TOF_ICC_REG_BGS_SIGNAL_MASK_SIG_RECV BIT(63)
+#define TOF_ICC_REG_BGS_SIGNAL_MASK_TLP_RECV BIT(62)
+#define TOF_ICC_REG_BGS_SIGNAL_MASK_SIG_SEND BIT(61)
+#define TOF_ICC_REG_BGS_SIGNAL_MASK_TLP_SEND BIT(60)
+#define TOF_ICC_REG_BGS_LOCAL_LINK 0x60
+#define TOF_ICC_REG_BGS_LOCAL_LINK_BGID_RECV GENMASK(37, 32)
+#define TOF_ICC_REG_BGS_LOCAL_LINK_BGID_SEND GENMASK(5, 0)
+#define TOF_ICC_REG_BGS_REMOTE_LINK 0x68
+#define TOF_ICC_REG_BGS_REMOTE_LINK_BG_ADDRESS_RECV GENMASK(59, 32)
+#define TOF_ICC_REG_BGS_REMOTE_LINK_BG_ADDRESS_SEND GENMASK(31, 0)
+#define TOF_ICC_REG_BGS_SUBNET_SIZE 0x70
+#define TOF_ICC_REG_BGS_GPID_BSEQ 0x78
+#define TOF_ICC_REG_BGS_DATA_A0 0x108
+#define TOF_ICC_REG_BGS_DATA_AE 0x178
+#define TOF_ICC_REG_BGS_DATA_B0 0x188
+#define TOF_ICC_REG_BGS_DATA_BE 0x1f8
+#define TOF_ICC_REG_BGS_BCH_MASK 0x800
+#define TOF_ICC_REG_BGS_BCH_MASK_MASK BIT(63)
+#define TOF_ICC_REG_BGS_BCH_MASK_STATUS 0x808
+#define TOF_ICC_REG_BGS_BCH_MASK_STATUS_RUN BIT(63)
+#define TOF_ICC_REG_BGS_BCH_NOTICE_IPA 0x810
+#define TOF_ICC_REG_BGS_DUMP_START 0x0
+#define TOF_ICC_REG_BGS_DUMP_END 0x818
+
+/* TNI */
+#define TOF_ICC_REG_TNI_PA(tni) (tof_icc_reg_pa + 0x0000c00000 + (tni) * 0x1000000)
+#define TOF_ICC_REG_TNI_IRR 0x8
+#define TOF_ICC_REG_TNI_IMR 0x10
+#define TOF_ICC_REG_TNI_IRC 0x18
+#define TOF_ICC_REG_TNI_IMC 0x20
+#define TOF_ICC_REG_TNI_ICL 0x28
+#define TOF_ICC_REG_TNI_STATE 0x30
+#define TOF_ICC_REG_TNI_STATE_MASK GENMASK(1, 0)
+#define TOF_ICC_REG_TNI_STATE_DISABLE 0
+#define TOF_ICC_REG_TNI_STATE_NORMAL 2
+#define TOF_ICC_REG_TNI_STATE_ERROR 3
+#define TOF_ICC_REG_TNI_ENABLE 0x38
+#define TOF_ICC_REG_TNI_CQ_PRESENT 0x40
+#define TOF_ICC_REG_TNI_EXCEPTION_INFO_INACTIVE_BG 0x48
+#define TOF_ICC_REG_TNI_EXCEPTION_INFO_INACTIVE_BG_DEST_BG GENMASK(37, 32)
+#define TOF_ICC_REG_TNI_EXCEPTION_INFO_INACTIVE_BG_SOURCE_BG_ADDRESS GENMASK(27, 0)
+#define TOF_ICC_REG_TNI_PRQ_FULL_POINTER 0x100
+#define TOF_ICC_REG_TNI_PBQ_PA 0x108
+#define TOF_ICC_REG_TNI_PBQ_SIZE 0x110
+#define TOF_ICC_REG_TNI_PRQ_PA 0x118
+#define TOF_ICC_REG_TNI_PRQ_PA_CACHE_INJECTION BIT(63)
+#define TOF_ICC_REG_TNI_PRQ_SIZE 0x120
+#define TOF_ICC_REG_TNI_PRQ_MASK 0x128
+#define TOF_ICC_REG_TNI_PRQ_ENTRY_COALESCING_TIMER 0x130
+#define TOF_ICC_REG_TNI_PRQ_INTERRUPT_COALESCING_TIMER 0x138
+#define TOF_ICC_REG_TNI_PRQ_INTERRUPT_COALESCING_COUNT 0x140
+#define TOF_ICC_REG_TNI_SEND_COUNT 0x148
+#define TOF_ICC_REG_TNI_NO_SEND_COUNT 0x150
+#define TOF_ICC_REG_TNI_BLOCK_SEND_COUNT 0x158
+#define TOF_ICC_REG_TNI_RECEIVE_COUNT 0x160
+#define TOF_ICC_REG_TNI_NO_RECEIVE_COUNT 0x168
+#define TOF_ICC_REG_TNI_NUM_SEND_TLP 0x170
+#define TOF_ICC_REG_TNI_BYTE_SEND_TLP 0x178
+#define TOF_ICC_REG_TNI_NUM_SEND_SYSTEM_TLP 0x180
+#define TOF_ICC_REG_TNI_NUM_RECEIVE_TLP 0x188
+#define TOF_ICC_REG_TNI_BYTE_RECEIVE_TLP 0x190
+#define TOF_ICC_REG_TNI_NUM_RECEIVE_NULLIFIED_TLP 0x198
+#define TOF_ICC_REG_TNI_RX_NUM_UNKNOWN_TLP 0x1a0
+#define TOF_ICC_REG_TNI_RX_NUM_SYSTEM_TLP 0x1a8
+#define TOF_ICC_REG_TNI_RX_NUM_EXCEPTION_TLP 0x1b0
+#define TOF_ICC_REG_TNI_RX_NUM_DISCARD_UNKNOWN_TLP 0x1b8
+#define TOF_ICC_REG_TNI_RX_NUM_DISCARD_SYSTEM_TLP 0x1c0
+#define TOF_ICC_REG_TNI_RX_NUM_DISCARD_EXCEPTION_TLP 0x1c8
+#define TOF_ICC_REG_TNI_DUMP_START 0x8
+#define TOF_ICC_REG_TNI_DUMP_END 0x1d0
+
+/* Port */
+#define TOF_ICC_REG_PORT_PA(port) (tof_icc_reg_pa + 0x0006000000 + (port) * 0x1000)
+#define TOF_ICC_REG_PORT_TX_VC0_ZERO_CREDIT_COUNT 0x0
+#define TOF_ICC_REG_PORT_TX_VC1_ZERO_CREDIT_COUNT 0x8
+#define TOF_ICC_REG_PORT_TX_VC2_ZERO_CREDIT_COUNT 0x10
+#define TOF_ICC_REG_PORT_TX_VC3_ZERO_CREDIT_COUNT 0x18
+#define TOF_ICC_REG_PORT_FREE_RUN_COUNT 0x80
+#define TOF_ICC_REG_PORT_NUM_SEND_DLLP 0xc0
+#define TOF_ICC_REG_PORT_NUM_SEND_TLP 0xc8
+#define TOF_ICC_REG_PORT_BYTE_SEND_TLP 0xd0
+#define TOF_ICC_REG_PORT_NUM_SEND_SYSTEM_TLP 0xd8
+#define TOF_ICC_REG_PORT_NUM_SEND_NULLIFIED_TLP 0xe0
+#define TOF_ICC_REG_PORT_NUM_TX_DISCARD_SYSTEM_TLP 0xe8
+#define TOF_ICC_REG_PORT_NUM_TX_DISCARD_NORMAL_TLP 0xf0
+#define TOF_ICC_REG_PORT_NUM_TX_FILTERED_NORMAL_TLP 0xf8
+#define TOF_ICC_REG_PORT_NUM_VIRTUAL_CUT_THROUGH_TLP 0x100
+#define TOF_ICC_REG_PORT_NUM_GENERATE_NULLIFIED_TLP 0x108
+#define TOF_ICC_REG_PORT_NUM_RECEIVE_DLLP 0x110
+#define TOF_ICC_REG_PORT_NUM_RECEIVE_TLP 0x118
+#define TOF_ICC_REG_PORT_BYTE_RECEIVE_TLP 0x120
+#define TOF_ICC_REG_PORT_NUM_RECEIVE_SYSTEM_TLP 0x128
+#define TOF_ICC_REG_PORT_NUM_RECEIVE_NULLIFIED_TLP 0x130
+#define TOF_ICC_REG_PORT_NUM_RX_DISCARD_SYSTEM_TLP 0x138
+#define TOF_ICC_REG_PORT_NUM_RX_DISCARD_NORMAL_TLP 0x140
+#define TOF_ICC_REG_PORT_NUM_RX_FILTERED_NORMAL_TLP 0x158
+#define TOF_ICC_REG_PORT_NUM_RX_DISCARD_NULLIFIED_TLP 0x160
+#define TOF_ICC_REG_PORT_FRAME_LCRC_ERROR_COUNT 0x170
+#define TOF_ICC_REG_PORT_TX_RETRY_BUFFER_CE_COUNT 0x180
+#define TOF_ICC_REG_PORT_RX_VC_BUFFER_CE_COUNT 0x188
+#define TOF_ICC_REG_PORT_XB_CE_COUNT 0x190
+#define TOF_ICC_REG_PORT_ACK_NACK_TIME_OUT_COUNT 0x198
+#define TOF_ICC_REG_PORT_SLICE0_FCS_ERROR_COUNT 0x1a0
+#define TOF_ICC_REG_PORT_SLICE1_FCS_ERROR_COUNT 0x1a8
+#define TOF_ICC_REG_PORT_DUMP_START 0x0
+#define TOF_ICC_REG_PORT_DUMP_END 0x1b0
+
+/* XB */
+#define TOF_ICC_REG_XB_PA (tof_icc_reg_pa + 0x000600f000)
+#define TOF_ICC_REG_XB_STQ_ENABLE 0x0
+#define TOF_ICC_REG_XB_STQ_UPDATE_INTERVAL 0x8
+#define TOF_ICC_REG_XB_STQ_PA 0x10
+#define TOF_ICC_REG_XB_STQ_SIZE 0x18
+#define TOF_ICC_REG_XB_STQ_NEXT_OFFSET 0x20
+#define TOF_ICC_REG_XB_DUMP_START 0x0
+#define TOF_ICC_REG_XB_DUMP_END 0x28
+
+#define TOF_ICC_XB_TC_DATA_CYCLE_COUNT(tni) ((tni) * 0x10 + 0x0)
+#define TOF_ICC_XB_TC_WAIT_CYCLE_COUNT(tni) ((tni) * 0x10 + 0x8)
+#define TOF_ICC_XB_TD_DATA_CYCLE_COUNT(tnr) ((tnr) * 0x10 + 0x60)
+#define TOF_ICC_XB_TD_WAIT_CYCLE_COUNT(tnr) ((tnr) * 0x10 + 0x68)
+
+/* Tofu */
+#define TOF_ICC_REG_TOFU_PA (tof_icc_reg_pa + 0x0007000000)
+#define TOF_ICC_REG_TOFU_NODE_ADDRESS 0x0
+#define TOF_ICC_REG_TOFU_NODE_ADDRESS_X GENMASK(22, 18)
+#define TOF_ICC_REG_TOFU_NODE_ADDRESS_Y GENMASK(17, 13)
+#define TOF_ICC_REG_TOFU_NODE_ADDRESS_Z GENMASK(12, 8)
+#define TOF_ICC_REG_TOFU_NODE_ADDRESS_A BIT(7)
+#define TOF_ICC_REG_TOFU_NODE_ADDRESS_B GENMASK(6, 5)
+#define TOF_ICC_REG_TOFU_NODE_ADDRESS_C BIT(4)
+#define TOF_ICC_REG_TOFU_PORT_SETTING 0x8
+#define TOF_ICC_REG_TOFU_TD_TLP_FILTER(tnr) ((tnr) * 0x10 + 0x10)
+#define TOF_ICC_REG_TOFU_TD_SETTINGS(tnr) ((tnr) * 0x10 + 0x18)
+#define TOF_ICC_REG_TOFU_TNR_MSI_BASE 0xc0
+#define TOF_ICC_REG_TOFU_TNR_IRR 0xc8
+#define TOF_ICC_REG_TOFU_TNR_IMR 0xd0
+#define TOF_ICC_REG_TOFU_TNR_IRC 0xd8
+#define TOF_ICC_REG_TOFU_TNR_IMC 0xe0
+#define TOF_ICC_REG_TOFU_TNR_ICL 0xe8
+#define TOF_ICC_REG_TOFU_TNI_VMS(tni, vmsid) ((tni) * 0x100 + (vmsid) * 0x8 + 0x100)
+#define TOF_ICC_REG_TOFU_TNI_VMS_CQ00(tni) ((tni) * 0x100 + 0x180)
+#define TOF_ICC_REG_TOFU_TNI_VMS_BG00(tni) ((tni) * 0x100 + 0x1a0)
+#define TOF_ICC_REG_TOFU_TNI_VMS_BG16(tni) ((tni) * 0x100 + 0x1a8)
+#define TOF_ICC_REG_TOFU_TNI_VMS_BG32(tni) ((tni) * 0x100 + 0x1b0)
+#define TOF_ICC_REG_TOFU_TNI_MSI_BASE(tni) ((tni) * 0x100 + 0x1c0)
+#define TOF_ICC_REG_TOFU_DUMP_START 0x0
+#define TOF_ICC_REG_TOFU_DUMP_END 0x6c8
+
+/** Interrupts **/
+#define TOF_ICC_IRQ_CQS_TOQ_READ_EXCEPTION BIT(0)
+#define TOF_ICC_IRQ_CQS_TOQ_DIRECT_DESCRIPTOR_EXCEPTION BIT(1)
+#define TOF_ICC_IRQ_CQS_TOQ_MARKED_UE BIT(2)
+#define TOF_ICC_IRQ_CQS_TCQ_WRITE_EXCEPTION BIT(3)
+#define TOF_ICC_IRQ_CQS_TOQ_SOURCE_TYPE_EXCEPTION BIT(4)
+#define TOF_ICC_IRQ_CQS_TCQ_WRITE_ACKNOWLEDGE BIT(5)
+#define TOF_ICC_IRQ_CQS_MRQ_WRITE_ACKNOWLEDGE BIT(7)
+#define TOF_ICC_IRQ_CQS_MRQ_WRITE_EXCEPTION BIT(8)
+#define TOF_ICC_IRQ_CQS_MRQ_OVERFLOW BIT(9)
+#define TOF_ICC_IRQ_CQS_STEERING_READ_EXCEPTION BIT(36)
+#define TOF_ICC_IRQ_CQS_MB_READ_EXCEPTION BIT(38)
+#define TOF_ICC_IRQ_CQS_PAYLOAD_READ_EXCEPTION BIT(39)
+#define TOF_ICC_IRQ_CQS_PAYLOAD_WRITE_EXCEPTION BIT(40)
+/* Just for convinience of irr value, no exists CQS CACHEFLUSH_TIMEOUT interrupt  */
+#define TOF_ICC_DUMMY_IRQ_CQS_CACHEFLUSH_TIMEOUT BIT(63)
+
+#define TOF_ICC_IRQ_BGS_NODE_ADDRESS_UNMATCH BIT(0)
+#define TOF_ICC_IRQ_BGS_BG_RECV_ADDRESS_EXCEPTION BIT(1)
+#define TOF_ICC_IRQ_BGS_BG_SEND_ADDRESS_EXCEPTION BIT(2)
+#define TOF_ICC_IRQ_BGS_GPID_UNMATCH BIT(3)
+#define TOF_ICC_IRQ_BGS_BSEQ_UNMATCH BIT(4)
+#define TOF_ICC_IRQ_BGS_SIGNAL_STATE_ERROR BIT(5)
+#define TOF_ICC_IRQ_BGS_SYNCHRONIZATION_ACKNOWLEDGE BIT(24)
+#define TOF_ICC_IRQ_BGS_ERROR_SYNCHRONIZATION_ACKNOWLEDGE BIT(25)
+#define TOF_ICC_IRQ_BGS_DMA_COMPLETION_EXCEPTION BIT(26)
+
+#define TOF_ICC_IRQ_TNI_PBQ_READ_EXCEPTION BIT(0)
+#define TOF_ICC_IRQ_TNI_PBQ_MARKED_UE BIT(1)
+#define TOF_ICC_IRQ_TNI_PBQ_UNDERFLOW BIT(2)
+#define TOF_ICC_IRQ_TNI_PRQ_PACKET_DISCARD BIT(3)
+#define TOF_ICC_IRQ_TNI_PRQ_WRITE_ACKNOWLEDGE BIT(4)
+#define TOF_ICC_IRQ_TNI_PRQ_WRITE_EXCEPTION BIT(5)
+#define TOF_ICC_IRQ_TNI_PRQ_OVERFLOW BIT(6)
+#define TOF_ICC_IRQ_TNI_INACTIVE_BG BIT(16)
+#define TOF_ICC_IRQ_TNI_STAGE2_TRANSLATION_FAULT BIT(32)
+
+#define TOF_ICC_IRQ_TNR_TNR0_RX_FILTER_OUT BIT(0)
+#define TOF_ICC_IRQ_TNR_TNR0_TX_FILTER_OUT BIT(1)
+#define TOF_ICC_IRQ_TNR_TNR0_PORT_ERROR BIT(2)
+#define TOF_ICC_IRQ_TNR_TNR0_DATELINE_ERROR BIT(3)
+#define TOF_ICC_IRQ_TNR_TNR0_ROUTING_ERROR BIT(4)
+#define TOF_ICC_IRQ_TNR_TNR1_RX_FILTER_OUT BIT(6)
+#define TOF_ICC_IRQ_TNR_TNR1_TX_FILTER_OUT BIT(7)
+#define TOF_ICC_IRQ_TNR_TNR1_PORT_ERROR BIT(8)
+#define TOF_ICC_IRQ_TNR_TNR1_DATELINE_ERROR BIT(9)
+#define TOF_ICC_IRQ_TNR_TNR1_ROUTING_ERROR BIT(10)
+#define TOF_ICC_IRQ_TNR_TNR2_RX_FILTER_OUT BIT(12)
+#define TOF_ICC_IRQ_TNR_TNR2_TX_FILTER_OUT BIT(13)
+#define TOF_ICC_IRQ_TNR_TNR2_PORT_ERROR BIT(14)
+#define TOF_ICC_IRQ_TNR_TNR2_DATELINE_ERROR BIT(15)
+#define TOF_ICC_IRQ_TNR_TNR2_ROUTING_ERROR BIT(16)
+#define TOF_ICC_IRQ_TNR_TNR3_RX_FILTER_OUT BIT(18)
+#define TOF_ICC_IRQ_TNR_TNR3_TX_FILTER_OUT BIT(19)
+#define TOF_ICC_IRQ_TNR_TNR3_PORT_ERROR BIT(20)
+#define TOF_ICC_IRQ_TNR_TNR3_DATELINE_ERROR BIT(21)
+#define TOF_ICC_IRQ_TNR_TNR3_ROUTING_ERROR BIT(22)
+#define TOF_ICC_IRQ_TNR_TNR4_RX_FILTER_OUT BIT(24)
+#define TOF_ICC_IRQ_TNR_TNR4_TX_FILTER_OUT BIT(25)
+#define TOF_ICC_IRQ_TNR_TNR4_PORT_ERROR BIT(26)
+#define TOF_ICC_IRQ_TNR_TNR4_DATELINE_ERROR BIT(27)
+#define TOF_ICC_IRQ_TNR_TNR4_ROUTING_ERROR BIT(28)
+#define TOF_ICC_IRQ_TNR_TNR5_RX_FILTER_OUT BIT(30)
+#define TOF_ICC_IRQ_TNR_TNR5_TX_FILTER_OUT BIT(31)
+#define TOF_ICC_IRQ_TNR_TNR5_PORT_ERROR BIT(32)
+#define TOF_ICC_IRQ_TNR_TNR5_DATELINE_ERROR BIT(33)
+#define TOF_ICC_IRQ_TNR_TNR5_ROUTING_ERROR BIT(34)
+#define TOF_ICC_IRQ_TNR_TNR6_RX_FILTER_OUT BIT(36)
+#define TOF_ICC_IRQ_TNR_TNR6_TX_FILTER_OUT BIT(37)
+#define TOF_ICC_IRQ_TNR_TNR6_PORT_ERROR BIT(38)
+#define TOF_ICC_IRQ_TNR_TNR6_DATELINE_ERROR BIT(39)
+#define TOF_ICC_IRQ_TNR_TNR6_ROUTING_ERROR BIT(40)
+#define TOF_ICC_IRQ_TNR_TNR7_RX_FILTER_OUT BIT(42)
+#define TOF_ICC_IRQ_TNR_TNR7_TX_FILTER_OUT BIT(43)
+#define TOF_ICC_IRQ_TNR_TNR7_PORT_ERROR BIT(44)
+#define TOF_ICC_IRQ_TNR_TNR7_DATELINE_ERROR BIT(45)
+#define TOF_ICC_IRQ_TNR_TNR7_ROUTING_ERROR BIT(46)
+#define TOF_ICC_IRQ_TNR_TNR8_RX_FILTER_OUT BIT(48)
+#define TOF_ICC_IRQ_TNR_TNR8_TX_FILTER_OUT BIT(49)
+#define TOF_ICC_IRQ_TNR_TNR8_PORT_ERROR BIT(50)
+#define TOF_ICC_IRQ_TNR_TNR8_DATELINE_ERROR BIT(51)
+#define TOF_ICC_IRQ_TNR_TNR8_ROUTING_ERROR BIT(52)
+#define TOF_ICC_IRQ_TNR_TNR9_RX_FILTER_OUT BIT(54)
+#define TOF_ICC_IRQ_TNR_TNR9_TX_FILTER_OUT BIT(55)
+#define TOF_ICC_IRQ_TNR_TNR9_PORT_ERROR BIT(56)
+#define TOF_ICC_IRQ_TNR_TNR9_DATELINE_ERROR BIT(57)
+#define TOF_ICC_IRQ_TNR_TNR9_ROUTING_ERROR BIT(58)
+
+#endif
+
+/* vim: set noet ts=8 sw=8 sts=0 tw=0 : */
+

executer/kernel/mcctrl/arch/arm64/include/tofu/tof_uapi.h

@@ -0,0 +1,345 @@
+#ifndef _TOF_UAPI_H_
+#define _TOF_UAPI_H_
+
+#ifdef __KERNEL__
+#include <linux/types.h>
+#else
+#include <stdint.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#endif
+
+enum tof_sig_errno_cq {
+	TOF_TOQ_DIRECT_DESCRIPTOR_EXCEPTION,
+	TOF_TOQ_SOURCE_TYPE_EXCEPTION,
+	TOF_MRQ_OVERFLOW,
+	TOF_CQS_CACHEFLUSH_TIMEOUT,
+};
+
+enum tof_sig_errno_bg {
+	TOF_NODE_ADDRESS_UNMATCH,
+	TOF_BSEQ_UNMATCH,
+	TOF_SIGNAL_STATE_ERROR,
+	TOF_ERROR_SYNCHRONIZATION_ACKNOWLEDGE,
+};
+
+#define TOF_UAPI_VERSION 0x2a00
+
+struct tof_init_cq {
+	uint16_t version;
+	uint8_t session_mode;
+	uint8_t toq_size;
+	uint8_t mrq_size;
+	uint8_t num_stag;
+	uint8_t tcq_cinj;
+	uint8_t mrq_cinj;
+	void *toq_mem;
+	void *tcq_mem;
+	void *mrq_mem;
+};
+
+struct tof_alloc_stag {
+	uint32_t flags;
+	int stag;
+	uint64_t offset;
+	void *va;
+	uint64_t len;
+};
+
+struct tof_free_stags {
+	uint16_t num;
+	int *stags;
+};
+
+struct tof_addr {
+	uint8_t pa;
+	uint8_t pb;
+	uint8_t pc;
+	uint8_t x;
+	uint8_t y;
+	uint8_t z;
+	uint8_t a;
+	uint8_t b;
+	uint8_t c;
+};
+
+struct tof_set_bg {
+	int tni;
+	int gate;
+	int source_lgate;
+	struct tof_addr source_raddr;
+	int source_rtni;
+	int source_rgate;
+	int dest_lgate;
+	struct tof_addr dest_raddr;
+	int dest_rtni;
+	int dest_rgate;
+};
+
+struct tof_enable_bch {
+	void *addr;
+	int bseq;
+	int num;
+	struct tof_set_bg *bgs;
+};
+
+struct tof_set_subnet {
+	int res0;
+	int res1;
+	uint8_t nx;
+	uint8_t sx;
+	uint8_t lx;
+	uint8_t ny;
+	uint8_t sy;
+	uint8_t ly;
+	uint8_t nz;
+	uint8_t sz;
+	uint8_t lz;
+};
+
+struct tof_reg_user {
+	uid_t uid;
+	uint32_t gpid;
+	struct tof_set_subnet subnet;
+	uint64_t *cqmask;
+	uint64_t *bgmask;
+};
+
+struct tof_notify_linkdown {
+	int num;
+	struct {
+		uint8_t x;
+		uint8_t y;
+		uint8_t z;
+		uint8_t a;
+		uint8_t b;
+		uint8_t c;
+		uint16_t ports;
+	} *items;
+};
+
+struct tof_get_port_stat {
+	int port_no;
+	uint64_t mask;
+	uint64_t pa[31];
+};
+
+struct tof_get_cq_stat {
+	int tni;
+	int cqid;
+	uint64_t txbyte;
+	uint64_t rxbyte;
+};
+
+struct tof_load_register {
+	uint64_t pa;
+	uint64_t len;
+	void *buf;
+};
+
+struct tof_load_resource {
+	uint64_t rsc_id;
+	uint64_t offset;
+	uint64_t len;
+	void *buf;
+};
+
+union tof_trans_table_bitfield {
+	struct {
+		uint64_t start:36;
+		uint64_t len:27;
+		uint64_t ps_code:1;
+	} bits;
+	uint64_t atomic;
+};
+
+struct tof_trans_table {
+	union tof_trans_table_bitfield steering;
+	union tof_trans_table_bitfield mbpt;
+};
+
+void tof_utofu_set_linkdown_callback(void (*callback)(int, const void *));
+void tof_utofu_unset_linkdown_callback(void);
+
+#define TOF_MMAP_CQ_REGISTER 0
+#ifdef __KERNEL__
+#define TOF_MMAP_CQ_TRANSTABLE (PAGE_SIZE)
+#else
+#define TOF_MMAP_CQ_TRANSTABLE (sysconf(_SC_PAGESIZE))
+#endif
+#define TOF_MMAP_BCH_REGISTER 0
+#define TOF_MMAP_XB_STQ 0
+
+#define TOF_ST_RDWR 0x0
+#define TOF_ST_RDONLY 0x1
+#define TOF_ST_LPG 0x2
+
+#define TOF_STAG_TRANS_PS_CODE_64KB 0
+#define TOF_STAG_TRANS_PS_CODE_2MB  1
+
+#define TOF_IOC_MAGIC 'd'
+#define TOF_IOCTL_INIT_CQ _IOWR(TOF_IOC_MAGIC, 0, long)
+#define TOF_IOCTL_ALLOC_STAG _IOWR(TOF_IOC_MAGIC, 1, long)
+#define TOF_IOCTL_FREE_STAGS _IOWR(TOF_IOC_MAGIC, 2, long)
+#define TOF_IOCTL_ENABLE_BCH _IOWR(TOF_IOC_MAGIC, 3, long)
+#define TOF_IOCTL_DISABLE_BCH _IOWR(TOF_IOC_MAGIC, 4, long)
+#define TOF_IOCTL_SET_RT_SIGNAL _IOWR(TOF_IOC_MAGIC, 5, long)
+#define TOF_IOCTL_SET_SUBNET _IOWR(TOF_IOC_MAGIC, 6, long)
+#define TOF_IOCTL_REG_USER _IOWR(TOF_IOC_MAGIC, 7, long)
+#define TOF_IOCTL_NOTIFY_LINKDOWN _IOWR(TOF_IOC_MAGIC, 8, long)
+#define TOF_IOCTL_GET_PORT_STAT _IOWR(TOF_IOC_MAGIC, 9, long)
+#define TOF_IOCTL_GET_CQ_STAT _IOWR(TOF_IOC_MAGIC, 10, long)
+#define TOF_IOCTL_LOAD_REGISTER _IOWR(TOF_IOC_MAGIC, 11, long)
+#define TOF_IOCTL_LOAD_RESOURCE _IOWR(TOF_IOC_MAGIC, 12, long)
+
+#ifndef __KERNEL__
+#define TOF_INIT_CQ TOF_IOCTL_INIT_CQ
+#define TOF_ALLOC_STAG TOF_IOCTL_ALLOC_STAG
+#define TOF_FREE_STAGS TOF_IOCTL_FREE_STAGS
+#define TOF_ENABLE_BCH TOF_IOCTL_ENABLE_BCH
+#define TOF_DISABLE_BCH TOF_IOCTL_DISABLE_BCH
+#define TOF_SET_RT_SIGNAL TOF_IOCTL_SET_RT_SIGNAL
+#define TOF_SET_SUBNET TOF_IOCTL_SET_SUBNET
+#define TOF_REG_USER TOF_IOCTL_REG_USER
+#define TOF_NOTIFY_LINKDOWN TOF_IOCTL_NOTIFY_LINKDOWN
+#define TOF_GET_PORT_STAT TOF_IOCTL_GET_PORT_STAT
+#define TOF_GET_CQ_STAT TOF_IOCTL_GET_CQ_STAT
+#define TOF_LOAD_REGISTER TOF_IOCTL_LOAD_REGISTER
+#define TOF_LOAD_RESOURCE TOF_IOCTL_LOAD_RESOURCE
+#endif
+
+enum {
+	/* TOQ (0 - 71) */
+	TOF_RSC_TNI0_TOQ0 = 0, TOF_RSC_TNI0_TOQ1, TOF_RSC_TNI0_TOQ2, TOF_RSC_TNI0_TOQ3,
+	TOF_RSC_TNI0_TOQ4, TOF_RSC_TNI0_TOQ5, TOF_RSC_TNI0_TOQ6, TOF_RSC_TNI0_TOQ7,
+	TOF_RSC_TNI0_TOQ8, TOF_RSC_TNI0_TOQ9, TOF_RSC_TNI0_TOQ10, TOF_RSC_TNI0_TOQ11,
+	TOF_RSC_TNI1_TOQ0, TOF_RSC_TNI1_TOQ1, TOF_RSC_TNI1_TOQ2, TOF_RSC_TNI1_TOQ3,
+	TOF_RSC_TNI1_TOQ4, TOF_RSC_TNI1_TOQ5, TOF_RSC_TNI1_TOQ6, TOF_RSC_TNI1_TOQ7,
+	TOF_RSC_TNI1_TOQ8, TOF_RSC_TNI1_TOQ9, TOF_RSC_TNI1_TOQ10, TOF_RSC_TNI1_TOQ11,
+	TOF_RSC_TNI2_TOQ0, TOF_RSC_TNI2_TOQ1, TOF_RSC_TNI2_TOQ2, TOF_RSC_TNI2_TOQ3,
+	TOF_RSC_TNI2_TOQ4, TOF_RSC_TNI2_TOQ5, TOF_RSC_TNI2_TOQ6, TOF_RSC_TNI2_TOQ7,
+	TOF_RSC_TNI2_TOQ8, TOF_RSC_TNI2_TOQ9, TOF_RSC_TNI2_TOQ10, TOF_RSC_TNI2_TOQ11,
+	TOF_RSC_TNI3_TOQ0, TOF_RSC_TNI3_TOQ1, TOF_RSC_TNI3_TOQ2, TOF_RSC_TNI3_TOQ3,
+	TOF_RSC_TNI3_TOQ4, TOF_RSC_TNI3_TOQ5, TOF_RSC_TNI3_TOQ6, TOF_RSC_TNI3_TOQ7,
+	TOF_RSC_TNI3_TOQ8, TOF_RSC_TNI3_TOQ9, TOF_RSC_TNI3_TOQ10, TOF_RSC_TNI3_TOQ11,
+	TOF_RSC_TNI4_TOQ0, TOF_RSC_TNI4_TOQ1, TOF_RSC_TNI4_TOQ2, TOF_RSC_TNI4_TOQ3,
+	TOF_RSC_TNI4_TOQ4, TOF_RSC_TNI4_TOQ5, TOF_RSC_TNI4_TOQ6, TOF_RSC_TNI4_TOQ7,
+	TOF_RSC_TNI4_TOQ8, TOF_RSC_TNI4_TOQ9, TOF_RSC_TNI4_TOQ10, TOF_RSC_TNI4_TOQ11,
+	TOF_RSC_TNI5_TOQ0, TOF_RSC_TNI5_TOQ1, TOF_RSC_TNI5_TOQ2, TOF_RSC_TNI5_TOQ3,
+	TOF_RSC_TNI5_TOQ4, TOF_RSC_TNI5_TOQ5, TOF_RSC_TNI5_TOQ6, TOF_RSC_TNI5_TOQ7,
+	TOF_RSC_TNI5_TOQ8, TOF_RSC_TNI5_TOQ9, TOF_RSC_TNI5_TOQ10, TOF_RSC_TNI5_TOQ11,
+
+	/* TOQ (72 - 143) */
+	TOF_RSC_TNI0_TCQ0, TOF_RSC_TNI0_TCQ1, TOF_RSC_TNI0_TCQ2, TOF_RSC_TNI0_TCQ3,
+	TOF_RSC_TNI0_TCQ4, TOF_RSC_TNI0_TCQ5, TOF_RSC_TNI0_TCQ6, TOF_RSC_TNI0_TCQ7,
+	TOF_RSC_TNI0_TCQ8, TOF_RSC_TNI0_TCQ9, TOF_RSC_TNI0_TCQ10, TOF_RSC_TNI0_TCQ11,
+	TOF_RSC_TNI1_TCQ0, TOF_RSC_TNI1_TCQ1, TOF_RSC_TNI1_TCQ2, TOF_RSC_TNI1_TCQ3,
+	TOF_RSC_TNI1_TCQ4, TOF_RSC_TNI1_TCQ5, TOF_RSC_TNI1_TCQ6, TOF_RSC_TNI1_TCQ7,
+	TOF_RSC_TNI1_TCQ8, TOF_RSC_TNI1_TCQ9, TOF_RSC_TNI1_TCQ10, TOF_RSC_TNI1_TCQ11,
+	TOF_RSC_TNI2_TCQ0, TOF_RSC_TNI2_TCQ1, TOF_RSC_TNI2_TCQ2, TOF_RSC_TNI2_TCQ3,
+	TOF_RSC_TNI2_TCQ4, TOF_RSC_TNI2_TCQ5, TOF_RSC_TNI2_TCQ6, TOF_RSC_TNI2_TCQ7,
+	TOF_RSC_TNI2_TCQ8, TOF_RSC_TNI2_TCQ9, TOF_RSC_TNI2_TCQ10, TOF_RSC_TNI2_TCQ11,
+	TOF_RSC_TNI3_TCQ0, TOF_RSC_TNI3_TCQ1, TOF_RSC_TNI3_TCQ2, TOF_RSC_TNI3_TCQ3,
+	TOF_RSC_TNI3_TCQ4, TOF_RSC_TNI3_TCQ5, TOF_RSC_TNI3_TCQ6, TOF_RSC_TNI3_TCQ7,
+	TOF_RSC_TNI3_TCQ8, TOF_RSC_TNI3_TCQ9, TOF_RSC_TNI3_TCQ10, TOF_RSC_TNI3_TCQ11,
+	TOF_RSC_TNI4_TCQ0, TOF_RSC_TNI4_TCQ1, TOF_RSC_TNI4_TCQ2, TOF_RSC_TNI4_TCQ3,
+	TOF_RSC_TNI4_TCQ4, TOF_RSC_TNI4_TCQ5, TOF_RSC_TNI4_TCQ6, TOF_RSC_TNI4_TCQ7,
+	TOF_RSC_TNI4_TCQ8, TOF_RSC_TNI4_TCQ9, TOF_RSC_TNI4_TCQ10, TOF_RSC_TNI4_TCQ11,
+	TOF_RSC_TNI5_TCQ0, TOF_RSC_TNI5_TCQ1, TOF_RSC_TNI5_TCQ2, TOF_RSC_TNI5_TCQ3,
+	TOF_RSC_TNI5_TCQ4, TOF_RSC_TNI5_TCQ5, TOF_RSC_TNI5_TCQ6, TOF_RSC_TNI5_TCQ7,
+	TOF_RSC_TNI5_TCQ8, TOF_RSC_TNI5_TCQ9, TOF_RSC_TNI5_TCQ10, TOF_RSC_TNI5_TCQ11,
+
+	/* MRQ (144 - 215) */
+	TOF_RSC_TNI0_MRQ0, TOF_RSC_TNI0_MRQ1, TOF_RSC_TNI0_MRQ2, TOF_RSC_TNI0_MRQ3,
+	TOF_RSC_TNI0_MRQ4, TOF_RSC_TNI0_MRQ5, TOF_RSC_TNI0_MRQ6, TOF_RSC_TNI0_MRQ7,
+	TOF_RSC_TNI0_MRQ8, TOF_RSC_TNI0_MRQ9, TOF_RSC_TNI0_MRQ10, TOF_RSC_TNI0_MRQ11,
+	TOF_RSC_TNI1_MRQ0, TOF_RSC_TNI1_MRQ1, TOF_RSC_TNI1_MRQ2, TOF_RSC_TNI1_MRQ3,
+	TOF_RSC_TNI1_MRQ4, TOF_RSC_TNI1_MRQ5, TOF_RSC_TNI1_MRQ6, TOF_RSC_TNI1_MRQ7,
+	TOF_RSC_TNI1_MRQ8, TOF_RSC_TNI1_MRQ9, TOF_RSC_TNI1_MRQ10, TOF_RSC_TNI1_MRQ11,
+	TOF_RSC_TNI2_MRQ0, TOF_RSC_TNI2_MRQ1, TOF_RSC_TNI2_MRQ2, TOF_RSC_TNI2_MRQ3,
+	TOF_RSC_TNI2_MRQ4, TOF_RSC_TNI2_MRQ5, TOF_RSC_TNI2_MRQ6, TOF_RSC_TNI2_MRQ7,
+	TOF_RSC_TNI2_MRQ8, TOF_RSC_TNI2_MRQ9, TOF_RSC_TNI2_MRQ10, TOF_RSC_TNI2_MRQ11,
+	TOF_RSC_TNI3_MRQ0, TOF_RSC_TNI3_MRQ1, TOF_RSC_TNI3_MRQ2, TOF_RSC_TNI3_MRQ3,
+	TOF_RSC_TNI3_MRQ4, TOF_RSC_TNI3_MRQ5, TOF_RSC_TNI3_MRQ6, TOF_RSC_TNI3_MRQ7,
+	TOF_RSC_TNI3_MRQ8, TOF_RSC_TNI3_MRQ9, TOF_RSC_TNI3_MRQ10, TOF_RSC_TNI3_MRQ11,
+	TOF_RSC_TNI4_MRQ0, TOF_RSC_TNI4_MRQ1, TOF_RSC_TNI4_MRQ2, TOF_RSC_TNI4_MRQ3,
+	TOF_RSC_TNI4_MRQ4, TOF_RSC_TNI4_MRQ5, TOF_RSC_TNI4_MRQ6, TOF_RSC_TNI4_MRQ7,
+	TOF_RSC_TNI4_MRQ8, TOF_RSC_TNI4_MRQ9, TOF_RSC_TNI4_MRQ10, TOF_RSC_TNI4_MRQ11,
+	TOF_RSC_TNI5_MRQ0, TOF_RSC_TNI5_MRQ1, TOF_RSC_TNI5_MRQ2, TOF_RSC_TNI5_MRQ3,
+	TOF_RSC_TNI5_MRQ4, TOF_RSC_TNI5_MRQ5, TOF_RSC_TNI5_MRQ6, TOF_RSC_TNI5_MRQ7,
+	TOF_RSC_TNI5_MRQ8, TOF_RSC_TNI5_MRQ9, TOF_RSC_TNI5_MRQ10, TOF_RSC_TNI5_MRQ11,
+
+	/* PBQ (216 - 221) */
+	TOF_RSC_TNI0_PBQ, TOF_RSC_TNI1_PBQ, TOF_RSC_TNI2_PBQ, TOF_RSC_TNI3_PBQ,
+	TOF_RSC_TNI4_PBQ, TOF_RSC_TNI5_PBQ,
+
+	/* PRQ (222 - 227) */
+	TOF_RSC_TNI0_PRQ, TOF_RSC_TNI1_PRQ, TOF_RSC_TNI2_PRQ, TOF_RSC_TNI3_PRQ,
+	TOF_RSC_TNI4_PRQ, TOF_RSC_TNI5_PRQ,
+
+	/* STEERINGTABLE (228 - 299) */
+	TOF_RSC_TNI0_STEERINGTABLE0, TOF_RSC_TNI0_STEERINGTABLE1, TOF_RSC_TNI0_STEERINGTABLE2,
+	TOF_RSC_TNI0_STEERINGTABLE3, TOF_RSC_TNI0_STEERINGTABLE4, TOF_RSC_TNI0_STEERINGTABLE5,
+	TOF_RSC_TNI0_STEERINGTABLE6, TOF_RSC_TNI0_STEERINGTABLE7, TOF_RSC_TNI0_STEERINGTABLE8,
+	TOF_RSC_TNI0_STEERINGTABLE9, TOF_RSC_TNI0_STEERINGTABLE10, TOF_RSC_TNI0_STEERINGTABLE11,
+	TOF_RSC_TNI1_STEERINGTABLE0, TOF_RSC_TNI1_STEERINGTABLE1, TOF_RSC_TNI1_STEERINGTABLE2,
+	TOF_RSC_TNI1_STEERINGTABLE3, TOF_RSC_TNI1_STEERINGTABLE4, TOF_RSC_TNI1_STEERINGTABLE5,
+	TOF_RSC_TNI1_STEERINGTABLE6, TOF_RSC_TNI1_STEERINGTABLE7, TOF_RSC_TNI1_STEERINGTABLE8,
+	TOF_RSC_TNI1_STEERINGTABLE9, TOF_RSC_TNI1_STEERINGTABLE10, TOF_RSC_TNI1_STEERINGTABLE11,
+	TOF_RSC_TNI2_STEERINGTABLE0, TOF_RSC_TNI2_STEERINGTABLE1, TOF_RSC_TNI2_STEERINGTABLE2,
+	TOF_RSC_TNI2_STEERINGTABLE3, TOF_RSC_TNI2_STEERINGTABLE4, TOF_RSC_TNI2_STEERINGTABLE5,
+	TOF_RSC_TNI2_STEERINGTABLE6, TOF_RSC_TNI2_STEERINGTABLE7, TOF_RSC_TNI2_STEERINGTABLE8,
+	TOF_RSC_TNI2_STEERINGTABLE9, TOF_RSC_TNI2_STEERINGTABLE10, TOF_RSC_TNI2_STEERINGTABLE11,
+	TOF_RSC_TNI3_STEERINGTABLE0, TOF_RSC_TNI3_STEERINGTABLE1, TOF_RSC_TNI3_STEERINGTABLE2,
+	TOF_RSC_TNI3_STEERINGTABLE3, TOF_RSC_TNI3_STEERINGTABLE4, TOF_RSC_TNI3_STEERINGTABLE5,
+	TOF_RSC_TNI3_STEERINGTABLE6, TOF_RSC_TNI3_STEERINGTABLE7, TOF_RSC_TNI3_STEERINGTABLE8,
+        TOF_RSC_TNI3_STEERINGTABLE9, TOF_RSC_TNI3_STEERINGTABLE10, TOF_RSC_TNI3_STEERINGTABLE11,
+        TOF_RSC_TNI4_STEERINGTABLE0, TOF_RSC_TNI4_STEERINGTABLE1, TOF_RSC_TNI4_STEERINGTABLE2,
+        TOF_RSC_TNI4_STEERINGTABLE3, TOF_RSC_TNI4_STEERINGTABLE4, TOF_RSC_TNI4_STEERINGTABLE5,
+        TOF_RSC_TNI4_STEERINGTABLE6, TOF_RSC_TNI4_STEERINGTABLE7, TOF_RSC_TNI4_STEERINGTABLE8,
+        TOF_RSC_TNI4_STEERINGTABLE9, TOF_RSC_TNI4_STEERINGTABLE10, TOF_RSC_TNI4_STEERINGTABLE11,
+        TOF_RSC_TNI5_STEERINGTABLE3, TOF_RSC_TNI5_STEERINGTABLE4, TOF_RSC_TNI5_STEERINGTABLE5,
+        TOF_RSC_TNI5_STEERINGTABLE6, TOF_RSC_TNI5_STEERINGTABLE7, TOF_RSC_TNI5_STEERINGTABLE8,
+        TOF_RSC_TNI5_STEERINGTABLE9, TOF_RSC_TNI5_STEERINGTABLE10, TOF_RSC_TNI5_STEERINGTABLE11,
+
+        /* MBTABLE (300 - 371) */
+        TOF_RSC_TNI0_MBTABLE0, TOF_RSC_TNI0_MBTABLE1, TOF_RSC_TNI0_MBTABLE2,
+        TOF_RSC_TNI0_MBTABLE3, TOF_RSC_TNI0_MBTABLE4, TOF_RSC_TNI0_MBTABLE5,
+        TOF_RSC_TNI0_MBTABLE6, TOF_RSC_TNI0_MBTABLE7, TOF_RSC_TNI0_MBTABLE8,
+        TOF_RSC_TNI0_MBTABLE9, TOF_RSC_TNI0_MBTABLE10, TOF_RSC_TNI0_MBTABLE11,
+        TOF_RSC_TNI1_MBTABLE0, TOF_RSC_TNI1_MBTABLE1, TOF_RSC_TNI1_MBTABLE2,
+        TOF_RSC_TNI1_MBTABLE3, TOF_RSC_TNI1_MBTABLE4, TOF_RSC_TNI1_MBTABLE5,
+        TOF_RSC_TNI1_MBTABLE6, TOF_RSC_TNI1_MBTABLE7, TOF_RSC_TNI1_MBTABLE8,
+        TOF_RSC_TNI1_MBTABLE9, TOF_RSC_TNI1_MBTABLE10, TOF_RSC_TNI1_MBTABLE11,
+        TOF_RSC_TNI2_MBTABLE0, TOF_RSC_TNI2_MBTABLE1, TOF_RSC_TNI2_MBTABLE2,
+        TOF_RSC_TNI2_MBTABLE3, TOF_RSC_TNI2_MBTABLE4, TOF_RSC_TNI2_MBTABLE5,
+        TOF_RSC_TNI2_MBTABLE6, TOF_RSC_TNI2_MBTABLE7, TOF_RSC_TNI2_MBTABLE8,
+        TOF_RSC_TNI2_MBTABLE9, TOF_RSC_TNI2_MBTABLE10, TOF_RSC_TNI2_MBTABLE11,
+        TOF_RSC_TNI3_MBTABLE0, TOF_RSC_TNI3_MBTABLE1, TOF_RSC_TNI3_MBTABLE2,
+        TOF_RSC_TNI3_MBTABLE3, TOF_RSC_TNI3_MBTABLE4, TOF_RSC_TNI3_MBTABLE5,
+        TOF_RSC_TNI3_MBTABLE6, TOF_RSC_TNI3_MBTABLE7, TOF_RSC_TNI3_MBTABLE8,
+        TOF_RSC_TNI3_MBTABLE9, TOF_RSC_TNI3_MBTABLE10, TOF_RSC_TNI3_MBTABLE11,
+        TOF_RSC_TNI4_MBTABLE0, TOF_RSC_TNI4_MBTABLE1, TOF_RSC_TNI4_MBTABLE2,
+        TOF_RSC_TNI4_MBTABLE3, TOF_RSC_TNI4_MBTABLE4, TOF_RSC_TNI4_MBTABLE5,
+        TOF_RSC_TNI4_MBTABLE6, TOF_RSC_TNI4_MBTABLE7, TOF_RSC_TNI4_MBTABLE8,
+        TOF_RSC_TNI4_MBTABLE9, TOF_RSC_TNI4_MBTABLE10, TOF_RSC_TNI4_MBTABLE11,
+        TOF_RSC_TNI5_MBTABLE0, TOF_RSC_TNI5_MBTABLE1, TOF_RSC_TNI5_MBTABLE2,
+        TOF_RSC_TNI5_MBTABLE3, TOF_RSC_TNI5_MBTABLE4, TOF_RSC_TNI5_MBTABLE5,
+        TOF_RSC_TNI5_MBTABLE6, TOF_RSC_TNI5_MBTABLE7, TOF_RSC_TNI5_MBTABLE8,
+        TOF_RSC_TNI5_MBTABLE9, TOF_RSC_TNI5_MBTABLE10, TOF_RSC_TNI5_MBTABLE11,
+
+        TOF_RSC_NUM     /* 372 */
+};
+#define TOF_RSC_TOQ(TNI, CQID)  (TOF_RSC_TNI0_TOQ0 + (TNI * 12) + CQID)
+#define TOF_RSC_TCQ(TNI, CQID)  (TOF_RSC_TNI0_TCQ0 + (TNI * 12) + CQID)
+#define TOF_RSC_MRQ(TNI, CQID)  (TOF_RSC_TNI0_MRQ0 + (TNI * 12) + CQID)
+#define TOF_RSC_PBQ(TNI)        (TOF_RSC_TNI0_PBQ + TNI)
+#define TOF_RSC_PRQ(TNI)        (TOF_RSC_TNI0_PRQ + TNI)
+#define TOF_RSC_STT(TNI, CQID)  (TOF_RSC_TNI0_STEERINGTABLE0 + (TNI * 12) + CQID)
+#define TOF_RSC_MBT(TNI, CQID)  (TOF_RSC_TNI0_MBTABLE0 + (TNI * 12) + CQID)
+
+#endif
+
+/* vim: set noet ts=8 sw=8 sts=0 tw=0 : */
+

executer/kernel/mcctrl/arch/arm64/include/tofu/tof_utofu_bg_bch.h

@@ -0,0 +1,6 @@
+	struct {
+		bool enabled;
+		uint64_t bgmask[TOF_ICC_NTNIS];
+		uintptr_t iova;
+		void *kaddr;
+	} bch;

executer/kernel/mcctrl/arch/arm64/include/tofu/tof_utofu_cq_trans.h

@@ -0,0 +1,6 @@
+			struct {
+				struct tof_utofu_trans_list *mru;
+				struct tof_trans_table *table;
+				int mruhead;
+				ihk_spinlock_t mru_lock;
+			} trans;

executer/kernel/mcctrl/arch/arm64/include/tofu/tofu_generated-tof_utofu_bg.h

@@ -0,0 +1,21 @@
+struct tof_utofu_bg {
+	union {
+		char whole_struct[160];
+		struct {
+			char padding0[0];
+			struct tof_utofu_device common;
+		};
+		struct {
+			char padding1[80];
+			uint8_t tni;
+		};
+		struct {
+			char padding2[81];
+			uint8_t bgid;
+		};
+		struct {
+			char padding3[88];
+			#include "tof_utofu_bg_bch.h"
+		};
+	};
+};

executer/kernel/mcctrl/arch/arm64/include/tofu/tofu_generated-tof_utofu_cq.h

@@ -0,0 +1,37 @@
+struct tof_utofu_cq {
+	union {
+		char whole_struct[384];
+		struct {
+			char padding0[0];
+			struct tof_utofu_device common;
+		};
+		struct {
+			char padding1[80];
+			uint8_t tni;
+		};
+		struct {
+			char padding2[81];
+			uint8_t cqid;
+		};
+		struct {
+			char padding3[104];
+			#include "tof_utofu_cq_trans.h"
+		};
+		struct {
+			char padding4[128];
+			struct tof_icc_steering_entry *steering;
+		};
+		struct {
+			char padding5[136];
+			struct tof_icc_mb_entry *mb;
+		};
+		struct {
+			char padding6[186];
+			uint8_t num_stag;
+		};
+		struct {
+			char padding7[336];
+			struct mmu_notifier mn;
+		};
+	};
+};

executer/kernel/mcctrl/arch/arm64/include/tofu/tofu_generated-tof_utofu_device.h

@@ -0,0 +1,17 @@
+struct tof_utofu_device {
+	union {
+		char whole_struct[80];
+		struct {
+			char padding0[0];
+			bool enabled;
+		};
+		struct {
+			char padding1[12];
+			uint32_t gpid;
+		};
+		struct {
+			char padding2[24];
+			uint64_t subnet;
+		};
+	};
+};

executer/kernel/mcctrl/arch/arm64/include/tofu/tofu_generated-tof_utofu_mbpt.h

@@ -0,0 +1,33 @@
+struct tof_utofu_mbpt {
+	union {
+		char whole_struct[56];
+		struct {
+			char padding0[0];
+			struct kref kref;
+		};
+		struct {
+			char padding1[8];
+			struct tof_utofu_cq *ucq;
+		};
+		struct {
+			char padding2[16];
+			uintptr_t iova;
+		};
+		struct {
+			char padding3[24];
+			struct scatterlist *sg;
+		};
+		struct {
+			char padding4[32];
+			size_t nsgents;
+		};
+		struct {
+			char padding5[40];
+			uintptr_t mbptstart;
+		};
+		struct {
+			char padding6[48];
+			size_t pgsz;
+		};
+	};
+};

executer/kernel/mcctrl/control.c

@@ -230,6 +230,9 @@ static long mcexec_prepare_image(ihk_os_t os,
 
 	dprintk("%s: pid %d, rpgtable: 0x%lx added\n",
 		__FUNCTION__, ppd->pid, ppd->rpgtable);
+#ifdef ENABLE_TOFU
+	ppd->enable_tofu = pdesc->enable_tofu;
+#endif
 
 	ret = 0;
 
@@ -279,6 +282,11 @@ int mcexec_transfer_image(ihk_os_t os, struct remote_transfer *__user upt)
 	rpm = ihk_device_map_virtual(ihk_os_to_dev(os), phys, PAGE_SIZE, NULL, 0);
 #endif
 
+	if (!rpm) {
+		pr_err("%s(): error: invalid remote address\n", __func__);
+		return -EFAULT;
+	}
+
 	if (pt.direction == MCEXEC_UP_TRANSFER_TO_REMOTE) {
 		if (copy_from_user(rpm, pt.userp, pt.size)) {
 			ret = -EFAULT;
@@ -1258,7 +1266,7 @@ void mcctrl_put_per_proc_data(struct mcctrl_per_proc_data *ppd)
 			   process is gone and the application should be terminated. */
 			packet = (struct ikc_scd_packet *)ptd->data;
 			dprintk("%s: calling __return_syscall (hash),target pid=%d,tid=%d\n", __FUNCTION__, ppd->pid, packet->req.rtid);
-			__return_syscall(ppd->ud->os, packet, -ERESTARTSYS,
+			__return_syscall(ppd->ud->os, ppd, packet, -ERESTARTSYS,
 					 packet->req.rtid);
 			ihk_ikc_release_packet((struct ihk_ikc_free_packet *)packet);
 
@@ -1282,7 +1290,7 @@ void mcctrl_put_per_proc_data(struct mcctrl_per_proc_data *ppd)
 
 		/* We use ERESTARTSYS to tell the LWK that the proxy
 		 * process is gone and the application should be terminated */
-		__return_syscall(ppd->ud->os, packet, -ERESTARTSYS,
+		__return_syscall(ppd->ud->os, ppd, packet, -ERESTARTSYS,
 				packet->req.rtid);
 		ihk_ikc_release_packet((struct ihk_ikc_free_packet *)packet);
 	}
@@ -1323,7 +1331,7 @@ int mcexec_syscall(struct mcctrl_usrdata *ud, struct ikc_scd_packet *packet)
 
 		/* We use ERESTARTSYS to tell the LWK that the proxy
 		 * process is gone and the application should be terminated */
-		__return_syscall(ud->os, packet, -ERESTARTSYS,
+		__return_syscall(ud->os, NULL, packet, -ERESTARTSYS,
 				packet->req.rtid);
 		ihk_ikc_release_packet((struct ihk_ikc_free_packet *)packet);
 
@@ -1766,7 +1774,7 @@ long mcexec_ret_syscall(ihk_os_t os, struct syscall_ret_desc *__user arg)
 		ihk_device_unmap_memory(ihk_os_to_dev(os), phys, ret.size);
 	} 
 
-	__return_syscall(os, packet, ret.ret, task_pid_vnr(current));
+	__return_syscall(os, ppd, packet, ret.ret, task_pid_vnr(current));
 
 	error = 0;
 out:
@@ -2794,7 +2802,7 @@ static long mcexec_terminate_thread_unsafe(ihk_os_t os, int pid, int tid, long c
 				__FUNCTION__, tid);
 		goto no_ptd;
 	}
-	__return_syscall(usrdata->os, packet, code, tid);
+	__return_syscall(usrdata->os, ppd, packet, code, tid);
 	ihk_ikc_release_packet((struct ihk_ikc_free_packet *)packet);
 
 	/* Drop reference for this function */
@@ -3582,7 +3590,11 @@ int mcctrl_get_request_os_cpu(ihk_os_t os, int *ret_cpu)
 	*ret_cpu = ch->send.queue->read_cpu;
 	ret = 0;
 
+#ifndef ENABLE_FUGAKU_HACKS
 	pr_info("%s: OS: %lx, CPU: %d\n",
+#else
+	dprintk("%s: OS: %lx, CPU: %d\n",
+#endif
 		__func__, (unsigned long)os, *ret_cpu);
 
 out_put_ppd:
@@ -3646,7 +3658,11 @@ int __mcctrl_os_read_write_cpu_register(ihk_os_t os, int cpu,
 	/* Notify caller (for future async implementation) */
 	atomic_set(&desc->sync, 1);
 
+#ifndef ENABLE_FUGAKU_HACKS
 	dprintk("%s: MCCTRL_OS_CPU_%s_REGISTER: CPU: %d, addr_ext: 0x%lx, val: 0x%lx\n",
+#else
+	printk("%s: MCCTRL_OS_CPU_%s_REGISTER: CPU: %d, addr_ext: 0x%lx, val: 0x%lx\n",
+#endif
 		__FUNCTION__,
 		(op == MCCTRL_OS_CPU_READ_REGISTER ? "READ" : "WRITE"), cpu,
 		desc->addr_ext, desc->val);

executer/kernel/mcctrl/driver.c

@@ -235,7 +235,6 @@ void (*mcctrl_zap_page_range)(struct vm_area_struct *vma,
 
 struct inode_operations *mcctrl_hugetlbfs_inode_operations;
 
-
 static int symbols_init(void)
 {
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4,17,0)

executer/kernel/mcctrl/ikc.c

@@ -536,9 +536,6 @@ int prepare_ikc_channels(ihk_os_t os)
 	usrdata->os = os;
 	ihk_host_os_set_usrdata(os, usrdata);
 
-	ihk_ikc_listen_port(os, &lp_ikc2linux);
-	ihk_ikc_listen_port(os, &lp_ikc2mckernel);
-
 	init_waitqueue_head(&usrdata->wq_procfs);
 	mutex_init(&usrdata->reserve_lock);
 	mutex_init(&usrdata->part_exec_lock);
@@ -555,6 +552,16 @@ int prepare_ikc_channels(ihk_os_t os)
 	INIT_LIST_HEAD(&usrdata->wakeup_descs_list);
 	spin_lock_init(&usrdata->wakeup_descs_lock);
 
+	/* ihk_ikc_listen_port should be performed after
+	 * usrdata->cpu_topology_list is initialized because the
+	 * function enables syscall_packet_handler which accesses
+	 * the list (the call path is sysfsm_packet_handler -->
+	 * sysfsm_work_main --> sysfsm_setup --> setup_sysfs_files
+	 * --> setup_cpus_sysfs_files).
+	 */
+	ihk_ikc_listen_port(os, &lp_ikc2linux);
+	ihk_ikc_listen_port(os, &lp_ikc2mckernel);
+
 	return 0;
 
 error:

executer/kernel/mcctrl/mcctrl.h

@@ -263,6 +263,7 @@ struct mcctrl_per_proc_data {
 
 	struct list_head devobj_pager_list;
 	struct semaphore devobj_pager_lock;
+	int enable_tofu;
 };
 
 struct sysfsm_req {
@@ -464,7 +465,8 @@ struct mcctrl_per_thread_data *mcctrl_get_per_thread_data(struct mcctrl_per_proc
 							  struct task_struct *task);
 int mcctrl_clear_pte_range(uintptr_t start, uintptr_t len);
 
-void __return_syscall(ihk_os_t os, struct ikc_scd_packet *packet, 
+void __return_syscall(ihk_os_t os, struct mcctrl_per_proc_data *ppd,
+		struct ikc_scd_packet *packet,
 		long ret, int stid);
 int clear_pte_range(uintptr_t start, uintptr_t len);
 
@@ -577,10 +579,13 @@ struct mcctrl_file_to_pidfd {
 	int pid;
 	int fd;
 	struct list_head hash;
+	char tofu_dev_path[128];
+	void *pde_data;
 };
 
 int mcctrl_file_to_pidfd_hash_insert(struct file *filp,
-	ihk_os_t os, int pid, struct task_struct *group_leader, int fd);
+	ihk_os_t os, int pid, struct task_struct *group_leader, int fd,
+	char *path, void *pde_data);
 struct mcctrl_file_to_pidfd *mcctrl_file_to_pidfd_hash_lookup(
 	struct file *filp, struct task_struct *group_leader);
 int mcctrl_file_to_pidfd_hash_remove(struct file *filp,

executer/kernel/mcctrl/syscall.c

@@ -692,14 +692,20 @@ static int rus_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 					vma->vm_start, vma->vm_end, pgsize, pix);
 			}
 		}
-		else
+		else {
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 18, 0)
 			error = vmf_insert_pfn(vma, rva+(pix*PAGE_SIZE),
 					       pfn+pix);
+			if (error == VM_FAULT_NOPAGE) {
+				dprintk("%s: vmf_insert_pfn returned %d\n",
+					__func__, error);
+				error = 0;
+			}
 #else
 			error = vm_insert_pfn(vma, rva+(pix*PAGE_SIZE),
 					      pfn+pix);
 #endif
+		}
 		if (error) {
 			pr_err("%s: vm_insert_pfn returned %d\n",
 			       __func__, error);
@@ -1858,7 +1864,8 @@ void mcctrl_file_to_pidfd_hash_init(void)
 }
 
 int mcctrl_file_to_pidfd_hash_insert(struct file *filp,
-	ihk_os_t os, int pid, struct task_struct *group_leader, int fd)
+	ihk_os_t os, int pid, struct task_struct *group_leader, int fd,
+	char *path, void *pde_data)
 {
 	unsigned long irqflags;
 	struct mcctrl_file_to_pidfd *file2pidfd_iter;
@@ -1876,6 +1883,9 @@ int mcctrl_file_to_pidfd_hash_insert(struct file *filp,
 	file2pidfd->pid = pid;
 	file2pidfd->group_leader = group_leader;
 	file2pidfd->fd = fd;
+	/* Only copy the name under /proc/tofu/dev/ */
+	strncpy(file2pidfd->tofu_dev_path, path + 15, 128);
+	file2pidfd->pde_data = pde_data;
 
 	spin_lock_irqsave(&mcctrl_file_to_pidfd_hash_lock, irqflags);
 	list_for_each_entry(file2pidfd_iter,
@@ -1974,7 +1984,8 @@ unlock_out:
 }
 #endif
 
-void __return_syscall(ihk_os_t os, struct ikc_scd_packet *packet,
+void __return_syscall(ihk_os_t os, struct mcctrl_per_proc_data *ppd,
+		struct ikc_scd_packet *packet,
 		long ret, int stid)
 {
 	unsigned long phys;
@@ -2005,54 +2016,104 @@ void __return_syscall(ihk_os_t os, struct ikc_scd_packet *packet,
 	res->stid = stid;
 
 #ifdef ENABLE_TOFU
-	/* Record PDE_DATA after open() calls for Tofu driver */
+	/* Tofu enabled process? */
-	if (packet->req.number == __NR_openat && ret > 1) {
+	if (ppd && ppd->enable_tofu) {
 		char *pathbuf, *fullpath;
-		struct fd f;
-		int fd;
 
-		fd = ret;
+		/* Record PDE_DATA after open() calls for Tofu driver */
-		f = fdget(fd);
+		if (packet->req.number == __NR_openat && ret > 1) {
+			struct fd f;
+			int fd;
 
-		if (!f.file) {
+			fd = ret;
-			goto out_notify;
+			f = fdget(fd);
+
+			if (!f.file) {
+				goto out_notify;
+			}
+
+			pathbuf = (char *)__get_free_page(GFP_ATOMIC);
+			if (!pathbuf) {
+				goto out_fdput_open;
+			}
+
+			fullpath = d_path(&f.file->f_path, pathbuf, PAGE_SIZE);
+			if (IS_ERR(fullpath)) {
+				goto out_free_open;
+			}
+
+			if (!strncmp("/proc/tofu/dev/", fullpath, 15)) {
+				res->pde_data = PDE_DATA(file_inode(f.file));
+				dprintk("%s: fd: %d, path: %s, PDE_DATA: 0x%lx\n",
+						__func__,
+						fd,
+						fullpath,
+						(unsigned long)res->pde_data);
+				dprintk("%s: pgd_index: %ld, pmd_index: %ld, pte_index: %ld\n",
+						__func__,
+						pgd_index((unsigned long)res->pde_data),
+						pmd_index((unsigned long)res->pde_data),
+						pte_index((unsigned long)res->pde_data));
+				dprintk("MAX_USER_VA_BITS: %d, PGDIR_SHIFT: %d\n",
+						MAX_USER_VA_BITS, PGDIR_SHIFT);
+				mcctrl_file_to_pidfd_hash_insert(f.file, os,
+						task_tgid_vnr(current),
+						current->group_leader, fd,
+						fullpath, res->pde_data);
+			}
+
+out_free_open:
+			free_page((unsigned long)pathbuf);
+out_fdput_open:
+			fdput(f);
 		}
 
-		pathbuf = kmalloc(PATH_MAX, GFP_ATOMIC);
+		/* Ioctl on Tofu CQ? */
-		if (!pathbuf) {
+		else if (packet->req.number == __NR_ioctl &&
-			goto out_fdput;
+				packet->req.args[0] > 0 && ret == 0) {
-		}
+			struct fd f;
+			int fd;
+			int tni, cq;
+			long __ret;
 
-		fullpath = d_path(&f.file->f_path, pathbuf, PATH_MAX);
+			fd = packet->req.args[0];
-		if (IS_ERR(fullpath)) {
+			f = fdget(fd);
-			goto out_free;
-		}
 
-		if (!strncmp("/proc/tofu/dev/", fullpath, 15)) {
+			if (!f.file) {
-			res->pde_data = PDE_DATA(file_inode(f.file));
+				goto out_notify;
-			dprintk("%s: fd: %d, path: %s, PDE_DATA: 0x%lx\n",
+			}
-					__func__,
-					fd,
-					fullpath,
-					(unsigned long)res->pde_data);
-			dprintk("%s: pgd_index: %ld, pmd_index: %ld, pte_index: %ld\n",
-				__func__,
-				pgd_index((unsigned long)res->pde_data),
-				pmd_index((unsigned long)res->pde_data),
-				pte_index((unsigned long)res->pde_data));
-#ifdef CONFIG_ARM64
-			dprintk("CONFIG_ARM64_VA_BITS: %d, PGDIR_SHIFT: %d\n",
-				CONFIG_ARM64_VA_BITS, PGDIR_SHIFT);
-#endif
-			mcctrl_file_to_pidfd_hash_insert(f.file, os,
-					task_tgid_vnr(current),
-					current->group_leader, fd);
-		}
 
-out_free:
+			pathbuf = (char *)__get_free_page(GFP_ATOMIC);
-		kfree(pathbuf);
+			if (!pathbuf) {
-out_fdput:
+				goto out_fdput_ioctl;
-		fdput(f);
+			}
+
+			fullpath = d_path(&f.file->f_path, pathbuf, PAGE_SIZE);
+			if (IS_ERR(fullpath)) {
+				goto out_free_ioctl;
+			}
+
+			/* Looking for /proc/tofu/dev/tniXcqY pattern */
+			__ret = sscanf(fullpath, "/proc/tofu/dev/tni%dcq%d", &tni, &cq);
+			if (__ret == 2) {
+				extern long __mcctrl_tof_utofu_unlocked_ioctl_cq(void *pde_data,
+						unsigned int cmd, unsigned long arg);
+
+				dprintk("%s: ioctl(): fd: %d, path: %s\n",
+						__func__,
+						fd,
+						fullpath);
+
+				__ret = __mcctrl_tof_utofu_unlocked_ioctl_cq(
+						PDE_DATA(file_inode(f.file)),
+						packet->req.args[1], packet->req.args[2]);
+			}
+
+out_free_ioctl:
+			free_page((unsigned long)pathbuf);
+out_fdput_ioctl:
+			fdput(f);
+		}
 	}
 
 out_notify:
@@ -2359,7 +2420,7 @@ int __do_in_kernel_irq_syscall(ihk_os_t os, struct ikc_scd_packet *packet)
 	if (ret == -ENOSYS)
 		return -ENOSYS;
 
-	__return_syscall(os, packet, ret, 0);
+	__return_syscall(os, NULL, packet, ret, 0);
 
 	return 0;
 }
@@ -2584,7 +2645,7 @@ sched_setparam_out:
 		break;
 	}
 
-	__return_syscall(os, packet, ret, 0);
+	__return_syscall(os, NULL, packet, ret, 0);
 
 out_no_syscall_return:
 	ihk_ikc_release_packet((struct ihk_ikc_free_packet *)packet);

executer/user/eclair.c

@@ -1179,7 +1179,7 @@ static int start_gdb(void) {
 
 		sprintf(buf, "target remote :%d", ntohs(sin.sin_port));
 		execlp("gdb", "eclair", "-q", "-ex", "set prompt (eclair) ",
-				"-ex", buf, opt.kernel_path, NULL);
+				"-ex", buf, opt.kernel_path, "-ex", "set pagination off", NULL);
 		perror("execlp");
 		return 3;
 	}

kernel/cls.c

@@ -58,16 +58,43 @@ struct cpu_local_var *get_cpu_local_var(int id)
 	return clv + id;
 }
 
+#ifdef ENABLE_FUGAKU_HACKS
+void __show_context_stack(struct thread *thread,
+        unsigned long pc, uintptr_t sp, int kprintf_locked);
+#endif
 void preempt_enable(void)
 {
+#ifndef ENABLE_FUGAKU_HACKS
 	if (cpu_local_var_initialized)
 		--cpu_local_var(no_preempt);
+#else
+	if (cpu_local_var_initialized) {
+		--cpu_local_var(no_preempt);
+
+		if (cpu_local_var(no_preempt) < 0) {
+			//cpu_disable_interrupt();
+
+	__kprintf("%s: %d\n", __func__, cpu_local_var(no_preempt));
+    __kprintf("TID: %d, call stack from builtin frame (most recent first):\n",
+        cpu_local_var(current)->tid);
+	__show_context_stack(cpu_local_var(current), (uintptr_t)&preempt_enable,
+			(unsigned long)__builtin_frame_address(0), 1);
+
+			//arch_cpu_stop();
+			//cpu_halt();
+#ifdef ENABLE_FUGAKU_HACKS
+		panic("panic: negative preemption??");
+#endif
+		}
+	}
+#endif
 }
 
 void preempt_disable(void)
 {
-	if (cpu_local_var_initialized)
+	if (cpu_local_var_initialized) {
 		++cpu_local_var(no_preempt);
+	}
 }
 
 int add_backlog(int (*func)(void *arg), void *arg)
@@ -120,3 +147,10 @@ void do_backlog(void)
 		}
 	}
 }
+
+#ifdef ENABLE_FUGAKU_HACKS
+ihk_spinlock_t *get_this_cpu_runq_lock(void)
+{
+	return &get_this_cpu_local_var()->runq_lock;
+}
+#endif

kernel/host.c

@@ -788,7 +788,11 @@ out_remote_pf:
 		syscall_channel_send(resp_channel, &pckt);
 
 		rc = do_kill(NULL, info.pid, info.tid, info.sig, &info.info, 0);
+#ifndef ENABLE_FUGAKU_HACKS
 		dkprintf("SCD_MSG_SEND_SIGNAL: do_kill(pid=%d, tid=%d, sig=%d)=%d\n", info.pid, info.tid, info.sig, rc);
+#else
+		kprintf("SCD_MSG_SEND_SIGNAL: do_kill(pid=%d, tid=%d, sig=%d)=%d\n", info.pid, info.tid, info.sig, rc);
+#endif
 		ret = 0;
 		break;
 

kernel/hugefileobj.c

@@ -85,7 +85,11 @@ static int hugefileobj_get_page(struct memobj *memobj, off_t off,
 		}
 
 		memset(obj->pages[pgind], 0, obj->pgsize);
+#ifndef ENABLE_FUGAKU_HACKS
 		dkprintf("%s: obj: 0x%lx, allocated page for off: %lu"
+#else
+		kprintf("%s: obj: 0x%lx, allocated page for off: %lu"
+#endif
 				" (ind: %d), page size: %lu\n",
 				__func__, obj, off, pgind, obj->pgsize);
 	}
@@ -274,13 +278,51 @@ int hugefileobj_create(struct memobj *memobj, size_t len, off_t off,
 
 		obj->nr_pages = nr_pages;
 		obj->pages = pages;
+#ifndef ENABLE_FUGAKU_HACKS
 		dkprintf("%s: obj: 0x%lx, VA: 0x%lx, page array allocated"
+#else
+		kprintf("%s: obj: 0x%lx, VA: 0x%lx, page array allocated"
+#endif
 				" for %d pages, pagesize: %lu\n",
 				__func__,
 				obj,
 				virt_addr,
 				nr_pages,
 				obj->pgsize);
+
+#ifdef ENABLE_FUGAKU_HACKS
+		if (!hugetlbfs_on_demand) {
+			int pgind;
+			int npages;
+
+#ifndef ENABLE_FUGAKU_HACKS
+			for (pgind = 0; pgind < obj->nr_pages; ++pgind) {
+#else
+			/* Map in only the last 8 pages */
+			for (pgind = ((obj->nr_pages > 8) ? (obj->nr_pages - 8) : 0);
+					pgind < obj->nr_pages; ++pgind) {
+#endif
+				if (obj->pages[pgind]) {
+					continue;
+				}
+
+				npages = obj->pgsize >> PAGE_SHIFT;
+				obj->pages[pgind] = ihk_mc_alloc_aligned_pages_user(npages,
+						obj->pgshift - PTL1_SHIFT,
+						IHK_MC_AP_NOWAIT | IHK_MC_AP_USER, 0);
+				if (!obj->pages[pgind]) {
+					kprintf("%s: error: could not allocate page for off: %lu"
+							", page size: %lu\n", __func__, off, obj->pgsize);
+					continue;
+				}
+
+				memset(obj->pages[pgind], 0, obj->pgsize);
+				dkprintf("%s: obj: 0x%lx, pre-allocated page for off: %lu"
+						" (ind: %d), page size: %lu\n",
+						__func__, obj, off, pgind, obj->pgsize);
+			}
+		}
+#endif
 	}
 
 	obj->memobj.size = len;

kernel/include/cls.h

@@ -106,6 +106,9 @@ struct cpu_local_var {
 	ihk_spinlock_t migq_lock;
 	struct list_head migq;
 	int in_interrupt;
+#ifdef ENABLE_FUGAKU_HACKS
+	int in_page_fault;
+#endif
 	int no_preempt;
 	int timer_enabled;
 	unsigned long nr_ctx_switches;

kernel/include/page.h

@@ -69,4 +69,7 @@ static inline int page_is_multi_mapped(struct page *page)
 
 /* Should we take page faults on ANONYMOUS mappings? */
 extern int anon_on_demand;
+#ifdef ENABLE_FUGAKU_HACKS
+extern int hugetlbfs_on_demand;
+#endif
 #endif

kernel/include/process.h

@@ -395,6 +395,9 @@ struct vm_range {
 	off_t objoff;
 	int pgshift;	/* page size. 0 means THP */
 	int padding;
+#ifdef ENABLE_TOFU
+	struct list_head tofu_stag_list;
+#endif
 	void *private_data;
 };
 
@@ -764,6 +767,9 @@ struct thread {
 };
 
 #define VM_RANGE_CACHE_SIZE	4
+#ifdef ENABLE_TOFU
+#define TOFU_STAG_HASH_SIZE 4
+#endif
 
 struct process_vm {
 	struct address_space *address_space;
@@ -796,6 +802,12 @@ struct process_vm {
 	struct vm_range *range_cache[VM_RANGE_CACHE_SIZE];
 	int range_cache_ind;
 	struct swapinfo *swapinfo;
+
+#ifdef ENABLE_TOFU
+	/* Tofu STAG hash */
+	ihk_spinlock_t tofu_stag_lock;
+	struct list_head tofu_stag_hash[TOFU_STAG_HASH_SIZE];
+#endif
 };
 
 static inline int has_cap_ipc_lock(struct thread *th)

kernel/include/profile.h

@@ -1,9 +1,6 @@
 #ifndef __PROCESS_PROFILE_H_
 #define __PROCESS_PROFILE_H_
 
-/* Uncomment this to enable profiling */
-#define PROFILE_ENABLE
-
 #ifdef PROFILE_ENABLE
 #define PROFILE_SYSCALL_MAX                          2000
 #define PROFILE_OFFLOAD_MAX   (PROFILE_SYSCALL_MAX << 1)
@@ -58,8 +55,6 @@ enum profile_event_type {
 	PROFILE_EVENT_MAX	/* Should be the last event type */
 };
 
-#define __NR_profile                   PROFILE_EVENT_MAX
-
 #ifdef __KERNEL__
 struct thread;
 struct process;
@@ -79,6 +74,8 @@ void profile_dealloc_proc_events(struct process *proc);
 #include <unistd.h>
 #include <sys/syscall.h>
 
+#define __NR_profile                   PROFILE_EVENT_MAX
+
 /* Per-thread */
 static inline void mckernel_profile_thread_on(void)
 {

kernel/include/syscall.h

@@ -20,6 +20,7 @@
 #include <ihk/ikc.h>
 #include <rlimit.h>
 #include <time.h>
+#include <profile.h>
 
 #define NUM_SYSCALLS 255
 

kernel/include/tofu/generate_headers.sh

@@ -6,10 +6,25 @@ CURRENT_DIR=`pwd`
 
 cd ${SCRIPT_DIR}
 
-DWARF_TOOL=~/src/mckernel-apollo+a64fx/mckernel/tools/dwarf-extract-struct/dwarf-extract-struct
+DWARF_TOOL=${SCRIPT_DIR}/../../../tools/dwarf-extract-struct/dwarf-extract-struct
+if [ ! -x ${DWARF_TOOL} ]; then
+	echo "error: couldn't find DWARF extractor executable (${DWARF_TOOL}), have you compiled it?"
+	cd -
+	exit 1
+fi
+
+echo "Looking for Tofu driver debug symbols..."
+if [ "`find /lib/modules/ -name "tof_module.tar.gz" | xargs -r ls -t | head -n 1 | wc -l`" == "0" ]; then
+	echo "error: couldn't find Tofu modules with debug symbols"
+	cd -
+	exit 1
+fi
+
+MODULE_TAR_GZ=`find /lib/modules/ -name "tof_module.tar.gz" | xargs ls -t | head -n 1`
+echo "Using Tofu driver debug symbols: ${MODULE_TAR_GZ}"
 
 KMODULE=tof_utofu.ko 
-if ! tar zxvf /lib/modules/`uname -r`+debug/extra/tof_module.tar.gz ${KMODULE} 2>&1 > /dev/null; then
+if ! tar zxvf ${MODULE_TAR_GZ} ${KMODULE} 2>&1 > /dev/null; then
 	echo "error: uncompressing kernel module with debug symbols"
 	cd -
 	exit 1
@@ -22,7 +37,7 @@ ${DWARF_TOOL} ${KMODULE} tof_utofu_bg common tni bgid bch | sed "s/struct FILL_I
 rm ${KMODULE}
 
 KMODULE=tof_core.ko 
-if ! tar zxvf /lib/modules/`uname -r`+debug/extra/tof_module.tar.gz ${KMODULE} 2>&1 > /dev/null; then
+if ! tar zxvf ${MODULE_TAR_GZ} ${KMODULE} 2>&1 > /dev/null; then
 	echo "error: uncompressing kernel module with debug symbols"
 	cd -
 	exit 1
@@ -33,4 +48,4 @@ ${DWARF_TOOL} ${KMODULE} tof_core_bg lock reg irq subnet gpid sighandler | sed "
 rm ${KMODULE}
 
 #cat tofu_generated*.h
-cd -
+cd - > /dev/null

kernel/mem.c

@@ -63,6 +63,9 @@ extern int interrupt_from_user(void *);
 struct tlb_flush_entry tlb_flush_vector[IHK_TLB_FLUSH_IRQ_VECTOR_SIZE];
 
 int anon_on_demand = 0;
+#ifdef ENABLE_FUGAKU_HACKS
+int hugetlbfs_on_demand;
+#endif
 int sysctl_overcommit_memory = OVERCOMMIT_ALWAYS;
 
 static struct ihk_mc_pa_ops *pa_ops;
@@ -744,7 +747,11 @@ distance_based:
 			}
 			else {
 			if (i == 0)
+#ifndef ENABLE_FUGAKU_HACKS
 				kprintf("%s: distance: CPU @ node %d failed to allocate "
+#else
+				dkprintf("%s: distance: CPU @ node %d failed to allocate "
+#endif
 						"%d pages from node %d\n",
 						__FUNCTION__,
 						ihk_mc_get_numa_id(),
@@ -951,6 +958,9 @@ static void query_free_mem_interrupt_handler(void *priv)
 	}
 
 	kprintf("McKernel free pages in total: %d\n", pages);
+#ifdef ENABLE_FUGAKU_HACKS
+	panic("PANIC");
+#endif
 
 	if (find_command_line("memdebug")) {
 		extern void kmalloc_memcheck(void);
@@ -1286,6 +1296,9 @@ void tlb_flush_handler(int vector)
 	}
 #endif // PROFILE_ENABLE
 }
+#ifdef ENABLE_FUGAKU_HACKS
+extern unsigned long arch_get_instruction_address(const void *reg);
+#endif
 
 static void unhandled_page_fault(struct thread *thread, void *fault_addr,
 				 uint64_t reason, void *regs)
@@ -1317,6 +1330,22 @@ static void unhandled_page_fault(struct thread *thread, void *fault_addr,
 		__kprintf("address is out of range!\n");
 	}
 
+#ifdef ENABLE_FUGAKU_HACKS
+	{
+		unsigned long pc = arch_get_instruction_address(regs);
+		range = lookup_process_memory_range(vm, pc, pc + 1);
+		if (range) {
+			__kprintf("PC: 0x%lx (%lx in %s)\n",
+					pc,
+					(range->memobj && range->memobj->flags & MF_REG_FILE) ?
+					pc - range->start + range->objoff :
+					pc - range->start,
+					(range->memobj && range->memobj->path) ?
+						range->memobj->path : "(unknown)");
+		}
+	}
+#endif
+
 	kprintf_unlock(irqflags);
 
 	/* TODO */
@@ -1324,7 +1353,13 @@ static void unhandled_page_fault(struct thread *thread, void *fault_addr,
 
 	if (!(reason & PF_USER)) {
 		cpu_local_var(kernel_mode_pf_regs) = regs;
+#ifndef ENABLE_FUGAKU_HACKS
 		panic("panic: kernel mode PF");
+#else
+		kprintf("panic: kernel mode PF");
+		for (;;) cpu_pause();
+		//panic("panic: kernel mode PF");
+#endif
 	}
 
 	//dkprintf("now dump a core file\n");
@@ -1360,6 +1395,20 @@ static void page_fault_handler(void *fault_addr, uint64_t reason, void *regs)
 			__FUNCTION__, fault_addr, reason, regs);
 
 	preempt_disable();
+#ifdef ENABLE_FUGAKU_HACKS
+	++cpu_local_var(in_page_fault);
+	if (cpu_local_var(in_page_fault) > 1) {
+		kprintf("%s: PF in PF??\n", __func__);
+		cpu_disable_interrupt();
+		if (!(reason & PF_USER)) {
+			cpu_local_var(kernel_mode_pf_regs) = regs;
+			panic("panic: kernel mode PF in PF");
+		}
+		while (1) {
+			panic("PANIC");
+		}
+	}
+#endif
 
 	cpu_enable_interrupt();
 
@@ -1427,6 +1476,13 @@ out_linux:
 			reason, error);
 		unhandled_page_fault(thread, fault_addr, reason, regs);
 		preempt_enable();
+
+#ifdef ENABLE_FUGAKU_DEBUG
+		kprintf("%s: sending SIGSTOP to TID: %d\n", __func__, thread->tid);
+		do_kill(thread, thread->proc->pid, thread->tid, SIGSTOP, NULL, 0);
+		goto out;
+#endif
+
 		memset(&info, '\0', sizeof info);
 		if (error == -ERANGE) {
 			info.si_signo = SIGBUS;
@@ -1455,6 +1511,9 @@ out_linux:
 out_ok:
 #endif
 	error = 0;
+#ifdef ENABLE_FUGAKU_HACKS
+	--cpu_local_var(in_page_fault);
+#endif
 	preempt_enable();
 out:
 	dkprintf("%s: addr: %p, reason: %lx, regs: %p -> error: %d\n",
@@ -2041,6 +2100,13 @@ void mem_init(void)
 		anon_on_demand = 1;
 	}
 	
+#ifdef ENABLE_FUGAKU_HACKS
+	if (find_command_line("hugetlbfs_on_demand")) {
+		kprintf("Demand paging on hugetlbfs mappings enabled.\n");
+		hugetlbfs_on_demand = 1;
+	}
+#endif
+
 	/* Init distance vectors */
 	numa_distances_init();
 }

kernel/process.c

@@ -36,6 +36,9 @@
 #include <rusage_private.h>
 #include <ihk/monitor.h>
 #include <ihk/debug.h>
+#ifdef ENABLE_TOFU
+#include <tofu/tofu_stag_range.h>
+#endif
 
 //#define DEBUG_PRINT_PROCESS
 
@@ -269,6 +272,12 @@ init_process_vm(struct process *owner, struct address_space *asp, struct process
 	}
 	vm->range_cache_ind = 0;
 
+#ifdef ENABLE_TOFU
+	ihk_mc_spinlock_init(&vm->tofu_stag_lock);
+	for (i = 0; i < TOFU_STAG_HASH_SIZE; ++i) {
+		INIT_LIST_HEAD(&vm->tofu_stag_hash[i]);
+	}
+#endif
 	return 0;
 }
 
@@ -824,6 +833,8 @@ static int copy_user_ranges(struct process_vm *vm, struct process_vm *orgvm)
 		range->objoff = src_range->objoff;
 		range->pgshift = src_range->pgshift;
 		range->private_data = src_range->private_data;
+		range->straight_start = src_range->straight_start;
+
 		if (range->memobj) {
 			memobj_ref(range->memobj);
 		}
@@ -955,6 +966,21 @@ int split_process_memory_range(struct process_vm *vm, struct vm_range *range,
 	newrange->pgshift = range->pgshift;
 	newrange->private_data = range->private_data;
 
+#ifdef ENABLE_TOFU
+	INIT_LIST_HEAD(&newrange->tofu_stag_list);
+	{
+		extern int tofu_stag_split_vm_range_on_addr(struct process_vm *vm,
+				struct vm_range *range_low, struct vm_range *range_high,
+				uintptr_t addr);
+
+		int moved =
+			tofu_stag_split_vm_range_on_addr(vm, range, newrange, addr);
+		if (moved > 0) {
+			kprintf("%s: moved %d stag ranges\n", __func__, moved);
+		}
+	}
+#endif
+
 	if (range->memobj) {
 		memobj_ref(range->memobj);
 		newrange->memobj = range->memobj;
@@ -1023,6 +1049,28 @@ int join_process_memory_range(struct process_vm *vm,
 		if (vm->range_cache[i] == merging)
 			vm->range_cache[i] = surviving;
 	}
+
+#ifdef ENABLE_TOFU
+	/* Move Tofu stag range entries */
+	if (vm->proc->enable_tofu) {
+		struct tofu_stag_range *tsr, *next;
+
+		ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+		list_for_each_entry_safe(tsr, next,
+				&merging->tofu_stag_list, list) {
+
+			list_del(&tsr->list);
+			list_add_tail(&tsr->list, &surviving->tofu_stag_list);
+			dkprintf("%s: stag: %d @ %p:%lu moved in VM range merge\n",
+					__func__,
+					tsr->stag,
+					tsr->start,
+					(unsigned long)(tsr->end - tsr->start));
+		}
+		ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+	}
+#endif
+
 	kfree(merging);
 
 	error = 0;
@@ -1137,6 +1185,24 @@ static int free_process_memory_range(struct process_vm *vm,
 	}
 
 straight_out:
+
+#ifdef ENABLE_TOFU
+	if (vm->proc->enable_tofu) {
+		int entries;
+		extern int tofu_stag_range_remove_overlapping(struct process_vm *vm,
+				struct vm_range *range);
+
+		entries = tofu_stag_range_remove_overlapping(vm, range);
+		if (entries > 0) {
+			dkprintf("%s: removed %d Tofu stag entries for range 0x%lx:%lu\n",
+				__func__,
+				entries,
+				range->start,
+				range->end - range->start);
+		}
+	}
+#endif
+
 	rb_erase(&range->vm_rb_node, &vm->vm_range_tree);
 	for (i = 0; i < VM_RANGE_CACHE_SIZE; ++i) {
 		if (vm->range_cache[i] == range)
@@ -1428,6 +1494,9 @@ int add_process_memory_range(struct process_vm *vm,
 	range->pgshift = pgshift;
 	range->private_data = NULL;
 	range->straight_start = 0;
+#ifdef ENABLE_TOFU
+	INIT_LIST_HEAD(&range->tofu_stag_list);
+#endif
 
 	rc = 0;
 	if (phys == NOPHYS) {
@@ -2521,6 +2590,14 @@ int init_process_stack(struct thread *thread, struct program_load_desc *pn,
 			__FUNCTION__, size, minsz,
 			ap_flag ? "(IHK_MC_AP_USER)" : "");
 
+#ifdef ENABLE_FUGAKU_HACKS
+	/*
+	 * XXX: Fugaku: Fujitsu's runtime remaps the stack
+	 * using hugetlbfs so don't bother allocating too much here..
+	 */
+	minsz = 8*1024*1024;
+#endif
+
 	stack = ihk_mc_alloc_aligned_pages_user(minsz >> PAGE_SHIFT,
 						USER_STACK_PAGE_P2ALIGN,
 						IHK_MC_AP_NOWAIT | ap_flag,

kernel/syscall.c

@@ -204,6 +204,14 @@ long do_syscall(struct syscall_request *req, int cpu)
 		++thread->in_syscall_offload;
 	}
 
+#ifdef ENABLE_FUGAKU_HACKS
+#if 0
+	if (req->number == __NR_write && req->args[0] == 1) {
+		return req->args[2];
+	}
+#endif
+#endif
+
 	/* The current thread is the requester */
 	req->rtid = cpu_local_var(current)->tid;
 
@@ -1382,6 +1390,15 @@ void terminate(int rc, int sig)
 	mcs_rwlock_writer_unlock(&proc->threads_lock, &lock);
 
 	vm = proc->vm;
+
+#ifdef ENABLE_TOFU
+	if (proc->enable_tofu) {
+		extern void tof_utofu_finalize();
+
+		tof_utofu_finalize();
+	}
+#endif
+
 	free_all_process_memory_range(vm);
 
 	if (proc->saved_cmdline) {
@@ -1930,6 +1947,10 @@ do_mmap(const uintptr_t addr0, const size_t len0, const int prot,
 				__FUNCTION__, proc->straight_va, range->pgshift);
 
 		ptattr = arch_vrflag_to_ptattr(range->flag, PF_POPULATE, NULL);
+
+#ifdef ENABLE_FUGAKU_HACKS
+		if (1) { // Un-safe mapping of covering physical range
+#endif
 		error = ihk_mc_pt_set_range(proc->vm->address_space->page_table,
 				proc->vm,
 				(void *)range->start,
@@ -1956,6 +1977,90 @@ do_mmap(const uintptr_t addr0, const size_t len0, const int prot,
 				proc->straight_pa,
 				psize,
 				proc->straight_map_threshold);
+
+#ifdef ENABLE_FUGAKU_HACKS
+		}
+		else { // Safe mapping of only LWK memory ranges
+			size_t max_pgsize = 0;
+			size_t min_pgsize = 0xFFFFFFFFFFFFFFFF;
+
+			/*
+			 * Iterate LWK phsyical memory chunks and map them to their
+			 * corresponding offset in the straight range using the largest
+			 * suitable pages.
+			 */
+			for (i = 0; i < ihk_mc_get_nr_memory_chunks(); ++i) {
+				unsigned long start, end, pa;
+				void *va, *va_end;
+				size_t pgsize;
+				int pg2align;
+
+				ihk_mc_get_memory_chunk(i, &start, &end, NULL);
+				va = proc->straight_va + (start - straight_pa_start);
+				va_end = va + (end - start);
+				pa = start;
+
+				while (va < va_end) {
+					pgsize = (va_end - va) + 1;
+retry:
+					error = arch_get_smaller_page_size(NULL, pgsize,
+							&pgsize, &pg2align);
+					if (error) {
+						ekprintf("%s: arch_get_smaller_page_size() failed"
+								" during straight mapping: %d\n",
+								__func__, error);
+						proc->straight_va = 0;
+						goto straight_out;
+					}
+
+					/* Are virtual or physical not page aligned for this size? */
+					if (((unsigned long)va & (pgsize - 1)) ||
+							(pa & (pgsize - 1))) {
+						goto retry;
+					}
+
+					error = ihk_mc_pt_set_range(
+							proc->vm->address_space->page_table,
+							proc->vm,
+							va,
+							va + pgsize,
+							pa,
+							ptattr,
+							pg2align + PAGE_SHIFT,
+							range,
+							0);
+
+					if (error) {
+						kprintf("%s: ihk_mc_pt_set_range() failed"
+								" during straight mapping: %d\n",
+								__func__, error);
+						proc->straight_va = 0;
+						goto straight_out;
+					}
+
+					if (pgsize > max_pgsize)
+						max_pgsize = pgsize;
+
+					if (pgsize < min_pgsize)
+						min_pgsize = pgsize;
+
+					va += pgsize;
+					pa += pgsize;
+				}
+			}
+
+			region->map_end = (unsigned long)proc->straight_va +
+				proc->straight_len;
+			proc->straight_pa = straight_pa_start;
+			kprintf("%s: straight mapping: 0x%lx:%lu @ "
+					"min_pgsize: %lu, max_pgsize: %lu\n",
+					__FUNCTION__,
+					proc->straight_va,
+					proc->straight_len,
+					min_pgsize,
+					max_pgsize);
+		}
+#endif
 	}
 straight_out:
 
@@ -2284,8 +2389,15 @@ straight_out:
 		range->straight_start =
 			(unsigned long)proc->straight_va +
 			(straight_phys - proc->straight_pa);
+#ifndef ENABLE_FUGAKU_HACKS
 		dkprintf("%s: range 0x%lx:%lu is straight starting at 0x%lx\n",
-				__FUNCTION__, addr, len, range->straight_start);
+			 __FUNCTION__, addr, len, range->straight_start);
+#else
+		dkprintf("%s: range 0x%lx:%lu is straight starting at 0x%lx"
+				" (phys: 0x%lx)\n",
+				__FUNCTION__, addr, len, range->straight_start,
+				straight_phys);
+#endif
 		if (!zero_at_free) {
 			memset((void *)phys_to_virt(straight_phys), 0, len);
 		}
@@ -2385,11 +2497,20 @@ out:
 	if (memobj) {
 		memobj_unref(memobj);
 	}
+
+#ifndef ENABLE_FUGAKU_HACKS
 	dkprintf("%s: 0x%lx:%8lu, (req: 0x%lx:%lu), prot: %x, flags: %x, "
+#else
+	if (cpu_local_var(current)->profile) {
+		kprintf("%s: 0x%lx:%8lu, (req: 0x%lx:%lu), prot: %x, flags: %x, "
+#endif
 			"fd: %d, off: %lu, error: %ld, addr: 0x%lx\n",
 			__FUNCTION__,
 			addr, len, addr0, len0, prot, flags,
 			fd, off0, error, addr);
+#ifdef ENABLE_FUGAKU_HACKS
+	}
+#endif
 
 	return !error ?
 		(range->straight_start ? range->straight_start : addr) :
@@ -2426,6 +2547,11 @@ SYSCALL_DECLARE(munmap)
 out:
 	dkprintf("[%d]sys_munmap(%lx,%lx): %d\n",
 			ihk_mc_get_processor_id(), addr, len0, error);
+#ifdef ENABLE_FUGAKU_HACKS
+	if (error) {
+		kprintf("%s: error: %d\n", __func__, error);
+	}
+#endif
 	return error;
 }
 

kernel/tofu/tof_utofu_main.c

@@ -24,6 +24,7 @@
 struct kmalloc_cache_header tofu_scatterlist_cache[8];
 struct kmalloc_cache_header tofu_mbpt_cache[8];
 struct ihk_mc_page_cache_header tofu_mbpt_sg_pages_cache[8];
+struct kmalloc_cache_header tofu_stag_range_cache[8];
 
 
 typedef ihk_spinlock_t spinlock_t;
@@ -43,6 +44,201 @@ typedef void (*tof_core_signal_handler)(int, int, uint64_t, uint64_t);
 #include <tofu/tofu_generated-tof_utofu_bg.h>
 #include <tofu/tofu_generated-tof_utofu_mbpt.h>
 
+#include <tofu/tofu_stag_range.h>
+
+/*
+ * Tofu STAG regions list keeps track of stags in a given VM range..
+ * Per-process tree is protected by process' vm_range_lock.
+ */
+int tof_utofu_stag_range_insert(struct process_vm *vm,
+		struct vm_range *range,
+		uintptr_t start, uintptr_t end,
+		struct tof_utofu_cq *ucq, int stag)
+{
+	struct tofu_stag_range *tsr; // = kmalloc(sizeof(*tsr), IHK_MC_AP_NOWAIT);
+	tsr = kmalloc_cache_alloc(&tofu_stag_range_cache[ihk_mc_get_numa_id()],
+			sizeof(*tsr));
+
+	if (!tsr) {
+		kprintf("%s: error: allocating tofu_stag_range\n", __func__);
+		return -ENOMEM;
+	}
+
+	tsr->start = start;
+	tsr->end = end;
+	tsr->ucq = ucq;
+	tsr->stag = stag;
+
+	ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+	list_add_tail(&tsr->list, &range->tofu_stag_list);
+	list_add_tail(&tsr->hash, &vm->tofu_stag_hash[stag % TOFU_STAG_HASH_SIZE]);
+	ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+
+	dkprintf("%s: stag: %d for TNI %d CQ %d @ %p:%lu\n",
+			__func__,
+			tsr->stag,
+			tsr->ucq->tni,
+			tsr->ucq->cqid,
+			tsr->start,
+			(unsigned long)(tsr->end - tsr->start));
+
+	return 0;
+}
+
+struct tofu_stag_range *tofu_stag_range_lookup_by_stag(struct process_vm *vm,
+	int stag)
+{
+	struct tofu_stag_range *tsr;
+	struct tofu_stag_range *match = NULL;
+
+	ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+	list_for_each_entry(tsr,
+			&vm->tofu_stag_hash[stag % TOFU_STAG_HASH_SIZE], hash) {
+		if (tsr->stag == stag) {
+			match = tsr;
+			break;
+		}
+	}
+	ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+
+	return match;
+}
+
+/* XXX: vm->tofu_stag_lock must be held */
+void __tofu_stag_range_remove(struct process_vm *vm, struct tofu_stag_range *tsr)
+{
+	dkprintf("%s: stag: %d for TNI %d CQ %d @ %p:%lu\n",
+			__func__,
+			tsr->stag,
+			tsr->ucq->tni,
+			tsr->ucq->cqid,
+			tsr->start,
+			(unsigned long)(tsr->end - tsr->start));
+
+	list_del(&tsr->list);
+	list_del(&tsr->hash);
+	//kfree(tsr);
+	kmalloc_cache_free(tsr);
+}
+
+void tofu_stag_range_remove(struct process_vm *vm, struct tofu_stag_range *tsr)
+{
+	ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+	__tofu_stag_range_remove(vm, tsr);
+	ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+}
+
+static int tof_utofu_free_stag(struct tof_utofu_cq *ucq, int stag);
+
+int tofu_stag_range_remove_overlapping(struct process_vm *vm,
+		struct vm_range *range)
+{
+	struct tofu_stag_range *tsr, *next;
+	int entries = 0;
+
+	ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+
+	list_for_each_entry_safe(tsr, next,
+			&range->tofu_stag_list, list) {
+
+		dkprintf("%s: stag: %d @ %p:%lu\n",
+				__func__,
+				tsr->stag,
+				tsr->start,
+				(unsigned long)(tsr->end - tsr->start));
+
+		linux_spin_lock(&tsr->ucq->trans.mru_lock);
+		tof_utofu_free_stag(tsr->ucq, tsr->stag);
+		linux_spin_unlock(&tsr->ucq->trans.mru_lock);
+
+		__tofu_stag_range_remove(vm, tsr);
+		++entries;
+	}
+
+	ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+
+	return entries;
+}
+
+void tofu_stag_range_remove_by_addr(struct process_vm *vm,
+	uintptr_t addr, size_t len)
+{
+	struct tofu_stag_range *tsr, *next;
+	int hash;
+
+	ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+	for (hash = 0; hash < TOFU_STAG_HASH_SIZE; ++hash) {
+		list_for_each_entry_safe(tsr, next,
+				&vm->tofu_stag_hash[hash], hash) {
+
+			if (tsr->start >= addr && tsr->end <= (addr + len)) {
+				linux_spin_lock(&tsr->ucq->trans.mru_lock);
+				tof_utofu_free_stag(tsr->ucq, tsr->stag);
+				linux_spin_unlock(&tsr->ucq->trans.mru_lock);
+
+				kprintf("%s: removed stag %d in %p:%lu\n",
+						__func__, tsr->stag, addr, len);
+				__tofu_stag_range_remove(vm, tsr);
+			}
+
+			{
+				uintptr_t max_start, min_end;
+
+				max_start = addr > tsr->start ? addr : tsr->start;
+				min_end = (addr + len) < tsr->end ? (addr + len) : tsr->end;
+
+				if ((tsr->start != 0 || vm->proc->status == PS_EXITED) &&
+						(max_start < min_end)) {
+					linux_spin_lock(&tsr->ucq->trans.mru_lock);
+					tof_utofu_free_stag(tsr->ucq, tsr->stag);
+					linux_spin_unlock(&tsr->ucq->trans.mru_lock);
+
+					kprintf("%s: removed stag %p:%lu (overlaps with range %p:%lu)\n",
+						__func__, tsr->start, (tsr->end - tsr->start), addr, len);
+					__tofu_stag_range_remove(vm, tsr);
+				}
+			}
+		}
+	}
+	ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+}
+
+int tofu_stag_split_vm_range_on_addr(struct process_vm *vm,
+		struct vm_range *range_low, struct vm_range *range_high,
+		uintptr_t addr)
+{
+	struct tofu_stag_range *tsr, *next;
+	int moved = 0;
+
+	ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+
+	list_for_each_entry_safe(tsr, next,
+			&range_low->tofu_stag_list, list) {
+
+		if (tsr->start >= addr) {
+			list_del(&tsr->list);
+			list_add_tail(&tsr->list, &range_high->tofu_stag_list);
+			++moved;
+
+			kprintf("%s: stag: %d @ %p:%lu moved to high range..\n",
+					__func__,
+					tsr->stag,
+					tsr->start,
+					(unsigned long)(tsr->end - tsr->start));
+		}
+
+		if (tsr->start < addr && tsr->end > addr) {
+			kprintf("%s: WARNING: VM range split in middle of stag range..\n", __func__);
+		}
+	}
+
+	ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+
+	return moved;
+}
+
+
+
 #define TOF_UTOFU_VERSION TOF_UAPI_VERSION
 #define TOF_UTOFU_NUM_STAG_NTYPES 3
 #define TOF_UTOFU_NUM_STAG_BITS(size) ((size) + 13)
@@ -146,6 +342,7 @@ static int tof_utofu_get_pagesize_locked(uintptr_t addr, size_t len,
 		}
 		else {
 			*_pgszbits = PTL2_SHIFT;
+			*_pgszbits = PTL1_CONT_SHIFT;
 		}
 		return 0;
 	}
@@ -171,7 +368,7 @@ static int tof_utofu_get_pagesize_locked(uintptr_t addr, size_t len,
 		}
 	}
 
-#if 0
+#if 1
 	/* Tofu only support 64kB and 2MB pages */
 	if (min_shift > PTL1_CONT_SHIFT)
 		min_shift = PTL1_CONT_SHIFT;
@@ -647,7 +844,6 @@ static int tof_utofu_update_mbpt_entries(struct tof_utofu_cq *ucq,
 	//struct page *page;
 	struct process *proc = cpu_local_var(current)->proc;
 	uintptr_t iova = 0, va;
-	int ret;
 	unsigned long phys = 0;
 
 	/* Special case for straight mapping */
@@ -697,6 +893,8 @@ static int tof_utofu_update_mbpt_entries(struct tof_utofu_cq *ucq,
 	}
 
 	for(va = start; va < end; va += pgsz, ix++){
+		size_t psize;
+		pte_t *ptep;
 
 		if (tof_utofu_mbpt_is_enabled(mbpt, ix)) {
 			/* this page is already mapped to mbpt */
@@ -715,15 +913,18 @@ static int tof_utofu_update_mbpt_entries(struct tof_utofu_cq *ucq,
 		//	return -ENOMEM;
 		//}
 
-		ret = ihk_mc_pt_virt_to_phys(
+		ptep = ihk_mc_pt_lookup_fault_pte(cpu_local_var(current)->vm,
-				cpu_local_var(current)->vm->address_space->page_table,
+				(void *)va, 0, NULL, &psize, NULL);
-				(void *)va, &phys);
 
-		if (ret) {
+		if (unlikely(!ptep || !pte_is_present(ptep))) {
-			raw_rc_output(ret);
+			kprintf("%s: ERROR: no valid PTE for 0x%lx\n",
+					__func__, va);
 			return -ENOMEM;
 		}
 
+		phys = (pte_get_phys(ptep) & ~(psize - 1)) +
+			(va & (psize - 1));
+
 		//iova = tof_smmu_get_ipa_cq(ucq->tni, ucq->cqid,
 		//			   pfn_to_kaddr(page_to_pfn(page)), pgsz);
 		//if (iova == 0) {
@@ -1012,6 +1213,7 @@ static int tof_utofu_ioctl_alloc_stag(struct tof_utofu_device *dev, unsigned lon
 	size_t pgsz;
 	int ret = -ENOTSUPP;
 	unsigned long irqflags;
+	struct vm_range *range = NULL;
 
 	ucq = container_of(dev, struct tof_utofu_cq, common);
 	if(!ucq->common.enabled){
@@ -1033,7 +1235,46 @@ static int tof_utofu_ioctl_alloc_stag(struct tof_utofu_device *dev, unsigned lon
 	}
 
 	readonly = (req.flags & 1) != 0;
+
 	ihk_rwspinlock_read_lock_noirq(&vm->memory_range_lock);
+
+	/* Assume smallest page size at first */
+	start = round_down((uintptr_t)req.va, PAGE_SIZE);
+	end = round_up((uintptr_t)req.va + req.len, PAGE_SIZE);
+
+	/* Find range, straight mapping special lookup */
+	if (vm->proc->straight_va &&
+			start >= (unsigned long)vm->proc->straight_va &&
+			end <= ((unsigned long)vm->proc->straight_va +
+				vm->proc->straight_len) &&
+			!(start == (unsigned long)vm->proc->straight_va &&
+				end == ((unsigned long)vm->proc->straight_va +
+					vm->proc->straight_len))) {
+		struct vm_range *range_iter;
+
+		range_iter = lookup_process_memory_range(vm, 0, -1);
+
+		while (range_iter) {
+			if (range_iter->straight_start &&
+					start >= range_iter->straight_start &&
+					start < (range_iter->straight_start +
+						(range_iter->end - range_iter->start))) {
+				range = range_iter;
+				break;
+			}
+
+			range_iter = next_process_memory_range(vm, range_iter);
+		}
+	}
+	else {
+		range = lookup_process_memory_range(vm, start, end);
+	}
+
+	if (!range) {
+		ret = -EINVAL;
+		goto unlock_out;
+	}
+
 	pgszbits = PAGE_SHIFT;
 	if (req.flags & TOF_UTOFU_ALLOC_STAG_LPG) {
 		ret = tof_utofu_get_pagesize_locked((uintptr_t)req.va,
@@ -1109,6 +1350,12 @@ static int tof_utofu_ioctl_alloc_stag(struct tof_utofu_device *dev, unsigned lon
 
 	//up(&ucq->ucq_sem);
 	ihk_mc_spinlock_unlock_noirq(&tofu_tni_cq_lock[ucq->tni][ucq->cqid]);
+
+	if (ret == 0) {
+		tof_utofu_stag_range_insert(vm, range, start, end, ucq, req.stag);
+	}
+
+unlock_out:
 	ihk_rwspinlock_read_unlock_noirq(&vm->memory_range_lock);
 
 	if(ret == 0){
@@ -1293,7 +1540,8 @@ static int tof_utofu_free_stag(struct tof_utofu_cq *ucq, int stag){
 #endif // PROFILE_ENABLE
 	kref_put(&ucq->trans.mru[stag].mbpt->kref, tof_utofu_mbpt_release);
 	ucq->trans.mru[stag].mbpt = NULL;
-	dkprintf("%s: stag: %d deallocated\n", __func__, stag);
+	dkprintf("%s: TNI: %d, CQ: %d, STAG: %d deallocated\n",
+			__func__, ucq->tni, ucq->cqid, stag);
 #ifdef PROFILE_ENABLE
 	profile_event_add(PROFILE_tofu_stag_free_stag_dealloc, rdtsc() - ts_rolling);
 	profile_event_add(PROFILE_tofu_stag_free_stag, rdtsc() - ts);
@@ -1316,7 +1564,7 @@ static int tof_utofu_ioctl_free_stags(struct tof_utofu_device *dev, unsigned lon
 		return -EFAULT;
 	}
 	//tof_log_if("[IN] tni=%d cqid=%d num=%u stags=%p\n", ucq->tni, ucq->cqid, req.num, req.stags);
-	dkprintf("%: [IN] tni=%d cqid=%d num=%u stags=%p\n",
+	dkprintf("%s: [IN] tni=%d cqid=%d num=%u stags=%p\n",
 			__func__, ucq->tni, ucq->cqid, req.num, req.stags);
 
 	if(req.num > 1024 || req.stags == NULL){
@@ -1332,6 +1580,21 @@ static int tof_utofu_ioctl_free_stags(struct tof_utofu_device *dev, unsigned lon
 		linux_spin_lock_irqsave(&ucq->trans.mru_lock, irqflags);
 		ret = tof_utofu_free_stag(ucq, stags[i]);
 		linux_spin_unlock_irqrestore(&ucq->trans.mru_lock, irqflags);
+
+		{
+			struct tofu_stag_range *tsr;
+
+			tsr = tofu_stag_range_lookup_by_stag(
+					cpu_local_var(current)->vm, stags[i]);
+
+			if (tsr) {
+				tofu_stag_range_remove(cpu_local_var(current)->vm, tsr);
+			}
+			else {
+				kprintf("%s: no stag range object for %d??\n", __func__, stags[i]);
+			}
+		}
+
 		if(ret == 0){
 			stags[i] = -1;
 		}
@@ -1374,9 +1637,9 @@ static int tof_utofu_ioctl_free_stags(struct tof_utofu_device *dev, unsigned lon
 void tof_utofu_release_cq(void *pde_data)
 {
 	struct tof_utofu_cq *ucq;
-	int stag;
 	struct tof_utofu_device *dev;
 	unsigned long irqflags;
+	int do_free = 1;
 
 	dev = (struct tof_utofu_device *)pde_data;
 	ucq = container_of(dev, struct tof_utofu_cq, common);
@@ -1384,13 +1647,49 @@ void tof_utofu_release_cq(void *pde_data)
 	if (!ucq->common.enabled) {
 		kprintf("%s: UCQ TNI %d, CQ %d is disabled\n",
 			__func__, ucq->tni, ucq->cqid);
-		return;
+		do_free = 0;
 	}
 
-	for (stag = 0; stag < TOF_UTOFU_NUM_STAG(ucq->num_stag); stag++) {
+	{
-		linux_spin_lock_irqsave(&ucq->trans.mru_lock, irqflags);
+		int i;
-		tof_utofu_free_stag(ucq, stag);
+		struct tofu_stag_range *tsr, *next;
-		linux_spin_unlock_irqrestore(&ucq->trans.mru_lock, irqflags);
+		struct process_vm *vm = cpu_local_var(current)->vm;
+
+		ihk_mc_spinlock_lock_noirq(&vm->tofu_stag_lock);
+		for (i = 0; i < TOFU_STAG_HASH_SIZE; ++i) {
+			list_for_each_entry_safe(tsr, next,
+					&vm->tofu_stag_hash[i], hash) {
+				if (tsr->ucq != ucq)
+					continue;
+
+				if (do_free) {
+					dkprintf("%s: removing stag %d for TNI %d CQ %d\n",
+							__func__, tsr->stag, ucq->tni, ucq->cqid);
+
+					linux_spin_lock_irqsave(&ucq->trans.mru_lock, irqflags);
+					tof_utofu_free_stag(tsr->ucq, tsr->stag);
+					linux_spin_unlock_irqrestore(&ucq->trans.mru_lock, irqflags);
+				}
+				else {
+					kprintf("%s: WARNING: could not free stag %d for TNI %d CQ %d (UCQ is disabled)\n",
+							__func__, tsr->stag, ucq->tni, ucq->cqid);
+				}
+
+				__tofu_stag_range_remove(vm, tsr);
+			}
+		}
+		ihk_mc_spinlock_unlock_noirq(&vm->tofu_stag_lock);
+	}
+
+	/* Loop through as well just to make sure everything is cleaned up */
+	if (do_free) {
+		int stag;
+
+		for (stag = 0; stag < TOF_UTOFU_NUM_STAG(ucq->num_stag); stag++) {
+			linux_spin_lock_irqsave(&ucq->trans.mru_lock, irqflags);
+			tof_utofu_free_stag(ucq, stag);
+			linux_spin_unlock_irqrestore(&ucq->trans.mru_lock, irqflags);
+		}
 	}
 
 	dkprintf("%s: UCQ (pde: %p) TNI %d, CQ %d\n",
@@ -1829,14 +2128,22 @@ static int tof_utofu_ioctl_enable_bch(struct tof_utofu_device *dev, unsigned lon
 	}
 
 	if (!phys) {
-		ret = ihk_mc_pt_virt_to_phys(vm->address_space->page_table,
+		size_t psize;
-				(void *)req.addr, &phys);
+		pte_t *ptep;
 
-		if (ret) {
+		ptep = ihk_mc_pt_lookup_fault_pte(cpu_local_var(current)->vm,
-			raw_rc_output(ret);
+				(void *)req.addr, 0, NULL, &psize, NULL);
+
+		if (unlikely(!ptep || !pte_is_present(ptep))) {
+			kprintf("%s: ERROR: no valid PTE for 0x%lx\n",
+					__func__, req.addr);
+			raw_rc_output(-ENOMEM);
 			ihk_rwspinlock_read_unlock_noirq(&vm->memory_range_lock);
 			return -ENOMEM;
 		}
+
+		phys = (pte_get_phys(ptep) & ~(psize - 1)) +
+			((uint64_t)req.addr & (psize - 1));
 	}
 
 	ihk_rwspinlock_read_unlock_noirq(&vm->memory_range_lock);
@@ -2068,6 +2375,7 @@ void tof_utofu_init_globals(void)
 	memset(tofu_scatterlist_cache, 0, sizeof(tofu_scatterlist_cache));
 	memset(tofu_mbpt_cache, 0, sizeof(tofu_mbpt_cache));
 	memset(tofu_mbpt_sg_pages_cache, 0, sizeof(tofu_mbpt_sg_pages_cache));
+	memset(tofu_stag_range_cache, 0, sizeof(tofu_stag_range_cache));
 
 	{
 		int tni, cq;
@@ -2120,6 +2428,25 @@ void tof_utofu_finalize(void)
 {
 	struct tofu_globals *tg = ihk_mc_get_tofu_globals();
 
+	/* Could be called from idle.. */
+	if (cpu_local_var(current)->proc->enable_tofu) {
+		int i;
+		struct process_vm *vm = cpu_local_var(current)->vm;
+		struct tofu_stag_range *tsr, *next;
+
+		for (i = 0; i < TOFU_STAG_HASH_SIZE; ++i) {
+			list_for_each_entry_safe(tsr, next,
+					&vm->tofu_stag_hash[i], hash) {
+
+				dkprintf("%s: WARNING: stray stag %d (%p:%lu) for TNI %d CQ %d?\n",
+						__func__, tsr->stag,
+						tsr->start, tsr->end - tsr->start,
+						tsr->ucq->tni, tsr->ucq->cqid);
+			}
+		}
+		kprintf("%s: STAG processing done\n", __func__);
+	}
+
 	ihk_mc_clear_kernel_range((void *)tg->linux_vmalloc_start,
 			(void *)tg->linux_vmalloc_end);
 }

lib/abort.c

@@ -22,12 +22,18 @@ void panic(const char *msg)
 
 	arch_print_stack();
 
+#ifndef ENABLE_FUGAKU_HACKS
 	/* do not assume anything after this is executed */
 	arch_cpu_stop();
 
 	while (1) {
 		cpu_halt();
 	}
+#else
+	while (1) {
+		cpu_halt_panic();
+	}
+#endif
 }
 
 extern void arch_show_interrupt_context(const void*);

lib/include/ihk/cpu.h

@@ -23,7 +23,13 @@ extern int num_processors;
 
 void cpu_enable_interrupt(void);
 void cpu_disable_interrupt(void);
+#ifdef ENABLE_FUGAKU_HACKS
+int cpu_interrupt_disabled(void);
+#endif
 void cpu_halt(void);
+#ifdef ENABLE_FUGAKU_HACKS
+void cpu_halt_panic(void);
+#endif
 void cpu_safe_halt(void);
 void cpu_restore_interrupt(unsigned long);
 void cpu_pause(void);

lib/page_alloc.c

@@ -332,6 +332,23 @@ int deferred_zero_at_free = 1;
  * of their corresponding memory (i.e., they are on the free memory chunk itself).
  */
 
+#ifdef ENABLE_FUGAKU_HACKS
+size_t __count_free_bytes(struct rb_root *root)
+{
+	struct free_chunk *chunk;
+	struct rb_node *node;
+	size_t size = 0;
+
+	for (node = rb_first(root); node; node = rb_next(node)) {
+		chunk = container_of(node, struct free_chunk, node);
+
+		size += chunk->size;
+	}
+
+	return size;
+}
+#endif
+
 /*
  * Free pages.
  * NOTE: locking must be managed by the caller.

scripts/mcreboot-lwk.sh.in

@@ -0,0 +1,99 @@
+#!/bin/bash
+# IHK/McKernel user priviledge reboot script.
+# author: Balazs Gerofi <bgerofi@riken.jp>
+#      Copyright (C) 2019 RIKEN
+#
+prefix="@prefix@"
+BINDIR="${prefix}/bin"
+SBINDIR="${prefix}/sbin"
+KERNDIR="@MCKERNELDIR@"
+
+mem=""
+cpus=""
+ikc_map=""
+
+while getopts c:m:r: OPT
+do
+	case ${OPT} in
+	c) cpus=${OPTARG}
+		;;
+	m) mem=${OPTARG}
+		;;
+	r) ikc_map=${OPTARG}
+		;;
+	\?) exit 1
+		;;
+	esac
+done
+
+if [ "${ikc_map}" == "" ]; then
+	# Query IKC map
+	if ! ${SBINDIR}/ihkosctl 0 get ikc_map > /dev/null; then
+		echo "error: querying IKC map" >&2
+		exit 1
+	fi
+
+	ikc_map=`${SBINDIR}/ihkosctl 0 get ikc_map`
+fi
+
+
+# Shutdown OS
+if ! ${SBINDIR}/ihkosctl 0 shutdown; then
+	echo "error: shuting down OS" >&2
+	exit 1
+fi
+
+sleep 2
+
+# Query IHK-SMP resources and reassign
+if ! ${SBINDIR}/ihkconfig 0 query cpu > /dev/null; then
+	echo "error: querying cpus" >&2
+	exit 1
+fi
+
+cpus=`${SBINDIR}/ihkconfig 0 query cpu`
+if [ "${cpus}" == "" ]; then
+	echo "error: querying CPUs" >&2
+	exit 1
+fi
+
+# Assign CPUs
+if ! ${SBINDIR}/ihkosctl 0 assign cpu ${cpus}; then
+	echo "error: assign CPUs" >&2
+	exit 1
+fi
+
+# Assign memory
+for i in `seq 0 15`; do
+	if ! ${SBINDIR}/ihkosctl 0 assign mem all@${i}; then
+		echo "error: assign memory" >&2
+		exit 1
+	fi
+done
+
+if [ "${ikc_map}" != "" ]; then
+	# Set IKC map
+	if ! ${SBINDIR}/ihkosctl 0 set ikc_map ${ikc_map}; then
+		echo "error: setting IKC map" >&2
+		exit 1
+	fi
+fi
+
+# Load kernel image
+if ! ${SBINDIR}/ihkosctl 0 load ${KERNDIR}/mckernel.img; then
+	echo "error: loading kernel image: ${KERNDIR}/mckernel.img" >&2
+	exit 1
+fi
+
+# Set kernel arguments
+if ! ${SBINDIR}/ihkosctl 0 kargs "hidos dump_level=24"; then
+	echo "error: setting kernel arguments" >&2
+	exit 1
+fi
+
+# Boot OS instance
+if ! ${SBINDIR}/ihkosctl 0 boot; then
+	echo "error: booting" >&2
+	exit 1
+fi
+

scripts/mcreboot-smp.sh.in

@@ -65,8 +65,9 @@ umask_old=`umask`
 idle_halt=""
 allow_oversubscribe=""
 time_sharing="time_sharing"
+force_reserve="no"
 
-while getopts stk:c:m:o:f:r:q:i:d:e:hOT: OPT
+while getopts stk:c:m:o:f:r:q:i:d:e:hROT: OPT
 do
 	case ${OPT} in
 	f)	facility=${OPTARG}
@@ -97,6 +98,8 @@ do
 		;;
 	O) allow_oversubscribe="allow_oversubscribe"
 		;;
+	R) force_reserve="yes"
+		;;
 	T)
 		case ${OPTARG} in
 		    1) time_sharing="time_sharing"
@@ -343,6 +346,17 @@ if ! grep ihk_smp_@ARCH@ /proc/modules &>/dev/null; then
 	fi
 fi
 
+if [ ${force_reserve} == "yes" ]; then
+	if ! ${SUDO} ${SBINDIR}/ihkconfig 0 reserve mem ${mem}; then
+		echo "error: reserving memory" >&2
+		error_exit "ihk_smp_loaded"
+	fi
+	if ! ${SUDO} ${SBINDIR}/ihkconfig 0 reserve cpu ${cpus}; then
+		echo "error: reserving CPUs" >&2;
+		error_exit "mem_reserved"
+	fi
+fi
+
 # Load mcctrl if not loaded
 if ! grep mcctrl /proc/modules &>/dev/null; then
 	if ! taskset -c 0 ${SUDO} insmod ${KMODDIR}/mcctrl.ko 2>/dev/null; then

scripts/mcstop+release-smp.sh.in

@@ -18,9 +18,10 @@ KERNDIR="@KERNDIR@"
 mem=""
 cpus=""
 kill_in_use=""
+dont_unload="no"
 
 RMMOD_PATH=/sbin/rmmod
-while getopts r:k OPT
+while getopts r:kR OPT
 do
 	case ${OPT} in
 	r)
@@ -29,6 +30,9 @@ do
 	k)
 		kill_in_use=1
 		;;
+	R)
+		dont_unload="yes"
+		;;
 	\?) exit 1
 		;;
 	esac
@@ -115,6 +119,10 @@ if ! sudo ${SBINDIR}/ihkconfig 0 release mem "all" > /dev/null; then
 	exit 1
 fi
 
+if [ "${dont_unload}" == "yes" ]; then
+	exit 0
+fi
+
 # Remove delegator if loaded
 if grep mcctrl /proc/modules &>/dev/null; then
 	if ! sudo ${RMMOD_PATH} mcctrl 2>/dev/null; then