diff --git a/arch/sw_64/Kconfig b/arch/sw_64/Kconfig
index d4e641b7388325f82dc2689b71c1d4b741af2023..3e3c44942936d53cf5245ed1a49973d23cee7161 100644
--- a/arch/sw_64/Kconfig
+++ b/arch/sw_64/Kconfig
@@ -2,42 +2,14 @@
 config SW64
 	bool
 	default y
-	select AUDIT_ARCH
-	select HAVE_IDE
-	select HAVE_OPROFILE
-	select HAVE_PCSPKR_PLATFORM
-	select HAVE_PERF_EVENTS
-	select HAVE_FAST_GUP
-	select GENERIC_CLOCKEVENTS
-	select GENERIC_IRQ_PROBE
-	select GENERIC_IRQ_LEGACY
-	select GENERIC_IRQ_SHOW
-	select ARCH_WANT_IPC_PARSE_VERSION
-	select ARCH_HAVE_NMI_SAFE_CMPXCHG
-	select ARCH_NO_PREEMPT
-	select ARCH_USE_CMPXCHG_LOCKREF
-	select GENERIC_SMP_IDLE_THREAD
-	select HAVE_MOD_ARCH_SPECIFIC
-	select MODULES_USE_ELF_RELA
-	select ARCH_SUPPORTS_NUMA_BALANCING
-	select HAVE_ARCH_TRANSPARENT_HUGEPAGE
-	select HAVE_ARCH_AUDITSYSCALL
-	select HAVE_ARCH_SECCOMP_FILTER
-	select OLD_SIGACTION
-	select OLD_SIGSUSPEND
-	select GENERIC_STRNCPY_FROM_USER
-	select GENERIC_STRNLEN_USER
-	select HAVE_ARCH_KGDB
+	select ACPI
+	select ACPI_REDUCED_HARDWARE_ONLY
+	select ARCH_HAS_ELF_RANDOMIZE
 	select ARCH_HAS_PHYS_TO_DMA
-	select SWIOTLB
-	select HAVE_MEMBLOCK
-	select HAVE_MEMBLOCK_NODE_MAP
-	select NO_BOOTMEM
-	select ARCH_USE_QUEUED_RWLOCKS
-	select ARCH_USE_QUEUED_SPINLOCKS
-	select COMMON_CLK
-	select HANDLE_DOMAIN_IRQ
 	select ARCH_HAS_STRICT_MODULE_RWX
+	select ARCH_HAS_PTE_SPECIAL
+	select ARCH_HAS_SG_CHAIN
+	select ARCH_HAVE_NMI_SAFE_CMPXCHG
 	select ARCH_INLINE_READ_LOCK
 	select ARCH_INLINE_READ_LOCK_BH
 	select ARCH_INLINE_READ_LOCK_IRQ
@@ -46,60 +18,88 @@ config SW64
 	select ARCH_INLINE_READ_UNLOCK_BH
 	select ARCH_INLINE_READ_UNLOCK_IRQ
 	select ARCH_INLINE_READ_UNLOCK_IRQRESTORE
-	select ARCH_INLINE_WRITE_LOCK
-	select ARCH_INLINE_WRITE_LOCK_BH
-	select ARCH_INLINE_WRITE_LOCK_IRQ
-	select ARCH_INLINE_WRITE_LOCK_IRQSAVE
-	select ARCH_INLINE_WRITE_UNLOCK
-	select ARCH_INLINE_WRITE_UNLOCK_BH
-	select ARCH_INLINE_WRITE_UNLOCK_IRQ
-	select ARCH_INLINE_WRITE_UNLOCK_IRQRESTORE
-	select ARCH_INLINE_SPIN_TRYLOCK
-	select ARCH_INLINE_SPIN_TRYLOCK_BH
 	select ARCH_INLINE_SPIN_LOCK
 	select ARCH_INLINE_SPIN_LOCK_BH
 	select ARCH_INLINE_SPIN_LOCK_IRQ
 	select ARCH_INLINE_SPIN_LOCK_IRQSAVE
+	select ARCH_INLINE_SPIN_TRYLOCK
+	select ARCH_INLINE_SPIN_TRYLOCK_BH
 	select ARCH_INLINE_SPIN_UNLOCK
 	select ARCH_INLINE_SPIN_UNLOCK_BH
 	select ARCH_INLINE_SPIN_UNLOCK_IRQ
 	select ARCH_INLINE_SPIN_UNLOCK_IRQRESTORE
+	select ARCH_INLINE_WRITE_LOCK
+	select ARCH_INLINE_WRITE_LOCK_BH
+	select ARCH_INLINE_WRITE_LOCK_IRQ
+	select ARCH_INLINE_WRITE_LOCK_IRQSAVE
+	select ARCH_INLINE_WRITE_UNLOCK
+	select ARCH_INLINE_WRITE_UNLOCK_BH
+	select ARCH_INLINE_WRITE_UNLOCK_IRQ
+	select ARCH_INLINE_WRITE_UNLOCK_IRQRESTORE
+	select ARCH_NO_PREEMPT
+	select ARCH_SUPPORTS_ACPI
 	select ARCH_SUPPORTS_ATOMIC_RMW
-	select ARCH_HAS_SG_CHAIN
-	select IRQ_FORCED_THREADING
+	select ARCH_SUPPORTS_NUMA_BALANCING
+	select ARCH_SUPPORTS_UPROBES
+	select ARCH_USE_CMPXCHG_LOCKREF
+	select ARCH_USE_QUEUED_RWLOCKS
+	select ARCH_USE_QUEUED_SPINLOCKS
+	select ARCH_WANT_FRAME_POINTERS
+	select ARCH_WANT_IPC_PARSE_VERSION
+	select AUDIT_ARCH
+	select COMMON_CLK
+	select DMA_OPS if PCI
+	select GENERIC_CLOCKEVENTS
+	select GENERIC_IRQ_LEGACY
 	select GENERIC_IRQ_MIGRATION if SMP
+	select GENERIC_IRQ_PROBE
+	select GENERIC_IRQ_SHOW
+	select GENERIC_PCI_IOMAP if PCI
+	select GENERIC_SMP_IDLE_THREAD
+	select GENERIC_STRNCPY_FROM_USER
+	select GENERIC_STRNLEN_USER
+	select GENERIC_TIME_VSYSCALL
+	select HANDLE_DOMAIN_IRQ
+	select HARDIRQS_SW_RESEND
+	select HAVE_ARCH_AUDITSYSCALL
+	select HAVE_ARCH_JUMP_LABEL
+	select HAVE_ARCH_KGDB
+	select HAVE_ARCH_SECCOMP_FILTER
 	select HAVE_ARCH_TRACEHOOK
-	select HAVE_FUNCTION_TRACER
+	select HAVE_ARCH_TRANSPARENT_HUGEPAGE
+	select HAVE_ASM_MODVERSIONS
+	select HAVE_C_RECORDMCOUNT
+	select HAVE_DEBUG_BUGVERBOSE
 	select HAVE_DYNAMIC_FTRACE
+	select HAVE_EBPF_JIT
+	select HAVE_FAST_GUP
 	select HAVE_FTRACE_MCOUNT_RECORD
-	select HAVE_C_RECORDMCOUNT
 	select HAVE_FUNCTION_GRAPH_TRACER
+	select HAVE_FUNCTION_TRACER
+	select HAVE_IDE
 	select HAVE_KPROBES
 	select HAVE_KRETPROBES
-	select HAVE_SYSCALL_TRACEPOINTS
-	select ARCH_SUPPORTS_UPROBES
-	select OF_EARLY_FLATTREE if OF
-	select HAVE_EBPF_JIT
-	select SPARSEMEM_EXTREME if SPARSEMEM
-	select HAVE_ARCH_JUMP_LABEL
-	select ARCH_WANT_FRAME_POINTERS
-	select HAVE_ASM_MODVERSIONS
-	select ARCH_HAS_ELF_RANDOMIZE
-	select HAVE_PERF_USER_STACK_DUMP
-	select HAVE_PERF_REGS
-	select ARCH_SUPPORTS_ACPI
-	select ACPI
-	select ACPI_REDUCED_HARDWARE_ONLY
-	select GENERIC_TIME_VSYSCALL
-	select SET_FS
+	select HAVE_MEMBLOCK
+	select HAVE_MEMBLOCK_NODE_MAP
+	select HAVE_MOD_ARCH_SPECIFIC
+	select HAVE_OPROFILE
 	select HAVE_PCI
-	select GENERIC_PCI_IOMAP if PCI
-	select PCI_MSI_ARCH_FALLBACKS
-	select DMA_OPS if PCI
+	select HAVE_PCSPKR_PLATFORM
+	select HAVE_PERF_EVENTS
+	select HAVE_PERF_REGS
+	select HAVE_PERF_USER_STACK_DUMP
 	select HAVE_REGS_AND_STACK_ACCESS_API
-	select ARCH_HAS_PTE_SPECIAL
-	select HARDIRQS_SW_RESEND
+	select HAVE_SYSCALL_TRACEPOINTS
+	select IRQ_FORCED_THREADING
 	select MEMORY_HOTPLUG_SPARSE if MEMORY_HOTPLUG
+	select MODULES_USE_ELF_RELA
+	select NO_BOOTMEM
+	select OF_EARLY_FLATTREE if OF
+	select OLD_SIGSUSPEND
+	select PCI_MSI_ARCH_FALLBACKS
+	select SET_FS
+	select SPARSEMEM_EXTREME if SPARSEMEM
+	select SWIOTLB
 
 config LOCKDEP_SUPPORT
 	def_bool y
@@ -144,6 +144,10 @@ config ARCH_HAS_ILOG2_U64
 config GENERIC_GPIO
 	bool
 
+config GENERIC_CALIBRATE_DELAY
+	bool
+	default y
+
 config ZONE_DMA32
 	bool
 	default y
@@ -873,6 +877,12 @@ config SW64_SUSPEND_DEEPSLEEP_BOOTCORE
 	bool "SW64 bootcore suspend into deep sleep mode"
 	default n
 
+config SW64_SUPPORT_S3_SLEEPING_STATE
+	depends on SUSPEND
+	bool "SW64 support S3 sleeping state"
+	default n
+	help
+	  Only SW831 support S3 sleep option and needs SROM, HMCode and BIOS support.
 
 source "drivers/cpuidle/Kconfig"
 
diff --git a/arch/sw_64/chip/chip3/chip.c b/arch/sw_64/chip/chip3/chip.c
index 8697891e39305bdc40a5349b21bcf8c8056fd7bb..d0b1c1c1c6df7ac3a95c1a78832477aee58e2c11 100644
--- a/arch/sw_64/chip/chip3/chip.c
+++ b/arch/sw_64/chip/chip3/chip.c
@@ -393,7 +393,6 @@ static void chip3_set_rc_piu(unsigned long node, unsigned long index)
 	/* set DMA offset value PCITODMA_OFFSET */
 	write_piu_ior0(node, index, EPDMABAR, PCITODMA_OFFSET);
 	if (IS_ENABLED(CONFIG_PCI_MSI)) {
-		write_piu_ior0(node, index, PIUCONFIG0, 0x38076);
 		write_piu_ior0(node, index, MSIADDR, MSIX_MSG_ADDR);
 		for (i = 0; i < 256; i++)
 			write_piu_ior0(node, index, MSICONFIG0 + (i << 7), 0);
@@ -494,6 +493,172 @@ static void chip3_device_interrupt(unsigned long irq_info)
 	}
 }
 
+static void chip3_i2c_srst(void)
+{
+	sw64_io_write(0, I2C0_SRST_L, 0x0);
+	sw64_io_write(0, I2C0_SRST_L, 0x1);
+
+	sw64_io_write(0, I2C1_SRST_L, 0x0);
+	sw64_io_write(0, I2C1_SRST_L, 0x1);
+
+	sw64_io_write(0, I2C2_SRST_L, 0x0);
+	sw64_io_write(0, I2C2_SRST_L, 0x1);
+}
+
+static void chip3_pcie_save(void)
+{
+	struct pci_controller *hose;
+	struct piu_saved *piu_save;
+	unsigned long node, index;
+	unsigned long i;
+
+	for (hose = hose_head; hose; hose = hose->next) {
+		piu_save = kzalloc(sizeof(*piu_save), GFP_KERNEL);
+
+		node = hose->node;
+		index = hose->index;
+		hose->sysdata = piu_save;
+
+		piu_save->piuconfig0 = read_piu_ior0(node, index, PIUCONFIG0);
+		piu_save->piuconfig1 = read_piu_ior1(node, index, PIUCONFIG1);
+		piu_save->epdmabar = read_piu_ior0(node, index, EPDMABAR);
+		piu_save->msiaddr = read_piu_ior0(node, index, MSIADDR);
+
+		for (i = 0; i < 256; i++) {
+			piu_save->msiconfig[i] = read_piu_ior0(node, index,
+					MSICONFIG0 + (i << 7));
+		}
+	}
+}
+
+static void chip3_pcie_restore(void)
+{
+	struct pci_controller *hose;
+	struct piu_saved *piu_save;
+	unsigned long node, index;
+	u32 rc_misc_ctrl;
+	unsigned int value;
+	unsigned long i;
+
+	for (hose = hose_head; hose; hose = hose->next) {
+		node = hose->node;
+		index = hose->index;
+		piu_save = hose->sysdata;
+
+		write_piu_ior0(node, index, PIUCONFIG0, piu_save->piuconfig0);
+		write_piu_ior1(node, index, PIUCONFIG1, piu_save->piuconfig1);
+		write_piu_ior0(node, index, EPDMABAR, piu_save->epdmabar);
+		write_piu_ior0(node, index, MSIADDR, piu_save->msiaddr);
+
+		for (i = 0; i < 256; i++) {
+			write_piu_ior0(node, index, MSICONFIG0 + (i << 7),
+					piu_save->msiconfig[i]);
+		}
+
+		/* Enable DBI_RO_WR_EN */
+		rc_misc_ctrl = read_rc_conf(node, index, RC_MISC_CONTROL_1);
+		write_rc_conf(node, index, RC_MISC_CONTROL_1, rc_misc_ctrl | 0x1);
+
+		/* Fix up DEVICE_ID_VENDOR_ID register */
+		value = (PCI_DEVICE_ID_CHIP3 << 16) | PCI_VENDOR_ID_JN;
+		write_rc_conf(node, index, RC_VENDOR_ID, value);
+
+		/* Set PCI-E root class code */
+		value = read_rc_conf(node, index, RC_REVISION_ID);
+		write_rc_conf(node, index, RC_REVISION_ID, (PCI_CLASS_BRIDGE_HOST << 16) | value);
+
+		/* Disable DBI_RO_WR_EN */
+		write_rc_conf(node, index, RC_MISC_CONTROL_1, rc_misc_ctrl);
+	}
+
+}
+
+static unsigned long saved_dvc_int, saved_long_time;
+
+static inline void chip3_intpu_save(void)
+{
+	saved_long_time = sw64_io_read(0, LONG_TIME);
+}
+
+static inline void chip3_intpu_restore(void)
+{
+	switch (cpu_desc.model) {
+	case CPU_SW831:
+		sw64_io_write(0, LONG_TIME, saved_long_time);
+		sw64_io_write(0, LONG_TIME_START_EN, 0x1);
+		break;
+	default:
+		pr_info("long time start is disable!");
+		break;
+	}
+}
+
+static inline void chip3_spbu_save(void)
+{
+	saved_dvc_int = sw64_io_read(0, MCU_DVC_INT_EN);
+}
+
+static inline void chip3_spbu_restore(void)
+{
+	chip3_i2c_srst();
+	sw64_io_write(0, MCU_DVC_INT_EN, saved_dvc_int);
+}
+
+#define BIOS_SECBIN	0x2F00000UL
+#define BIOS_SECSIZE	0x40000UL
+#define	BOUNCE_BUFFER	((1UL<<32) - BIOS_SECSIZE)
+#define	BIOS_MEMSAVE	((1UL<<32) - 2 * BIOS_SECSIZE)
+
+/*
+ * Due to specific architecture PCI MEM32 addressing, we reserve 512M memory
+ * size at PCI_32BIT_MEMIO (0xE000_0000) on SW64 platform.
+ *
+ * Since this memory region is still usable by OS, we implement a interface
+ * contract between BIOS and kernel:
+ *
+ * Firstly BIOS should back up SEC relative code segment to BIOS_MEMSAVE region
+ * with the length BIOS_SECSIZE in order to restore BIOS SEC phase binary during
+ * S3 sleep.
+ *
+ * Secondly kernel should use a bounce buffer to save memory region which may be
+ * overwritten by BIOS on resume from S3 sleep.
+ */
+static void chip3_mem_restore(void)
+{
+	void *dst, *src;
+	unsigned long size = BIOS_SECSIZE;
+
+	/* Firstly kernel back up to a bounce buffer */
+	src = __va(BIOS_SECBIN);
+	dst = __va(BOUNCE_BUFFER);
+	memcpy(dst, src, size);
+
+	/* Secondly restore BIOS SEC phase binary */
+	src = __va(BIOS_MEMSAVE);
+	dst = __va(BIOS_SECBIN);
+	memcpy(dst, src, size);
+}
+
+extern void cpld_write(uint8_t slave_addr, uint8_t reg, uint8_t data);
+
+static void chip3_suspend(bool wakeup)
+{
+
+	if (wakeup) {
+		chip3_pcie_restore();
+		chip3_intpu_restore();
+		chip3_spbu_restore();
+	} else {
+		/* Set S3 flag */
+		cpld_write(0x64, 0x34, 0x33);
+
+		chip3_spbu_save();
+		chip3_intpu_save();
+		chip3_pcie_save();
+		chip3_mem_restore();
+	}
+}
+
 static void chip3_hose_init(struct pci_controller *hose)
 {
 	unsigned long pci_io_base;
@@ -575,6 +740,7 @@ static struct sw64_chip_init_ops chip3_chip_init_ops = {
 
 static struct sw64_chip_ops chip3_chip_ops = {
 	.get_cpu_num = chip3_get_cpu_nums,
+	.suspend = chip3_suspend,
 	.fixup = chip3_ops_fixup,
 };
 
diff --git a/arch/sw_64/chip/chip3/cpufreq_debugfs.c b/arch/sw_64/chip/chip3/cpufreq_debugfs.c
index 13696360ef0294dd02cea16aa70cc31d2f6cb043..c58f1cee3907bd33b00a0c9ec49c664a5ff9e250 100644
--- a/arch/sw_64/chip/chip3/cpufreq_debugfs.c
+++ b/arch/sw_64/chip/chip3/cpufreq_debugfs.c
@@ -7,15 +7,15 @@
 #include <asm/sw64io.h>
 #include <asm/debug.h>
 
-#define	CLK_PRT		0x1UL
-#define	CORE_CLK0_V	(0x1UL << 1)
-#define	CORE_CLK0_R	(0x1UL << 2)
-#define	CORE_CLK2_V	(0x1UL << 15)
-#define	CORE_CLK2_R	(0x1UL << 16)
-
-#define	CLK_LV1_SEL_PRT		0x1UL
-#define	CLK_LV1_SEL_MUXA	(0x1UL << 2)
-#define	CLK_LV1_SEL_MUXB	(0x1UL << 3)
+#define CLK_PRT		0x1UL
+#define CORE_CLK0_V	(0x1UL << 1)
+#define CORE_CLK0_R	(0x1UL << 2)
+#define CORE_CLK2_V	(0x1UL << 15)
+#define CORE_CLK2_R	(0x1UL << 16)
+
+#define CLK_LV1_SEL_PRT		0x1UL
+#define CLK_LV1_SEL_MUXA	(0x1UL << 2)
+#define CLK_LV1_SEL_MUXB	(0x1UL << 3)
 
 #define CORE_PLL0_CFG_SHIFT	4
 #define CORE_PLL2_CFG_SHIFT	18
diff --git a/arch/sw_64/include/asm/asm-offsets.h b/arch/sw_64/include/asm/asm-offsets.h
index 72cd408a9c6fa6594b8a17b19cf7d6b73b582f6c..5ddfd96ccb793c8d1011bc1542968d29e9a68416 100644
--- a/arch/sw_64/include/asm/asm-offsets.h
+++ b/arch/sw_64/include/asm/asm-offsets.h
@@ -4,4 +4,4 @@
 
 #include <generated/asm-offsets.h>
 
-#endif
+#endif /* _ASM_SW64_ASM_OFFSETS_H */
diff --git a/arch/sw_64/include/asm/asm-prototypes.h b/arch/sw_64/include/asm/asm-prototypes.h
index 15bad8ef6883027ff7502a8e362e7b936dbc8d4b..67746d6bffb725b41d5997f983cded406e63573a 100644
--- a/arch/sw_64/include/asm/asm-prototypes.h
+++ b/arch/sw_64/include/asm/asm-prototypes.h
@@ -19,4 +19,4 @@ extern void __remlu(void);
 extern void __divwu(void);
 extern void __remwu(void);
 
-#endif
+#endif /* _ASM_SW64_ASM_PROTOTYPES_H */
diff --git a/arch/sw_64/include/asm/atomic.h b/arch/sw_64/include/asm/atomic.h
index 126417a1aeeeccf51434d3d2b7d69664ee48d888..66f4437be103766dbf81dd0542f85f5c91b44d40 100644
--- a/arch/sw_64/include/asm/atomic.h
+++ b/arch/sw_64/include/asm/atomic.h
@@ -336,7 +336,7 @@ ATOMIC_OPS(sub)
 #define atomic64_fetch_add_relaxed	atomic64_fetch_add_relaxed
 #define atomic64_fetch_sub_relaxed	atomic64_fetch_sub_relaxed
 
-#undef  ATOMIC_OPS
+#undef ATOMIC_OPS
 
 #define ATOMIC_OPS(op, asm)						\
 	ATOMIC_OP(op, asm)                                              \
diff --git a/arch/sw_64/include/asm/barrier.h b/arch/sw_64/include/asm/barrier.h
index c691038919cd181345cd63e248afd48b6e4a233a..5f4a03d700c68f667c62c17b45983f907779a3f7 100644
--- a/arch/sw_64/include/asm/barrier.h
+++ b/arch/sw_64/include/asm/barrier.h
@@ -21,4 +21,4 @@
 
 #include <asm-generic/barrier.h>
 
-#endif		/* _ASM_SW64_BARRIER_H */
+#endif /* _ASM_SW64_BARRIER_H */
diff --git a/arch/sw_64/include/asm/bugs.h b/arch/sw_64/include/asm/bugs.h
index c4a336fe04a2f66628e863aef87e9c5441c896c1..1cd94ed171fbc2c69523d87d37d19fecd37cc428 100644
--- a/arch/sw_64/include/asm/bugs.h
+++ b/arch/sw_64/include/asm/bugs.h
@@ -6,4 +6,4 @@ static void check_bugs(void)
 {
 }
 
-#endif
+#endif /* _ASM_SW64_BUGS_H */
diff --git a/arch/sw_64/include/asm/cache.h b/arch/sw_64/include/asm/cache.h
index 1dca2e2e04a4360c28b2a68fdccd6eac011001b9..fade2e095b8b60a4e7c087018eff19fa769f84ae 100644
--- a/arch/sw_64/include/asm/cache.h
+++ b/arch/sw_64/include/asm/cache.h
@@ -8,4 +8,4 @@
 #define L1_CACHE_SHIFT		7
 #define L1_CACHE_BYTES		(1 << L1_CACHE_SHIFT)
 
-#endif
+#endif /* _ASM_SW64_CACHE_H */
diff --git a/arch/sw_64/include/asm/cacheflush.h b/arch/sw_64/include/asm/cacheflush.h
index 985161896f71bb1edeb050b932e6551aff62c879..536b0b7b78bdbb7269c2e772818043fee937bbf4 100644
--- a/arch/sw_64/include/asm/cacheflush.h
+++ b/arch/sw_64/include/asm/cacheflush.h
@@ -2,94 +2,12 @@
 #ifndef _ASM_SW64_CACHEFLUSH_H
 #define _ASM_SW64_CACHEFLUSH_H
 
-#include <linux/mm.h>
-#include <asm/hw_init.h>
-
-/* Caches aren't brain-dead on the sw64. */
-#define flush_cache_all()			do { } while (0)
-#define flush_cache_mm(mm)			do { } while (0)
-#define flush_cache_dup_mm(mm)			do { } while (0)
-#define flush_cache_range(vma, start, end)	do { } while (0)
-#define flush_cache_page(vma, vmaddr, pfn)	do { } while (0)
-#define ARCH_IMPLEMENTS_FLUSH_DCACHE_PAGE	0
-#define flush_dcache_page(page)			do { } while (0)
-#define flush_dcache_mmap_lock(mapping)		do { } while (0)
-#define flush_dcache_mmap_unlock(mapping)	do { } while (0)
-#define flush_cache_vmap(start, end)		do { } while (0)
-#define flush_cache_vunmap(start, end)		do { } while (0)
-
-/* Note that the following two definitions are _highly_ dependent
- * on the contexts in which they are used in the kernel.  I personally
- * think it is criminal how loosely defined these macros are.
+/*
+ * DCache: PIPT
+ * ICache:
+ *	- C3A/B is VIVT with ICTAG, support coherence.
+ *	- C4 is VIPT
  */
-
-/* We need to flush the kernel's icache after loading modules.  The
- * only other use of this macro is in load_aout_interp which is not
- * used on sw64.
-
- * Note that this definition should *not* be used for userspace
- * icache flushing.  While functional, it is _way_ overkill.  The
- * icache is tagged with ASNs and it suffices to allocate a new ASN
- * for the process.
- */
-#ifndef CONFIG_SMP
-static inline void
-flush_icache_range(unsigned long start, unsigned long end)
-{
-	if (icache_is_vivt_no_ictag())
-		imb();
-}
-#define flush_icache_range flush_icache_range
-#else
-extern void smp_imb(void);
-static inline void
-flush_icache_range(unsigned long start, unsigned long end)
-{
-	if (icache_is_vivt_no_ictag())
-		smp_imb();
-}
-#define flush_icache_range flush_icache_range
-#endif
-
-/* We need to flush the userspace icache after setting breakpoints in
- * ptrace.
-
- * Instead of indiscriminately using imb, take advantage of the fact
- * that icache entries are tagged with the ASN and load a new mm context.
- */
-/* ??? Ought to use this in arch/sw_64/kernel/signal.c too.  */
-
-#ifndef CONFIG_SMP
-#include <linux/sched.h>
-
-extern void __load_new_mm_context(struct mm_struct *);
-static inline void
-flush_icache_user_page(struct vm_area_struct *vma, struct page *page,
-			unsigned long addr, int len)
-{
-	if ((vma->vm_flags & VM_EXEC) && icache_is_vivt_no_ictag())
-		imb();
-}
-#define flush_icache_user_page flush_icache_user_page
-#else
-extern void flush_icache_user_page(struct vm_area_struct *vma,
-				    struct page *page,
-				    unsigned long addr, int len);
-#define flush_icache_user_page flush_icache_user_page
-#endif
-
-/* This is used only in __do_fault and do_swap_page.  */
-#define flush_icache_page(vma, page) \
-	flush_icache_user_page((vma), (page), 0, 0)
-
-#define copy_to_user_page(vma, page, vaddr, dst, src, len) \
-do {	\
-	memcpy(dst, src, len); \
-	flush_icache_user_page(vma, page, vaddr, len); \
-} while (0)
-#define copy_from_user_page(vma, page, vaddr, dst, src, len) \
-	memcpy(dst, src, len)
-
 #include <asm-generic/cacheflush.h>
 
 #endif /* _ASM_SW64_CACHEFLUSH_H */
diff --git a/arch/sw_64/include/asm/checksum.h b/arch/sw_64/include/asm/checksum.h
index 284c1678f51ea084f71ee6a3747ce49854c512eb..7f3768290402bea31e0e5c445716012e81d6ad4e 100644
--- a/arch/sw_64/include/asm/checksum.h
+++ b/arch/sw_64/include/asm/checksum.h
@@ -123,4 +123,4 @@ static inline unsigned short from64to16(unsigned long x)
 	return out_v.us[0] + out_v.us[1];
 }
 
-#endif
+#endif /* _ASM_SW64_CHECKSUM_H */
diff --git a/arch/sw_64/include/asm/chip3_io.h b/arch/sw_64/include/asm/chip3_io.h
index 14d02c080607403805e357207ec8fa1de9eb6376..18e79cf2a36ba9aac5bd7dff9f93e3af59025c1f 100644
--- a/arch/sw_64/include/asm/chip3_io.h
+++ b/arch/sw_64/include/asm/chip3_io.h
@@ -165,6 +165,9 @@ enum {
 	MC_CAP_CFG =		MCU_BASE | 0x1180UL,
 	IO_START =		MCU_BASE | 0x1300UL,
 	UART_ONLINE =		MCU_BASE | 0x1780UL,
+	I2C0_SRST_L =		MCU_BASE | 0x1900UL,
+	I2C1_SRST_L =		MCU_BASE | 0x1980UL,
+	I2C2_SRST_L =		MCU_BASE | 0x1a00UL,
 	MCU_DVC_INT =		MCU_BASE | 0x3000UL,
 	MCU_DVC_INT_EN =	MCU_BASE | 0x3080UL,
 	SI_FAULT_STAT =		MCU_BASE | 0x3100UL,
@@ -314,4 +317,4 @@ enum {
 	GPIO_SWPORTA_DDR =	GPIO_BASE | 0x200UL,
 };
 /*--------------------------------------------------------------------------*/
-#endif
+#endif /* _ASM_SW64_CHIP3_IO_H */
diff --git a/arch/sw_64/include/asm/clock.h b/arch/sw_64/include/asm/clock.h
index 06ad4bcd6ad3f2599a15a8acfc18dd08eb3685e8..af6872ed9edbea4f6ab3f6b176bd0bc8398f9158 100644
--- a/arch/sw_64/include/asm/clock.h
+++ b/arch/sw_64/include/asm/clock.h
@@ -44,13 +44,13 @@ struct clk {
 
 int clk_init(void);
 
-int sw64_set_rate(int index, unsigned long rate);
+void sw64_set_rate(unsigned long rate);
 
 struct clk *sw64_clk_get(struct device *dev, const char *id);
 
-unsigned long sw64_clk_get_rate(struct clk *clk);
-
 void sw64_update_clockevents(unsigned long cpu, u32 freq);
 
 void sw64_store_policy(struct cpufreq_policy *policy);
-#endif				/* _ASM_SW64_CLOCK_H */
+
+unsigned int __sw64_cpufreq_get(struct cpufreq_policy *policy);
+#endif /* _ASM_SW64_CLOCK_H */
diff --git a/arch/sw_64/include/asm/cmpxchg.h b/arch/sw_64/include/asm/cmpxchg.h
index e07abc47c7dd2218862cb9df4a6c2f65b6d7d278..7f2d103db9c21c950825bc111bb0ee08b8d0e286 100644
--- a/arch/sw_64/include/asm/cmpxchg.h
+++ b/arch/sw_64/include/asm/cmpxchg.h
@@ -39,6 +39,7 @@
 #endif
 #undef ____xchg
 #undef ____cmpxchg
+#undef _ASM_SW64_XCHG_H
 #define ____xchg(type, args...)		__xchg ##type(args)
 #define ____cmpxchg(type, args...)	__cmpxchg ##type(args)
 #include <asm/xchg.h>
diff --git a/arch/sw_64/include/asm/core.h b/arch/sw_64/include/asm/core.h
index 72d752c8741261fa2a37caab52c1fb177d7925d5..e5e4cc138102791c5976634799afd60d646cc032 100644
--- a/arch/sw_64/include/asm/core.h
+++ b/arch/sw_64/include/asm/core.h
@@ -45,4 +45,4 @@ extern void entSys(void);
 extern void entUna(void);
 /* head.S */
 extern void __smp_callin(unsigned long);
-#endif
+#endif /* _ASM_SW64_CORE_H */
diff --git a/arch/sw_64/include/asm/delay.h b/arch/sw_64/include/asm/delay.h
index 45112c7c3c013b02b3d7a5368c9d29371e3af7b1..f4080753e9545c1f19cc3775b4ad362cc378b32f 100644
--- a/arch/sw_64/include/asm/delay.h
+++ b/arch/sw_64/include/asm/delay.h
@@ -8,4 +8,4 @@ extern void udelay(unsigned long usecs);
 extern void ndelay(unsigned long nsecs);
 #define ndelay ndelay
 
-#endif /* defined(_ASM_SW64_DELAY_H) */
+#endif /* _ASM_SW64_DELAY_H */
diff --git a/arch/sw_64/include/asm/device.h b/arch/sw_64/include/asm/device.h
index dadd756d693421ae5dc0e52d530cd293d88d4a75..bc1408c47dd3875bcb7b371411e876cb62ad93f2 100644
--- a/arch/sw_64/include/asm/device.h
+++ b/arch/sw_64/include/asm/device.h
@@ -10,4 +10,4 @@ struct dev_archdata {
 
 struct pdev_archdata {
 };
-#endif
+#endif /* _ASM_SW64_DEVICE_H */
diff --git a/arch/sw_64/include/asm/dma-mapping.h b/arch/sw_64/include/asm/dma-mapping.h
index bb84690eabfeeaef126c495567e3cbc395df06f3..37fce35b09d50d9a4a1b3e73456d2e97cbabd0de 100644
--- a/arch/sw_64/include/asm/dma-mapping.h
+++ b/arch/sw_64/include/asm/dma-mapping.h
@@ -9,4 +9,4 @@ static inline const struct dma_map_ops *get_arch_dma_ops(struct bus_type *bus)
 	return dma_ops;
 }
 
-#endif	/* _ASM_SW64_DMA_MAPPING_H */
+#endif /* _ASM_SW64_DMA_MAPPING_H */
diff --git a/arch/sw_64/include/asm/dmi.h b/arch/sw_64/include/asm/dmi.h
index 5142aa66ea45dfdd2710a0f40005fceca77ad6e7..05e80c9a3a76dc143e505440a41b7d150b873e3d 100644
--- a/arch/sw_64/include/asm/dmi.h
+++ b/arch/sw_64/include/asm/dmi.h
@@ -27,4 +27,4 @@
 #define dmi_unmap(x)			early_iounmap(x, 0)
 #define dmi_alloc(l)			kzalloc(l, GFP_KERNEL)
 
-#endif
+#endif /* _ASM_SW64_DMI_H */
diff --git a/arch/sw_64/include/asm/early_ioremap.h b/arch/sw_64/include/asm/early_ioremap.h
index 930c6bf36ad3c69a0be9dbe175649fc17d780d84..5459cba8a67772a1a4c904752272af13853eb02e 100644
--- a/arch/sw_64/include/asm/early_ioremap.h
+++ b/arch/sw_64/include/asm/early_ioremap.h
@@ -27,4 +27,4 @@ static inline void early_iounmap(volatile void __iomem *addr, unsigned long size
 }
 #define early_memunmap(addr, size)		early_iounmap(addr, size)
 
-#endif
+#endif /* _ASM_SW64_EARLY_IOREMAP_H */
diff --git a/arch/sw_64/include/asm/efi.h b/arch/sw_64/include/asm/efi.h
index 2bc863e3b8368be3d095495ed569c01383bb0f6b..f061cae2fc8a7f32c4c7445084b783ec662cddb3 100644
--- a/arch/sw_64/include/asm/efi.h
+++ b/arch/sw_64/include/asm/efi.h
@@ -21,7 +21,7 @@ extern void efi_init(void);
 	__f(args);							\
 })
 
-#define	ARCH_EFI_IRQ_FLAGS_MASK		0x00000001
+#define ARCH_EFI_IRQ_FLAGS_MASK		0x00000001
 
 /* arch specific definitions used by the stub code */
 
diff --git a/arch/sw_64/include/asm/extable.h b/arch/sw_64/include/asm/extable.h
index ae753772a45a9b12e8c4476b9e78be7921371822..3680b4a918a6676e8ce6b31fba2ef487dccfe9f4 100644
--- a/arch/sw_64/include/asm/extable.h
+++ b/arch/sw_64/include/asm/extable.h
@@ -56,4 +56,4 @@ struct exception_table_entry {
 extern short regoffsets[];
 #define map_regs(r) (*(unsigned long *)((char *)regs + regoffsets[r]))
 
-#endif
+#endif /* _ASM_SW64_EXTABLE_H */
diff --git a/arch/sw_64/include/asm/hcall.h b/arch/sw_64/include/asm/hcall.h
index b5438b477c87113db265f0fd7dd51b3a61f53bf4..65669e54c0f843927972f03753c0943240758895 100644
--- a/arch/sw_64/include/asm/hcall.h
+++ b/arch/sw_64/include/asm/hcall.h
@@ -39,4 +39,4 @@ static inline unsigned long hcall(unsigned long hcall, unsigned long arg0,
 	return __r0;
 }
 
-#endif  /* _ASM_SW64_HCALL_H */
+#endif /* _ASM_SW64_HCALL_H */
diff --git a/arch/sw_64/include/asm/hmcall.h b/arch/sw_64/include/asm/hmcall.h
index 310cc61a5a343b4e531fe98ae01b9eb3e5aba7f4..71d203efc587481bfdaf0a14aaecd15509dc0966 100644
--- a/arch/sw_64/include/asm/hmcall.h
+++ b/arch/sw_64/include/asm/hmcall.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
-#ifndef _ASM_SW64_HMC_H
-#define _ASM_SW64_HMC_H
+#ifndef _ASM_SW64_HMCALL_H
+#define _ASM_SW64_HMCALL_H
 
 /*
  * Common HMC-code
@@ -17,16 +17,12 @@
 #define HMC_wrksp		0x0E
 #define HMC_mtinten		0x0F
 #define HMC_load_mm		0x11
-#define HMC_rdpcbb		0x12
-#define HMC_wrpcbb		0x13
 #define HMC_tbisasn		0x14
 #define HMC_tbivpn		0x19
 #define HMC_ret			0x1A
 #define HMC_wrvpcr		0x29
 #define HMC_wrfen		0x2B
-#define HMC_kvcpucb		0x2C
 #define HMC_sflush		0x2F
-#define HMC_swpctx		0x30
 #define HMC_entervm		0x31
 #define HMC_hcall		0x32
 #define HMC_tbi			0x33
@@ -45,25 +41,27 @@
 
 
 /* 0x80  - 0xBF : User Level HMC routine */
-#define HMC_bpt			0x80
-#define HMC_callsys		0x83
-#define HMC_imb			0x86
+#include <uapi/asm/hmcall.h>
+
+/* Following will be deprecated from user level invocation */
 #define HMC_rwreg		0x87
-#define HMC_rdunique		0x9E
-#define HMC_wrunique		0x9F
 #define HMC_sz_uflush		0xA8
-#define HMC_gentrap		0xAA
-#define HMC_wrperfmon		0xB0
 #define HMC_longtime		0xB1
 
 #ifdef __KERNEL__
 #ifndef __ASSEMBLY__
 
+#include <linux/init.h>
+extern void __init fixup_hmcall(void);
+
 extern void halt(void) __attribute__((noreturn));
-#define __halt() __asm__ __volatile__ ("sys_call %0 #halt" : : "i" (HMC_halt))
 
-#define imb() \
-	__asm__ __volatile__ ("sys_call %0 #imb" : : "i" (HMC_imb) : "memory")
+#define __CALL_HMC_VOID(NAME)					\
+static inline void NAME(void)					\
+{								\
+	__asm__ __volatile__(					\
+		"sys_call %0 ": : "i" (HMC_ ## NAME));		\
+}
 
 #define __CALL_HMC_R0(NAME, TYPE)				\
 static inline TYPE NAME(void)					\
@@ -142,10 +140,14 @@ static inline RTYPE NAME(TYPE0 arg0, TYPE1 arg1, TYPE2 arg2)		\
 	return __r0;							\
 }
 
-#define sflush()						\
-{								\
-	__asm__ __volatile__("sys_call 0x2f");			\
-}
+
+__CALL_HMC_VOID(imb);
+__CALL_HMC_VOID(sflush);
+__CALL_HMC_VOID(wrfen);
+#define fpu_enable()	wrfen()
+
+__CALL_HMC_VOID(sleepen);
+__CALL_HMC_VOID(mtinten);
 
 __CALL_HMC_R0(rdps, unsigned long);
 
@@ -156,8 +158,6 @@ __CALL_HMC_R0(rdksp, unsigned long);
 __CALL_HMC_W1(wrksp, unsigned long);
 
 __CALL_HMC_W2(load_mm, unsigned long, unsigned long);
-__CALL_HMC_R0(rdpcbb, unsigned long);
-__CALL_HMC_W1(wrpcbb, unsigned long);
 
 __CALL_HMC_R0(rdptbr, unsigned long);
 __CALL_HMC_W1(wrptbr, unsigned long);
@@ -166,9 +166,6 @@ __CALL_HMC_RW1(swpipl, unsigned long, unsigned long);
 __CALL_HMC_R0(whami, unsigned long);
 __CALL_HMC_RW1(rdio64, unsigned long, unsigned long);
 __CALL_HMC_RW1(rdio32, unsigned int, unsigned long);
-__CALL_HMC_R0(kvcpucb, unsigned long);
-__CALL_HMC_R0(sleepen, unsigned long);
-__CALL_HMC_R0(mtinten, unsigned long);
 __CALL_HMC_W2(wrent, void*, unsigned long);
 __CALL_HMC_W2(tbisasn, unsigned long, unsigned long);
 __CALL_HMC_W1(wrkgp, unsigned long);
@@ -178,6 +175,7 @@ __CALL_HMC_W1(wrtimer, unsigned long);
 __CALL_HMC_RW3(tbivpn, unsigned long, unsigned long, unsigned long, unsigned long);
 __CALL_HMC_RW2(cpuid, unsigned long, unsigned long, unsigned long);
 
+__CALL_HMC_W1(wrtp, unsigned long);
 /*
  * TB routines..
  */
@@ -193,13 +191,29 @@ __CALL_HMC_RW2(cpuid, unsigned long, unsigned long, unsigned long);
 })
 
 #define tbi(x, y)	__tbi(x, __r17 = (y), "1" (__r17))
-#define tbisi(x)	__tbi(1, __r17 = (x), "1" (__r17))
-#define tbisd(x)	__tbi(2, __r17 = (x), "1" (__r17))
-#define tbis(x)		__tbi(3, __r17 = (x), "1" (__r17))
-#define tbiap()		__tbi(-1, /* no second argument */)
+
+/* Invalidate all TLB, only used by hypervisor */
 #define tbia()		__tbi(-2, /* no second argument */)
 
+/* Invalidate TLB for all processes with currnet VPN */
+#define tbivp()		__tbi(-1, /* no second argument */)
+
+/* Invalidate all TLB with current VPN */
+#define tbiv()		__tbi(0, /* no second argument */)
+
+/* Invalidate ITLB of addr with current UPN and VPN */
+#define tbisi(addr)	__tbi(1, __r17 = (addr), "1" (__r17))
+
+/* Invalidate DTLB of addr with current UPN and VPN */
+#define tbisd(addr)	__tbi(2, __r17 = (addr), "1" (__r17))
+
+/* Invalidate TLB of addr with current UPN and VPN */
+#define tbis(addr)	__tbi(3, __r17 = (addr), "1" (__r17))
+
+/* Invalidate all user TLB with current UPN and VPN */
+#define tbiu()		__tbi(4, /* no second argument */)
+
 #endif /* !__ASSEMBLY__ */
 #endif /* __KERNEL__ */
 
-#endif /* _ASM_SW64_HMC_H */
+#endif /* _ASM_SW64_HMCALL_H */
diff --git a/arch/sw_64/include/asm/hw_init.h b/arch/sw_64/include/asm/hw_init.h
index de9f93f9b26ee730f179629bfa12024c5cac8b77..81dd2581e0da017e5427a6dbb0dc60924e2a5bf1 100644
--- a/arch/sw_64/include/asm/hw_init.h
+++ b/arch/sw_64/include/asm/hw_init.h
@@ -18,10 +18,7 @@ struct cache_desc {
 };
 
 struct cpuinfo_sw64 {
-	unsigned long loops_per_jiffy;
 	unsigned long last_asn;
-	int need_new_asn;
-	int asn_lock;
 	unsigned long ipi_count;
 	struct cache_desc icache; /* Primary I-cache */
 	struct cache_desc dcache; /* Primary D or combined I/D cache */
@@ -85,12 +82,11 @@ static inline unsigned long get_cpu_freq(void)
 	return cpu_desc.frequency;
 }
 
-static inline bool icache_is_vivt_no_ictag(void)
+static inline void update_cpu_freq(unsigned long freq)
 {
-	/*
-	 * Icache of C3B is vivt with ICtag. C4 will be vipt.
-	 */
-	return (cpu_desc.arch_var == 0x3 && cpu_desc.arch_rev == 0x1);
+	freq = freq * 1000000;
+	if (cpu_desc.frequency != freq)
+		cpu_desc.frequency = freq;
 }
 
 #define EMUL_FLAG	(0x1UL << 63)
@@ -166,4 +162,4 @@ DECLARE_STATIC_KEY_FALSE(run_mode_emul_key);
 	(((val) & CACHE_INDEX_BITS_MASK) >> CACHE_INDEX_BITS_SHIFT)
 #define current_cpu_data cpu_data[smp_processor_id()]
 
-#endif /* HW_INIT_H */
+#endif /* _ASM_SW64_HW_INIT_H */
diff --git a/arch/sw_64/include/asm/hw_irq.h b/arch/sw_64/include/asm/hw_irq.h
index f6fd1d802abdf9356ae94d59240036cc673b3fd0..ad5aed26efb7f9ca58bd3b5881e0858d401780c0 100644
--- a/arch/sw_64/include/asm/hw_irq.h
+++ b/arch/sw_64/include/asm/hw_irq.h
@@ -13,4 +13,4 @@ DECLARE_PER_CPU(unsigned long, irq_pmi_count);
 typedef unsigned int vector_irq_t[PERCPU_MSI_IRQS];
 DECLARE_PER_CPU(vector_irq_t, vector_irq);
 #endif
-#endif
+#endif /* _ASM_SW64_HW_IRQ_H */
diff --git a/arch/sw_64/include/asm/insn.h b/arch/sw_64/include/asm/insn.h
index 54a9a20267845d3a16785bbc5d5728856f2a3741..ec0efae3aed0320fafa9175c6155318309acc649 100644
--- a/arch/sw_64/include/asm/insn.h
+++ b/arch/sw_64/include/asm/insn.h
@@ -14,8 +14,8 @@
  * You should have received a copy of the GNU General Public License
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-#ifndef	_ASM_SW64_INSN_H
-#define	_ASM_SW64_INSN_H
+#ifndef _ASM_SW64_INSN_H
+#define _ASM_SW64_INSN_H
 #include <linux/types.h>
 
 /* Register numbers */
@@ -28,7 +28,7 @@ enum {
 
 #define BR_MAX_DISP		0xfffff
 /* SW64 instructions are always 32 bits. */
-#define	SW64_INSN_SIZE		4
+#define SW64_INSN_SIZE		4
 
 #define ___SW64_RA(a)		(((a) & 0x1f) << 21)
 #define ___SW64_RB(b)		(((b) & 0x1f) << 16)
@@ -93,4 +93,4 @@ SW64_INSN(fbge,		0xf4000000, 0xfc000000);
 SW64_INSN(lldw,		0x20000000, 0xfc00f000);
 SW64_INSN(lldl,		0x20001000, 0xfc00f000);
 
-#endif	/* _ASM_SW64_INSN_H */
+#endif /* _ASM_SW64_INSN_H */
diff --git a/arch/sw_64/include/asm/irq_impl.h b/arch/sw_64/include/asm/irq_impl.h
index 48dbc486a126d6b37cf10897ef2d9518e6302a75..797af433a1267c8b49cfda9901baed051ff16b98 100644
--- a/arch/sw_64/include/asm/irq_impl.h
+++ b/arch/sw_64/include/asm/irq_impl.h
@@ -45,4 +45,4 @@ extern void handle_ipi(struct pt_regs *regs);
 extern void __init sw64_init_irq(void);
 extern irqreturn_t timer_interrupt(int irq, void *dev);
 
-#endif
+#endif /* _ASM_SW64_IRQ_IMPL_H */
diff --git a/arch/sw_64/include/asm/jump_label.h b/arch/sw_64/include/asm/jump_label.h
index 78d3fb6246f013862df71773d497281f7f82a226..32fbf7573b206bb2c935cc173de392b100d02010 100644
--- a/arch/sw_64/include/asm/jump_label.h
+++ b/arch/sw_64/include/asm/jump_label.h
@@ -1,7 +1,7 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 
-#ifndef __ASM_SW64_JUMP_LABEL_H
-#define __ASM_SW64_JUMP_LABEL_H
+#ifndef _ASM_SW64_JUMP_LABEL_H
+#define _ASM_SW64_JUMP_LABEL_H
 
 #ifndef __ASSEMBLY__
 
@@ -47,4 +47,4 @@ struct jump_entry {
 };
 
 #endif /* __ASSEMBLY__ */
-#endif /* __ASM_SW64_JUMP_LABEL_H */
+#endif /* _ASM_SW64_JUMP_LABEL_H */
diff --git a/arch/sw_64/include/asm/kgdb.h b/arch/sw_64/include/asm/kgdb.h
index 1d807362e867ce089bf2f7d78026a68b5abc3ac7..6478c7a989c37a92846dcb9ea9161ab218a31894 100644
--- a/arch/sw_64/include/asm/kgdb.h
+++ b/arch/sw_64/include/asm/kgdb.h
@@ -34,7 +34,7 @@
 
 static inline void arch_kgdb_breakpoint(void)
 {
-	asm __volatile__ ("sys_call/b 0x80");
+	asm __volatile__ ("sys_call %0" : : "i"(HMC_bpt) );
 }
 
 void sw64_task_to_gdb_regs(struct task_struct *task, unsigned long *regs);
diff --git a/arch/sw_64/include/asm/kprobes.h b/arch/sw_64/include/asm/kprobes.h
index c19b961a19da4143e419c4bef5de399ef7d2e086..6b7e4548a8bd365f17d73bcd468e25e872b56c0e 100644
--- a/arch/sw_64/include/asm/kprobes.h
+++ b/arch/sw_64/include/asm/kprobes.h
@@ -19,7 +19,7 @@
 #include <asm/cacheflush.h>
 #include <asm/kdebug.h>
 
-#define	 __ARCH_WANT_KPROBES_INSN_SLOT
+#define __ARCH_WANT_KPROBES_INSN_SLOT
 
 struct kprobe;
 struct pt_regs;
diff --git a/arch/sw_64/include/asm/kvm_asm.h b/arch/sw_64/include/asm/kvm_asm.h
index 7e2c92ed45749d0defdb2771d98a6e431bfbb0f3..841bfa1dd0aad0c4ac99365b856e9b5f5eda0440 100644
--- a/arch/sw_64/include/asm/kvm_asm.h
+++ b/arch/sw_64/include/asm/kvm_asm.h
@@ -4,6 +4,8 @@
 
 #define SW64_KVM_EXIT_HOST_INTR		0
 #define SW64_KVM_EXIT_IO		1
+#define SW64_KVM_MIGRATION_SET_DIRTY    2
+#define SW64_KVM_MIGRATION_SET_DIRTY_HM 3
 #define SW64_KVM_EXIT_HALT		10
 #define SW64_KVM_EXIT_SHUTDOWN		12
 #define SW64_KVM_EXIT_TIMER		13
@@ -14,4 +16,16 @@
 #ifdef CONFIG_KVM_MEMHOTPLUG
 #define SW64_KVM_EXIT_MEMHOTPLUG	23
 #endif
+
+#define kvm_sw64_exception_type	\
+	{0, "HOST_INTR" },	\
+	{1, "IO" },		\
+	{10, "HALT" },		\
+	{12, "SHUTDOWN" },	\
+	{13, "TIMER" },		\
+	{14, "IPI" },		\
+	{17, "RESTART" },	\
+	{22, "FATAL_ERROR" },	\
+	{23, "MEMHOTPLUG" }
+
 #endif /* _ASM_SW64_KVM_ASM_H */
diff --git a/arch/sw_64/include/asm/kvm_cma.h b/arch/sw_64/include/asm/kvm_cma.h
index 192bca43638089cef0c0e0d0ee6e1126231fb596..d50ba599ceb716ac0309122cf77c1e2ea38c70d7 100644
--- a/arch/sw_64/include/asm/kvm_cma.h
+++ b/arch/sw_64/include/asm/kvm_cma.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
-#ifndef _ASM_SW64_KVM_CMA_H__
-#define _ASM_SW64_KVM_CMA_H__
+#ifndef _ASM_SW64_KVM_CMA_H
+#define _ASM_SW64_KVM_CMA_H
 
 #include <linux/cma.h>
 
@@ -8,4 +8,4 @@ extern int __init kvm_cma_declare_contiguous(phys_addr_t base,
 			phys_addr_t size, phys_addr_t limit,
 			phys_addr_t alignment, unsigned int order_per_bit,
 			const char *name, struct cma **res_cma);
-#endif
+#endif /* _ASM_SW64_KVM_CMA_H */
diff --git a/arch/sw_64/include/asm/kvm_emulate.h b/arch/sw_64/include/asm/kvm_emulate.h
index d842008f189a326e497cd38d940a59f3d5e9c8f3..915aa6c0bce212b8b06bb3aa19aa6aee8c04e532 100644
--- a/arch/sw_64/include/asm/kvm_emulate.h
+++ b/arch/sw_64/include/asm/kvm_emulate.h
@@ -43,4 +43,4 @@ unsigned int interrupt_pending(struct kvm_vcpu *vcpu, bool *more);
 void clear_vcpu_irq(struct kvm_vcpu *vcpu);
 void inject_vcpu_irq(struct kvm_vcpu *vcpu, unsigned int irq);
 void try_deliver_interrupt(struct kvm_vcpu *vcpu, unsigned int irq, bool more);
-#endif
+#endif /* _ASM_SW64_KVM_EMULATE_H */
diff --git a/arch/sw_64/include/asm/kvm_host.h b/arch/sw_64/include/asm/kvm_host.h
index 6d292c0863478e6d2bef2d293280fe9ec4aa3cbe..02d7131f02865c5f2b832ee5db5be6aec8e13fed 100644
--- a/arch/sw_64/include/asm/kvm_host.h
+++ b/arch/sw_64/include/asm/kvm_host.h
@@ -124,4 +124,7 @@ static inline void kvm_arch_vcpu_blocking(struct kvm_vcpu *vcpu) {}
 static inline void kvm_arch_vcpu_unblocking(struct kvm_vcpu *vcpu) {}
 static inline void kvm_arch_vcpu_block_finish(struct kvm_vcpu *vcpu) {}
 
-#endif  /* _ASM_SW64_KVM_HOST_H */
+int kvm_sw64_perf_init(void);
+int kvm_sw64_perf_teardown(void);
+
+#endif /* _ASM_SW64_KVM_HOST_H */
diff --git a/arch/sw_64/include/asm/kvm_mmio.h b/arch/sw_64/include/asm/kvm_mmio.h
index 9ba31c91902fd2e16be4a7f379d21c2c2358cd11..c87b259e9395f0943b062c4d4a6e08a433bacf1d 100644
--- a/arch/sw_64/include/asm/kvm_mmio.h
+++ b/arch/sw_64/include/asm/kvm_mmio.h
@@ -14,4 +14,4 @@ int kvm_handle_mmio_return(struct kvm_vcpu *vcpu, struct kvm_run *run);
 int io_mem_abort(struct kvm_vcpu *vcpu, struct kvm_run *run,
 		 struct hcall_args *hargs);
 
-#endif	/* _ASM_SW64_KVM_MMIO_H */
+#endif /* _ASM_SW64_KVM_MMIO_H */
diff --git a/arch/sw_64/include/asm/kvm_para.h b/arch/sw_64/include/asm/kvm_para.h
index ba78c53715701b8a3f59ebc11631e1e6d648fa1d..442f1c7d9f832159bcb04068b8939c1fa3b107cc 100644
--- a/arch/sw_64/include/asm/kvm_para.h
+++ b/arch/sw_64/include/asm/kvm_para.h
@@ -23,4 +23,4 @@ static inline unsigned long kvm_hypercall3(unsigned long num,
 		: "$1", "$22", "$23", "$24", "$25");
 	return __r0;
 }
-#endif
+#endif /* _ASM_SW64_KVM_PARA_H */
diff --git a/arch/sw_64/include/asm/kvm_timer.h b/arch/sw_64/include/asm/kvm_timer.h
index be50bba9c4c684d11c13faccd9c03c176d930825..8080873c684f82b0e0c06b7e55c72ba475f497cd 100644
--- a/arch/sw_64/include/asm/kvm_timer.h
+++ b/arch/sw_64/include/asm/kvm_timer.h
@@ -6,4 +6,4 @@ void set_timer(struct kvm_vcpu *vcpu, unsigned long delta);
 void set_interrupt(struct kvm_vcpu *vcpu, unsigned int irq);
 enum hrtimer_restart clockdev_fn(struct hrtimer *timer);
 
-#endif
+#endif /* _ASM_SW64_KVM_TIMER_H */
diff --git a/arch/sw_64/include/asm/linkage.h b/arch/sw_64/include/asm/linkage.h
index 96c83663d9e84b81a03a1673a4e02284136e3826..6576fb46a000c3c6e7a6cfb644db9db8b981a959 100644
--- a/arch/sw_64/include/asm/linkage.h
+++ b/arch/sw_64/include/asm/linkage.h
@@ -6,4 +6,4 @@
 #define SYSCALL_ALIAS(alias, name)                                      \
 	asm(#alias " = " #name "\n\t.globl " #alias)
 
-#endif
+#endif /* _ASM_SW64_LINKAGE_H */
diff --git a/arch/sw_64/include/asm/mmu.h b/arch/sw_64/include/asm/mmu.h
index 548c73b318cb2acbeeaa501bbabc9d95c4151b9c..f24219fac654bb3381ea0e85e5faeeb3486d23d3 100644
--- a/arch/sw_64/include/asm/mmu.h
+++ b/arch/sw_64/include/asm/mmu.h
@@ -7,4 +7,4 @@ typedef struct {
 	unsigned long asid[NR_CPUS];
 	void *vdso;
 } mm_context_t;
-#endif
+#endif /* _ASM_SW64_MMU_H */
diff --git a/arch/sw_64/include/asm/mmu_context.h b/arch/sw_64/include/asm/mmu_context.h
index a797673273aff51ba254a18ea11b13eddae683c8..84e84048a3ba6527941b1b1271c1fdcafdddda3a 100644
--- a/arch/sw_64/include/asm/mmu_context.h
+++ b/arch/sw_64/include/asm/mmu_context.h
@@ -13,38 +13,14 @@
 #include <asm/io.h>
 
 /*
- * Force a context reload. This is needed when we change the page
- * table pointer or when we update the ASN of the current process.
+ * Load a mm context. This is needed when we change the page
+ * table pointer(CSR:PTBR) or when we update the ASID.
+ *
  */
-
-static inline unsigned long
-__reload_thread(struct pcb_struct *pcb)
-{
-	register unsigned long a0 __asm__("$16");
-	register unsigned long v0 __asm__("$0");
-
-	a0 = virt_to_phys(pcb);
-	__asm__ __volatile__(
-		"sys_call %2 #__reload_thread"
-		: "=r"(v0), "=r"(a0)
-		: "i"(HMC_swpctx), "r"(a0)
-		: "$1", "$22", "$23", "$24", "$25");
-
-	return v0;
-}
-
 #define load_asn_ptbr   load_mm
 
 /*
- * The maximum ASN's the processor supports.
- *
- * If a processor implements address space numbers (ASNs), and the old
- * PTE has the Address Space Match (ASM) bit clear (ASNs in use) and
- * the Valid bit set, then entries can also effectively be made coherent
- * by assigning a new, unused ASN to the currently running process and
- * not reusing the previous ASN before calling the appropriate HMcode
- * routine to invalidate the translation buffer (TB).
- *
+ * The maximum ASN's the processor supports. ASN is called ASID too.
  */
 
 #ifdef CONFIG_SUBARCH_C3B
@@ -72,7 +48,7 @@ __reload_thread(struct pcb_struct *pcb)
  * need to do "p->mm->context = 0".
  *
  * If we need more ASN's than the processor has, we invalidate the old
- * user TLB's (tbiap()) and start a new ASN version. That will automatically
+ * user TLB's (tbivp()) and start a new ASN version. That will automatically
  * force a new asn for any other processes the next time they want to
  * run.
  */
@@ -84,7 +60,7 @@ __get_new_mm_context(struct mm_struct *mm, long cpu)
 	unsigned long next = asn + 1;
 
 	if ((asn & HARDWARE_ASN_MASK) >= HARDWARE_ASN_MASK) {
-		tbiap();
+		tbivp();
 		next = (asn & ~HARDWARE_ASN_MASK) + ASN_FIRST_VERSION;
 	}
 	cpu_last_asn(cpu) = next;
@@ -92,18 +68,13 @@ __get_new_mm_context(struct mm_struct *mm, long cpu)
 }
 
 static inline void
-switch_mm(struct mm_struct *prev_mm, struct mm_struct *next_mm,
-	  struct task_struct *next)
+switch_mm_irqs_off(struct mm_struct *prev_mm, struct mm_struct *next_mm,
+		   struct task_struct *next)
 {
 	/* Check if our ASN is of an older version, and thus invalid. */
-	unsigned long asn;
-	unsigned long mmc;
+	unsigned long asn, mmc, ptbr;
 	long cpu = smp_processor_id();
 
-#ifdef CONFIG_SMP
-	cpu_data[cpu].asn_lock = 1;
-	barrier();
-#endif
 	asn = cpu_last_asn(cpu);
 	mmc = next_mm->context.asid[cpu];
 	if ((mmc ^ asn) & ~HARDWARE_ASN_MASK) {
@@ -111,50 +82,31 @@ switch_mm(struct mm_struct *prev_mm, struct mm_struct *next_mm,
 		mmc = __get_new_mm_context(next_mm, cpu);
 		next_mm->context.asid[cpu] = mmc;
 	}
-#ifdef CONFIG_SMP
-	else
-		cpu_data[cpu].need_new_asn = 1;
-#endif
 
 	/*
-	 * Always update the PCB ASN.  Another thread may have allocated
-	 * a new mm->context (via flush_tlb_mm) without the ASN serial
+	 * Update CSR:UPN and CSR:PTBR. Another thread may have allocated
+	 * a new mm->context[asid] (via flush_tlb_mm) without the ASN serial
 	 * number wrapping.  We have no way to detect when this is needed.
 	 */
-	task_thread_info(next)->pcb.asn = mmc & HARDWARE_ASN_MASK;
-	/*
-	 * Always update the PCB PTBR. If next is kernel thread, it must
-	 * update PTBR. If next is user process, it's ok to update PTBR.
-	 */
-	task_thread_info(next)->pcb.ptbr = virt_to_pfn(next_mm->pgd);
-	load_asn_ptbr(task_thread_info(next)->pcb.asn, task_thread_info(next)->pcb.ptbr);
+	asn = mmc & HARDWARE_ASN_MASK;
+	ptbr = virt_to_pfn(next_mm->pgd);
+	load_asn_ptbr(asn, ptbr);
 }
 
-extern void __load_new_mm_context(struct mm_struct *);
-
-#ifdef CONFIG_SMP
-#define check_mmu_context()					\
-do {								\
-	int cpu = smp_processor_id();				\
-	cpu_data[cpu].asn_lock = 0;				\
-	barrier();						\
-	if (cpu_data[cpu].need_new_asn) {			\
-		struct mm_struct *mm = current->active_mm;	\
-		cpu_data[cpu].need_new_asn = 0;			\
-		if (!mm->context.asid[cpu])			\
-			__load_new_mm_context(mm);		\
-	}							\
-} while (0)
-#else
-#define check_mmu_context()  do { } while (0)
-#endif
+#define switch_mm_irqs_off switch_mm_irqs_off
 
-static inline void activate_mm(struct mm_struct *prev_mm,
-			       struct mm_struct *next_mm)
+static inline void
+switch_mm(struct mm_struct *prev_mm, struct mm_struct *next_mm,
+	  struct task_struct *tsk)
 {
-	__load_new_mm_context(next_mm);
+	unsigned long flags;
+
+	local_irq_save(flags);
+	switch_mm_irqs_off(prev_mm, next_mm, tsk);
+	local_irq_restore(flags);
 }
 
+#define activate_mm(prev, next) switch_mm(prev, next, current)
 #define deactivate_mm(tsk, mm)	do { } while (0)
 
 static inline int init_new_context(struct task_struct *tsk,
@@ -164,8 +116,6 @@ static inline int init_new_context(struct task_struct *tsk,
 
 	for_each_possible_cpu(i)
 		mm->context.asid[i] = 0;
-	if (tsk != current)
-		task_thread_info(tsk)->pcb.ptbr = virt_to_pfn(mm->pgd);
 	return 0;
 }
 
@@ -177,7 +127,6 @@ static inline void destroy_context(struct mm_struct *mm)
 static inline void enter_lazy_tlb(struct mm_struct *mm,
 				  struct task_struct *tsk)
 {
-	task_thread_info(tsk)->pcb.ptbr = virt_to_pfn(mm->pgd);
 }
 
 static inline int arch_dup_mmap(struct mm_struct *oldmm,
diff --git a/arch/sw_64/include/asm/msi.h b/arch/sw_64/include/asm/msi.h
index 079fac0d128e9100fed22456cef93d6a951ba8b9..ca5850eb5957eb45014c8b3edd4422b5fc2fe4a5 100644
--- a/arch/sw_64/include/asm/msi.h
+++ b/arch/sw_64/include/asm/msi.h
@@ -42,6 +42,6 @@ struct irq_alloc_info {
 	irq_hw_number_t         hwirq;
 };
 typedef struct irq_alloc_info msi_alloc_info_t;
-#endif	/* CONFIG_PCI_MSI_IRQ_DOMAIN */
-#endif	/* CONFIG_PCI_MSI */
-#endif	/* _ASM_SW64_MSI_H */
+#endif /* CONFIG_PCI_MSI_IRQ_DOMAIN */
+#endif /* CONFIG_PCI_MSI */
+#endif /* _ASM_SW64_MSI_H */
diff --git a/arch/sw_64/include/asm/numa.h b/arch/sw_64/include/asm/numa.h
index 4ea8b8de248af170f1cc9107e20d078f17a29f37..930be783181308e688e6746a9d8cab86ee6e6806 100644
--- a/arch/sw_64/include/asm/numa.h
+++ b/arch/sw_64/include/asm/numa.h
@@ -30,6 +30,6 @@ static inline void  set_cpuid_to_node(int cpuid, s16 node)
 	__cpuid_to_node[cpuid] = node;
 }
 
-#endif	/* CONFIG_NUMA */
+#endif /* CONFIG_NUMA */
 
-#endif	/* _ASM_SW64_NUMA_H */
+#endif /* _ASM_SW64_NUMA_H */
diff --git a/arch/sw_64/include/asm/pci.h b/arch/sw_64/include/asm/pci.h
index a90f80152470911af9b6c3d2080fb0c3956982ea..ab79d503b84daff89f18ca06771bcc8893f96b0c 100644
--- a/arch/sw_64/include/asm/pci.h
+++ b/arch/sw_64/include/asm/pci.h
@@ -18,6 +18,13 @@ struct resource;
 struct sunway_iommu;
 struct page;
 
+struct piu_saved {
+	unsigned long piuconfig0;
+	unsigned long piuconfig1;
+	unsigned long epdmabar;
+	unsigned long msiaddr;
+	unsigned long msiconfig[256];
+};
 
 /* A controller.  Used to manage multiple PCI busses.  */
 
diff --git a/arch/sw_64/include/asm/perf_event.h b/arch/sw_64/include/asm/perf_event.h
index 5f5a45217544f57b2d8da3b0df56174d0587669a..4212342334d5d12926166235e69ee02806aea694 100644
--- a/arch/sw_64/include/asm/perf_event.h
+++ b/arch/sw_64/include/asm/perf_event.h
@@ -3,5 +3,13 @@
 #define _ASM_SW64_PERF_EVENT_H
 
 #include <asm/wrperfmon.h>
+#include <asm/ptrace.h>
+
+#ifdef CONFIG_PERF_EVENTS
+struct pt_regs;
+extern unsigned long perf_instruction_pointer(struct pt_regs *regs);
+extern unsigned long perf_misc_flags(struct pt_regs *regs);
+#define perf_misc_flags(regs)  perf_misc_flags(regs)
+#endif
 
 #endif /* _ASM_SW64_PERF_EVENT_H */
diff --git a/arch/sw_64/include/asm/pgtable-4level.h b/arch/sw_64/include/asm/pgtable-4level.h
index 8c45f441c52082673cc81c6f763ce8c17ff42695..719e2c5377e349b8765a947a57f273835f2dc52b 100644
--- a/arch/sw_64/include/asm/pgtable-4level.h
+++ b/arch/sw_64/include/asm/pgtable-4level.h
@@ -2,7 +2,7 @@
 #ifndef _ASM_SW64_PGTABLE_4LEVEL_H
 #define _ASM_SW64_PGTABLE_4LEVEL_H
 
-#ifdef	__KERNEL__
+#ifdef __KERNEL__
 #ifndef __ASSEMBLY__
 /*
  * These are used to make use of C type-checking..
diff --git a/arch/sw_64/include/asm/pgtable.h b/arch/sw_64/include/asm/pgtable.h
index d63ee1417f0fac4bbb4d24d0435191ef2824ac94..2be0934a8146c0e41859ff106026d4c8dcb3600b 100644
--- a/arch/sw_64/include/asm/pgtable.h
+++ b/arch/sw_64/include/asm/pgtable.h
@@ -545,7 +545,7 @@ static inline void pmdp_set_wrprotect(struct mm_struct *mm,
 
 #define mk_pmd(page, prot)	pfn_pmd(page_to_pfn(page), (prot))
 
-#define  __HAVE_ARCH_PMDP_SET_ACCESS_FLAGS
+#define __HAVE_ARCH_PMDP_SET_ACCESS_FLAGS
 extern int pmdp_set_access_flags(struct vm_area_struct *vma,
 				 unsigned long address, pmd_t *pmdp,
 				 pmd_t entry, int dirty);
diff --git a/arch/sw_64/include/asm/platform.h b/arch/sw_64/include/asm/platform.h
index 318b6ca732cd6bfa8d468b59f0764a877059f965..59418bba9de7c6f86207d7ac6cf3e9c93652e38b 100644
--- a/arch/sw_64/include/asm/platform.h
+++ b/arch/sw_64/include/asm/platform.h
@@ -14,4 +14,4 @@ extern struct sw64_platform_ops *sw64_platform;
 
 extern struct sw64_platform_ops xuelang_ops;
 
-#endif  /* _ASM_SW64_PLATFORM_H */
+#endif /* _ASM_SW64_PLATFORM_H */
diff --git a/arch/sw_64/include/asm/ptrace.h b/arch/sw_64/include/asm/ptrace.h
index ac99430156639b8e96fe2e0307dcd90cb10a2df7..5f6cd305f95e6c3fa1c2801e52903eb4a438d470 100644
--- a/arch/sw_64/include/asm/ptrace.h
+++ b/arch/sw_64/include/asm/ptrace.h
@@ -40,12 +40,7 @@ struct pt_regs {
 	unsigned long r26;
 	unsigned long r27;
 	unsigned long r28;
-	unsigned long hae;
-/* JRP - These are the values provided to a0-a2 by HMcode */
-	unsigned long trap_a0;
-	unsigned long trap_a1;
-	unsigned long trap_a2;
-/* These are saved by HMcode: */
+	/* These are saved by HMcode: */
 	unsigned long ps;
 	unsigned long pc;
 	unsigned long gp;
@@ -54,7 +49,6 @@ struct pt_regs {
 	unsigned long r18;
 };
 
-#define arch_has_single_step()		(1)
 #define user_mode(regs) (((regs)->ps & 8) != 0)
 #define instruction_pointer(regs) ((regs)->pc)
 #define profile_pc(regs) instruction_pointer(regs)
@@ -98,4 +92,4 @@ static inline unsigned long regs_return_value(struct pt_regs *regs)
 {
 	return regs->r0;
 }
-#endif
+#endif /* _ASM_SW64_PTRACE_H */
diff --git a/arch/sw_64/include/asm/setup.h b/arch/sw_64/include/asm/setup.h
index c0fb4e8bd80c40ce9f43d6f951bfc349bc62b7a0..384eeba021446347359d097528f205606e1a4334 100644
--- a/arch/sw_64/include/asm/setup.h
+++ b/arch/sw_64/include/asm/setup.h
@@ -43,4 +43,4 @@
 extern struct boot_params *sunway_boot_params;
 #endif
 
-#endif
+#endif /* _ASM_SW64_SETUP_H */
diff --git a/arch/sw_64/include/asm/sfp-machine.h b/arch/sw_64/include/asm/sfp-machine.h
index 9b3e8688feee7725e76e51d75d34106be638d86d..c1b914898543d01498834bd1d8dff4215fee768c 100644
--- a/arch/sw_64/include/asm/sfp-machine.h
+++ b/arch/sw_64/include/asm/sfp-machine.h
@@ -66,4 +66,4 @@ do {								\
 /* We write the results always */
 #define FP_INHIBIT_RESULTS 0
 
-#endif
+#endif /* _ASM_SW64_SFP_MACHINE_H */
diff --git a/arch/sw_64/include/asm/signal.h b/arch/sw_64/include/asm/signal.h
index 3e91b72c0b0a8af9c0803cc8d0257a1f917e4f77..9e0936e6db2b4265320d46a606a7bd04c91f0f8b 100644
--- a/arch/sw_64/include/asm/signal.h
+++ b/arch/sw_64/include/asm/signal.h
@@ -14,9 +14,11 @@ typedef struct {
 	unsigned long sig[_NSIG_WORDS];
 } sigset_t;
 
-#ifdef CONFIG_OLD_SIGACTION
-#define __ARCH_HAS_SA_RESTORER
-#endif
+struct odd_sigaction {
+	__sighandler_t	sa_handler;
+	old_sigset_t	sa_mask;
+	int		sa_flags;
+};
 
 #include <asm/sigcontext.h>
-#endif
+#endif /* _ASM_SW64_SIGNAL_H */
diff --git a/arch/sw_64/include/asm/smp.h b/arch/sw_64/include/asm/smp.h
index e7aa742f73f0dd95b77e4c0a26e56c93f7311665..0573361dc840d021f3acf88989e4eeae2ce5be97 100644
--- a/arch/sw_64/include/asm/smp.h
+++ b/arch/sw_64/include/asm/smp.h
@@ -178,4 +178,4 @@ static inline void send_ipi(int cpu, unsigned long type)
 
 #define reset_cpu(cpu)  send_ipi((cpu), II_RESET)
 
-#endif
+#endif /* _ASM_SW64_SMP_H */
diff --git a/arch/sw_64/include/asm/spinlock_types.h b/arch/sw_64/include/asm/spinlock_types.h
index 28f2183ced749485f0bbbdff5767bf081c943430..62e554e4f48c35b2d4578072231b58c75b202a4b 100644
--- a/arch/sw_64/include/asm/spinlock_types.h
+++ b/arch/sw_64/include/asm/spinlock_types.h
@@ -5,4 +5,4 @@
 #include <asm-generic/qspinlock_types.h>
 #include <asm-generic/qrwlock_types.h>
 
-#endif
+#endif /* _ASM_SW64_SPINLOCK_TYPES_H */
diff --git a/arch/sw_64/include/asm/stacktrace.h b/arch/sw_64/include/asm/stacktrace.h
index ed691a72573bd210be25f8c372324a0d2c076b1f..958c9892fd6d0943bf78484c7e870323694fbde8 100644
--- a/arch/sw_64/include/asm/stacktrace.h
+++ b/arch/sw_64/include/asm/stacktrace.h
@@ -69,4 +69,4 @@ static inline bool on_accessible_stack(struct task_struct *tsk,
 	return false;
 }
 
-#endif	/* _ASM_SW64_STACKTRACE_H */
+#endif /* _ASM_SW64_STACKTRACE_H */
diff --git a/arch/sw_64/include/asm/suspend.h b/arch/sw_64/include/asm/suspend.h
index 83fd413fd6e29c13b594e3945934fca76b6b6240..521ab099f94b80ca6f31abc0f5da5447a6debeb0 100644
--- a/arch/sw_64/include/asm/suspend.h
+++ b/arch/sw_64/include/asm/suspend.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
-#ifndef _ASM_SW64_SLEEP_H
-#define _ASM_SW64_SLEEP_H
+#ifndef _ASM_SW64_SUSPEND_H
+#define _ASM_SW64_SUSPEND_H
 
 #include <asm/hmcall.h>
 #include <asm/ptrace.h>
@@ -39,10 +39,10 @@ struct processor_state {
 	struct callee_saved_fpregs fpregs;
 	unsigned long fpcr;
 #ifdef CONFIG_HIBERNATION
-	struct pcb_struct pcb;
+	unsigned long sp;
 	struct vcpucb vcb;
 #endif
 };
 
 extern void sw64_suspend_deep_sleep(struct processor_state *state);
-#endif /* _ASM_SW64_SLEEP_H */
+#endif /* _ASM_SW64_SUSPEND_H */
diff --git a/arch/sw_64/include/asm/sw64_init.h b/arch/sw_64/include/asm/sw64_init.h
index 2d9140605d0b041694cb5c359656c9e8e9735125..893bac1c621b499d0fabeb495097616606d34fae 100644
--- a/arch/sw_64/include/asm/sw64_init.h
+++ b/arch/sw_64/include/asm/sw64_init.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
-#ifndef _ASM_SW64_INIT_H
-#define _ASM_SW64_INIT_H
+#ifndef _ASM_SW64_SW64_INIT_H
+#define _ASM_SW64_SW64_INIT_H
 
 #include <linux/cpu.h>
 #include <linux/pci.h>
@@ -32,7 +32,7 @@ struct sw64_chip_init_ops {
 struct sw64_chip_ops {
 	int (*get_cpu_num)(void);
 	void (*device_interrupt)(unsigned long irq_info);
-	void (*suspend)(int wake);
+	void (*suspend)(bool wake);
 	void (*fixup)(void);
 };
 
@@ -43,4 +43,4 @@ extern struct sw64_chip_init_ops *sw64_chip_init;
 
 DECLARE_PER_CPU(unsigned long, hard_node_id);
 
-#endif    /* _ASM_SW64_INIT_H */
+#endif /* _ASM_SW64_SW64_INIT_H */
diff --git a/arch/sw_64/include/asm/sw64io.h b/arch/sw_64/include/asm/sw64io.h
index 7d79a5b75090d82b2df8cde97918405cc4dcbb3c..0892356b8e6b7ad6cca0d81906afbcd48a24df33 100644
--- a/arch/sw_64/include/asm/sw64io.h
+++ b/arch/sw_64/include/asm/sw64io.h
@@ -95,4 +95,4 @@ sw64_io_write(unsigned long node, unsigned long reg, unsigned long data)
 	addr = __va(SW64_IO_BASE(node) | reg);
 	writeq(data, addr);
 }
-#endif
+#endif /* _ASM_SW64_SW64IO_H */
diff --git a/arch/sw_64/include/asm/switch_to.h b/arch/sw_64/include/asm/switch_to.h
index d503fc59390f51d0ba34d21785f0ac1811b453a8..e5596a735b2dbb4ef3fc98b86953db3c2666cd28 100644
--- a/arch/sw_64/include/asm/switch_to.h
+++ b/arch/sw_64/include/asm/switch_to.h
@@ -6,27 +6,39 @@
 
 extern void __fpstate_save(struct task_struct *save_to);
 extern void __fpstate_restore(struct task_struct *restore_from);
-extern struct task_struct *__switch_to(unsigned long pcb,
-		struct task_struct *prev, struct task_struct *next);
+extern struct task_struct *__switch_to(struct task_struct *prev,
+				       struct task_struct *next);
 extern void restore_da_match_after_sched(void);
 
-static inline void fpstate_save(struct task_struct *task)
+static inline void aux_save(struct task_struct *task)
 {
-	if (likely(!(task->flags & PF_KTHREAD)))
+	struct pcb_struct *pcb;
+
+	if (likely(!(task->flags & PF_KTHREAD))) {
+		pcb = &task_thread_info(task)->pcb;
+		pcb->usp = rdusp();
+		pcb->tp = rtid();
 		__fpstate_save(task);
+	}
 }
 
-static inline void fpstate_restore(struct task_struct *task)
+static inline void aux_restore(struct task_struct *task)
 {
-	if (likely(!(task->flags & PF_KTHREAD)))
+	struct pcb_struct *pcb;
+
+	if (likely(!(task->flags & PF_KTHREAD))) {
+		pcb = &task_thread_info(task)->pcb;
+		wrusp(pcb->usp);
+		wrtp(pcb->tp);
 		__fpstate_restore(task);
+	}
 }
 
 static inline void __switch_to_aux(struct task_struct *prev,
 				   struct task_struct *next)
 {
-	fpstate_save(prev);
-	fpstate_restore(next);
+	aux_save(prev);
+	aux_restore(next);
 }
 
 
@@ -34,10 +46,8 @@ static inline void __switch_to_aux(struct task_struct *prev,
 do {									\
 	struct task_struct *__prev = (prev);				\
 	struct task_struct *__next = (next);				\
-	__u64 __nextpcb = virt_to_phys(&task_thread_info(__next)->pcb);	\
 	__switch_to_aux(__prev, __next);				\
-	(last) = __switch_to(__nextpcb, __prev, __next);		\
-	check_mmu_context();						\
+	(last) = __switch_to(__prev, __next);				\
 } while (0)
 
 
diff --git a/arch/sw_64/include/asm/syscall.h b/arch/sw_64/include/asm/syscall.h
index 4b784c3d846b562e98c1af22e4fde90380169854..0b1556a460b463ab5962ce7368768fdfcf8a510e 100644
--- a/arch/sw_64/include/asm/syscall.h
+++ b/arch/sw_64/include/asm/syscall.h
@@ -72,4 +72,4 @@ static inline int syscall_get_arch(struct task_struct *task)
 	return AUDIT_ARCH_SW64;
 }
 
-#endif  /* _ASM_SW64_SYSCALL_H */
+#endif /* _ASM_SW64_SYSCALL_H */
diff --git a/arch/sw_64/include/asm/tc.h b/arch/sw_64/include/asm/tc.h
index f995a2a75f8550afde45213b51e785d16963a073..aa39c3528e3fa923022365c1db31c1ef2c61e220 100644
--- a/arch/sw_64/include/asm/tc.h
+++ b/arch/sw_64/include/asm/tc.h
@@ -13,4 +13,4 @@ static inline unsigned long rdtc(void)
 extern void tc_sync_clear(void);
 extern void tc_sync_ready(void *ignored);
 extern void tc_sync_set(void);
-#endif
+#endif /* _ASM_SW64_TC_H */
diff --git a/arch/sw_64/include/asm/termios.h b/arch/sw_64/include/asm/termios.h
index ef509946675a1f46e53284b272f6a652ded543e3..9849dd9b58bf60ff09edd0b11810de31b6a13223 100644
--- a/arch/sw_64/include/asm/termios.h
+++ b/arch/sw_64/include/asm/termios.h
@@ -78,4 +78,4 @@
 #define kernel_termios_to_user_termios(u, k) \
 	copy_to_user(u, k, sizeof(struct termios))
 
-#endif	/* _ASM_SW64_TERMIOS_H */
+#endif /* _ASM_SW64_TERMIOS_H */
diff --git a/arch/sw_64/include/asm/thread_info.h b/arch/sw_64/include/asm/thread_info.h
index 33b95f815448456b0bdd8723f102dc826285ab96..31740003d0b2d31204dc6ea36a14a7b9c8284595 100644
--- a/arch/sw_64/include/asm/thread_info.h
+++ b/arch/sw_64/include/asm/thread_info.h
@@ -15,13 +15,8 @@ typedef struct {
 
 
 struct pcb_struct {
-	unsigned long ksp;
 	unsigned long usp;
-	unsigned long ptbr;
-	unsigned int pcc;
-	unsigned int asn;
-	unsigned long unique;
-	unsigned long flags;
+	unsigned long tp;
 	unsigned long da_match, da_mask;
 	unsigned long dv_match, dv_mask;
 	unsigned long dc_ctl;
@@ -39,14 +34,19 @@ struct thread_info {
 	int			preempt_count;	/* 0 => preemptible, <0 => BUG */
 	unsigned int		status;		/* thread-synchronous flags */
 
-	int bpt_nsaved;
-	unsigned long bpt_addr[2];		/* breakpoint handling  */
-	unsigned int bpt_insn[2];
 #ifdef CONFIG_DYNAMIC_FTRACE
 	unsigned long		dyn_ftrace_addr;
 #endif
 };
 
+static __always_inline u64 rtid(void)
+{
+	u64 val;
+
+	asm volatile("rtid %0" : "=r" (val) : :);
+	return val;
+}
+
 /*
  * Macros/functions for gaining access to the thread information structure.
  */
diff --git a/arch/sw_64/include/asm/timex.h b/arch/sw_64/include/asm/timex.h
index 9065e39a0466cff54233f124bc34a053d9b8df3f..235197b0d1fd825713acafa7b5e3dc1a1ba25062 100644
--- a/arch/sw_64/include/asm/timex.h
+++ b/arch/sw_64/include/asm/timex.h
@@ -21,4 +21,4 @@ static inline cycles_t get_cycles(void)
 	return rdtc();
 }
 
-#endif
+#endif /* _ASM_SW64_TIMEX_H */
diff --git a/arch/sw_64/include/asm/tlb.h b/arch/sw_64/include/asm/tlb.h
index 4902624dba888d81d0c3ba6d036eb977218d544b..67ce55fc4c43cbade95cf356b317061bb0cb34db 100644
--- a/arch/sw_64/include/asm/tlb.h
+++ b/arch/sw_64/include/asm/tlb.h
@@ -15,4 +15,4 @@
 
 #define __pud_free_tlb(tlb, pud, address)	pud_free((tlb)->mm, pud)
 
-#endif
+#endif /* _ASM_SW64_TLB_H */
diff --git a/arch/sw_64/include/asm/tlbflush.h b/arch/sw_64/include/asm/tlbflush.h
index 7805bb28725792fe480c98bdf31a645df573d200..b35af83e6ec271f27819b2f4a5c29ea13d16d0bf 100644
--- a/arch/sw_64/include/asm/tlbflush.h
+++ b/arch/sw_64/include/asm/tlbflush.h
@@ -8,13 +8,26 @@
 #include <asm/pgalloc.h>
 #include <asm/hw_init.h>
 #include <asm/hmcall.h>
-
-extern void __load_new_mm_context(struct mm_struct *);
-
+#include <asm/mmu_context.h>
 
 static inline void flush_tlb_current(struct mm_struct *mm)
 {
-	__load_new_mm_context(mm);
+	unsigned long mmc, asn, ptbr, flags;
+
+	local_irq_save(flags);
+
+	mmc = __get_new_mm_context(mm, smp_processor_id());
+	mm->context.asid[smp_processor_id()] = mmc;
+
+	/*
+	 * Force a new ASN for a task. Note that there is no way to
+	 * write UPN only now, so call load_asn_ptbr here.
+	 */
+	asn = mmc & HARDWARE_ASN_MASK;
+	ptbr = virt_to_pfn(mm->pgd);
+	load_asn_ptbr(asn, ptbr);
+
+	local_irq_restore(flags);
 }
 
 /*
@@ -27,12 +40,10 @@ static inline void flush_tlb_current_page(struct mm_struct *mm,
 					  struct vm_area_struct *vma,
 					  unsigned long addr)
 {
-	if (vma->vm_flags & VM_EXEC) {
-		tbi(3, addr);
-		if (icache_is_vivt_no_ictag())
-			imb();
-	} else
-		tbi(2, addr);
+	if (vma->vm_flags & VM_EXEC)
+		tbis(addr);
+	else
+		tbisd(addr);
 }
 
 
@@ -65,7 +76,7 @@ static inline void flush_tlb_other(struct mm_struct *mm)
  */
 static inline void flush_tlb_all(void)
 {
-	tbia();
+	tbiv();
 }
 
 /* Flush a specified user mapping.  */
diff --git a/arch/sw_64/include/asm/uaccess.h b/arch/sw_64/include/asm/uaccess.h
index ceacfaa07cfb2f76ba3602a209d33ad7083c613c..730121aad1840459b94d01737bfb50848c878246 100644
--- a/arch/sw_64/include/asm/uaccess.h
+++ b/arch/sw_64/include/asm/uaccess.h
@@ -292,6 +292,8 @@ raw_copy_to_user(void __user *to, const void *from, unsigned long len)
 {
 	return __copy_user((__force void *)to, from, len);
 }
+#define INLINE_COPY_FROM_USER
+#define INLINE_COPY_TO_USER
 
 extern long __clear_user(void __user *to, long len);
 
diff --git a/arch/sw_64/include/asm/vcpu.h b/arch/sw_64/include/asm/vcpu.h
index 5b3fe80aed1b47e457690694a8cb2c5be3bac57d..c43ebe72e3a1e513b4e8f4e718e1190cc34a8d8e 100644
--- a/arch/sw_64/include/asm/vcpu.h
+++ b/arch/sw_64/include/asm/vcpu.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 #ifndef _ASM_SW64_VCPU_H
-#define  _ASM_SW64_VCPU_H
+#define _ASM_SW64_VCPU_H
 
 #ifndef __ASSEMBLY__
 
@@ -32,7 +32,7 @@ struct vcpucb {
 	unsigned long vcpu_irq_disabled;
 	unsigned long vcpu_irq;
 	unsigned long ptbr;
-	unsigned long int_stat0;
+	unsigned long tid;
 	unsigned long int_stat1;
 	unsigned long int_stat2;
 	unsigned long int_stat3;
@@ -41,7 +41,17 @@ struct vcpucb {
 	unsigned long exit_reason;
 	unsigned long ipaddr;
 	unsigned long vcpu_irq_vector;
+	unsigned long pri_base;
+	unsigned long stack_pc_dfault;
+	unsigned long guest_p20;
+	unsigned long guest_dfault_double;
+	unsigned long guest_irqs_pending;
+	unsigned long guest_hm_r30;
+	unsigned long migration_mark;
+	unsigned long guest_longtime;
+	unsigned long guest_longtime_offset;
+	unsigned long reserved[3];
 };
 
-#endif  /* __ASSEMBLY__ */
-#endif  /* _ASM_SW64_VCPU_H */
+#endif /* __ASSEMBLY__ */
+#endif /* _ASM_SW64_VCPU_H */
diff --git a/arch/sw_64/include/asm/wrperfmon.h b/arch/sw_64/include/asm/wrperfmon.h
index 15f7f6beb07c0606924c0c1e74a88762f8b88ed5..c06a05121a68be93e265928f0651bfda2ad43ac8 100644
--- a/arch/sw_64/include/asm/wrperfmon.h
+++ b/arch/sw_64/include/asm/wrperfmon.h
@@ -61,4 +61,4 @@
 #define MAX_HWEVENTS			2
 #define PMC_COUNT_MASK			((1UL << 58) - 1)
 
-#endif
+#endif /* _ASM_SW64_WRPERFMON_H */
diff --git a/arch/sw_64/include/asm/xchg.h b/arch/sw_64/include/asm/xchg.h
index bac67623da91442716be7df6b561c2bb91ae087d..ba4e6d1a27ad064aeef83ec4e4a2b2c89181093a 100644
--- a/arch/sw_64/include/asm/xchg.h
+++ b/arch/sw_64/include/asm/xchg.h
@@ -1,7 +1,10 @@
 /* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _ASM_SW64_XCHG_H
+#define _ASM_SW64_XCHG_H
+
 #ifndef _ASM_SW64_CMPXCHG_H
-#error Do not include xchg.h directly!
-#else
+#error Do not include xchg.h directly. Use cmpxchg.h
+#endif
 /*
  * xchg/xchg_local and cmpxchg/cmpxchg_local share the same code
  * except that local version do not have the expensive memory barrier.
@@ -325,4 +328,4 @@ static __always_inline unsigned long ____cmpxchg(, volatile void *ptr,
 	return old;
 }
 
-#endif
+#endif /* _ASM_SW64_XCHG_H */
diff --git a/arch/sw_64/include/asm/xor.h b/arch/sw_64/include/asm/xor.h
index af95259ed8ef90fe26a9d4a8f40fcab2c5250105..e9731f2a8f12302bf3cbf768ba915203862bdc11 100644
--- a/arch/sw_64/include/asm/xor.h
+++ b/arch/sw_64/include/asm/xor.h
@@ -844,4 +844,4 @@ static struct xor_block_template xor_block_sw64_prefetch = {
  */
 #define XOR_SELECT_TEMPLATE(FASTEST)   (&xor_block_sw64_prefetch)
 
-#endif
+#endif /* _ASM_SW64_XOR_H */
diff --git a/arch/sw_64/include/uapi/asm/errno.h b/arch/sw_64/include/uapi/asm/errno.h
index 0d8438f6bd402aea665d96c8f7d983271ad91d24..969ee99ee86c7a72c125a903e438a2373cf36c4a 100644
--- a/arch/sw_64/include/uapi/asm/errno.h
+++ b/arch/sw_64/include/uapi/asm/errno.h
@@ -4,125 +4,125 @@
 
 #include <asm-generic/errno-base.h>
 
-#undef	EAGAIN			/* 11 in errno-base.h */
-
-#define	EDEADLK		11	/* Resource deadlock would occur */
-
-#define	EAGAIN		35	/* Try again */
-#define	EWOULDBLOCK	EAGAIN	/* Operation would block */
-#define	EINPROGRESS	36	/* Operation now in progress */
-#define	EALREADY	37	/* Operation already in progress */
-#define	ENOTSOCK	38	/* Socket operation on non-socket */
-#define	EDESTADDRREQ	39	/* Destination address required */
-#define	EMSGSIZE	40	/* Message too long */
-#define	EPROTOTYPE	41	/* Protocol wrong type for socket */
-#define	ENOPROTOOPT	42	/* Protocol not available */
-#define	EPROTONOSUPPORT	43	/* Protocol not supported */
-#define	ESOCKTNOSUPPORT	44	/* Socket type not supported */
-#define	EOPNOTSUPP	45	/* Operation not supported on transport endpoint */
-#define	EPFNOSUPPORT	46	/* Protocol family not supported */
-#define	EAFNOSUPPORT	47	/* Address family not supported by protocol */
-#define	EADDRINUSE	48	/* Address already in use */
-#define	EADDRNOTAVAIL	49	/* Cannot assign requested address */
-#define	ENETDOWN	50	/* Network is down */
-#define	ENETUNREACH	51	/* Network is unreachable */
-#define	ENETRESET	52	/* Network dropped connection because of reset */
-#define	ECONNABORTED	53	/* Software caused connection abort */
-#define	ECONNRESET	54	/* Connection reset by peer */
-#define	ENOBUFS		55	/* No buffer space available */
-#define	EISCONN		56	/* Transport endpoint is already connected */
-#define	ENOTCONN	57	/* Transport endpoint is not connected */
-#define	ESHUTDOWN	58	/* Cannot send after transport endpoint shutdown */
-#define	ETOOMANYREFS	59	/* Too many references: cannot splice */
-#define	ETIMEDOUT	60	/* Connection timed out */
-#define	ECONNREFUSED	61	/* Connection refused */
-#define	ELOOP		62	/* Too many symbolic links encountered */
-#define	ENAMETOOLONG	63	/* File name too long */
-#define	EHOSTDOWN	64	/* Host is down */
-#define	EHOSTUNREACH	65	/* No route to host */
-#define	ENOTEMPTY	66	/* Directory not empty */
-
-#define	EUSERS		68	/* Too many users */
-#define	EDQUOT		69	/* Quota exceeded */
-#define	ESTALE		70	/* Stale NFS file handle */
-#define	EREMOTE		71	/* Object is remote */
-
-#define	ENOLCK		77	/* No record locks available */
-#define	ENOSYS		78	/* Function not implemented */
-
-#define	ENOMSG		80	/* No message of desired type */
-#define	EIDRM		81	/* Identifier removed */
-#define	ENOSR		82	/* Out of streams resources */
-#define	ETIME		83	/* Timer expired */
-#define	EBADMSG		84	/* Not a data message */
-#define	EPROTO		85	/* Protocol error */
-#define	ENODATA		86	/* No data available */
-#define	ENOSTR		87	/* Device not a stream */
-
-#define	ENOPKG		92	/* Package not installed */
-
-#define	EILSEQ		116	/* Illegal byte sequence */
+#undef EAGAIN			/* 11 in errno-base.h */
+
+#define EDEADLK		11	/* Resource deadlock would occur */
+
+#define EAGAIN		35	/* Try again */
+#define EWOULDBLOCK	EAGAIN	/* Operation would block */
+#define EINPROGRESS	36	/* Operation now in progress */
+#define EALREADY	37	/* Operation already in progress */
+#define ENOTSOCK	38	/* Socket operation on non-socket */
+#define EDESTADDRREQ	39	/* Destination address required */
+#define EMSGSIZE	40	/* Message too long */
+#define EPROTOTYPE	41	/* Protocol wrong type for socket */
+#define ENOPROTOOPT	42	/* Protocol not available */
+#define EPROTONOSUPPORT	43	/* Protocol not supported */
+#define ESOCKTNOSUPPORT	44	/* Socket type not supported */
+#define EOPNOTSUPP	45	/* Operation not supported on transport endpoint */
+#define EPFNOSUPPORT	46	/* Protocol family not supported */
+#define EAFNOSUPPORT	47	/* Address family not supported by protocol */
+#define EADDRINUSE	48	/* Address already in use */
+#define EADDRNOTAVAIL	49	/* Cannot assign requested address */
+#define ENETDOWN	50	/* Network is down */
+#define ENETUNREACH	51	/* Network is unreachable */
+#define ENETRESET	52	/* Network dropped connection because of reset */
+#define ECONNABORTED	53	/* Software caused connection abort */
+#define ECONNRESET	54	/* Connection reset by peer */
+#define ENOBUFS		55	/* No buffer space available */
+#define EISCONN		56	/* Transport endpoint is already connected */
+#define ENOTCONN	57	/* Transport endpoint is not connected */
+#define ESHUTDOWN	58	/* Cannot send after transport endpoint shutdown */
+#define ETOOMANYREFS	59	/* Too many references: cannot splice */
+#define ETIMEDOUT	60	/* Connection timed out */
+#define ECONNREFUSED	61	/* Connection refused */
+#define ELOOP		62	/* Too many symbolic links encountered */
+#define ENAMETOOLONG	63	/* File name too long */
+#define EHOSTDOWN	64	/* Host is down */
+#define EHOSTUNREACH	65	/* No route to host */
+#define ENOTEMPTY	66	/* Directory not empty */
+
+#define EUSERS		68	/* Too many users */
+#define EDQUOT		69	/* Quota exceeded */
+#define ESTALE		70	/* Stale NFS file handle */
+#define EREMOTE		71	/* Object is remote */
+
+#define ENOLCK		77	/* No record locks available */
+#define ENOSYS		78	/* Function not implemented */
+
+#define ENOMSG		80	/* No message of desired type */
+#define EIDRM		81	/* Identifier removed */
+#define ENOSR		82	/* Out of streams resources */
+#define ETIME		83	/* Timer expired */
+#define EBADMSG		84	/* Not a data message */
+#define EPROTO		85	/* Protocol error */
+#define ENODATA		86	/* No data available */
+#define ENOSTR		87	/* Device not a stream */
+
+#define ENOPKG		92	/* Package not installed */
+
+#define EILSEQ		116	/* Illegal byte sequence */
 
 /* The following are just random noise.. */
-#define	ECHRNG		88	/* Channel number out of range */
-#define	EL2NSYNC	89	/* Level 2 not synchronized */
-#define	EL3HLT		90	/* Level 3 halted */
-#define	EL3RST		91	/* Level 3 reset */
-
-#define	ELNRNG		93	/* Link number out of range */
-#define	EUNATCH		94	/* Protocol driver not attached */
-#define	ENOCSI		95	/* No CSI structure available */
-#define	EL2HLT		96	/* Level 2 halted */
-#define	EBADE		97	/* Invalid exchange */
-#define	EBADR		98	/* Invalid request descriptor */
-#define	EXFULL		99	/* Exchange full */
-#define	ENOANO		100	/* No anode */
-#define	EBADRQC		101	/* Invalid request code */
-#define	EBADSLT		102	/* Invalid slot */
-
-#define	EDEADLOCK	EDEADLK
-
-#define	EBFONT		104	/* Bad font file format */
-#define	ENONET		105	/* Machine is not on the network */
-#define	ENOLINK		106	/* Link has been severed */
-#define	EADV		107	/* Advertise error */
-#define	ESRMNT		108	/* Srmount error */
-#define	ECOMM		109	/* Communication error on send */
-#define	EMULTIHOP	110	/* Multihop attempted */
-#define	EDOTDOT		111	/* RFS specific error */
-#define	EOVERFLOW	112	/* Value too large for defined data type */
-#define	ENOTUNIQ	113	/* Name not unique on network */
-#define	EBADFD		114	/* File descriptor in bad state */
-#define	EREMCHG		115	/* Remote address changed */
-
-#define	EUCLEAN		117	/* Structure needs cleaning */
-#define	ENOTNAM		118	/* Not a XENIX named type file */
-#define	ENAVAIL		119	/* No XENIX semaphores available */
-#define	EISNAM		120	/* Is a named type file */
-#define	EREMOTEIO	121	/* Remote I/O error */
-
-#define	ELIBACC		122	/* Can not access a needed shared library */
-#define	ELIBBAD		123	/* Accessing a corrupted shared library */
-#define	ELIBSCN		124	/* .lib section in a.out corrupted */
-#define	ELIBMAX		125	/* Attempting to link in too many shared libraries */
-#define	ELIBEXEC	126	/* Cannot exec a shared library directly */
-#define	ERESTART	127	/* Interrupted system call should be restarted */
-#define	ESTRPIPE	128	/* Streams pipe error */
+#define ECHRNG		88	/* Channel number out of range */
+#define EL2NSYNC	89	/* Level 2 not synchronized */
+#define EL3HLT		90	/* Level 3 halted */
+#define EL3RST		91	/* Level 3 reset */
+
+#define ELNRNG		93	/* Link number out of range */
+#define EUNATCH		94	/* Protocol driver not attached */
+#define ENOCSI		95	/* No CSI structure available */
+#define EL2HLT		96	/* Level 2 halted */
+#define EBADE		97	/* Invalid exchange */
+#define EBADR		98	/* Invalid request descriptor */
+#define EXFULL		99	/* Exchange full */
+#define ENOANO		100	/* No anode */
+#define EBADRQC		101	/* Invalid request code */
+#define EBADSLT		102	/* Invalid slot */
+
+#define EDEADLOCK	EDEADLK
+
+#define EBFONT		104	/* Bad font file format */
+#define ENONET		105	/* Machine is not on the network */
+#define ENOLINK		106	/* Link has been severed */
+#define EADV		107	/* Advertise error */
+#define ESRMNT		108	/* Srmount error */
+#define ECOMM		109	/* Communication error on send */
+#define EMULTIHOP	110	/* Multihop attempted */
+#define EDOTDOT		111	/* RFS specific error */
+#define EOVERFLOW	112	/* Value too large for defined data type */
+#define ENOTUNIQ	113	/* Name not unique on network */
+#define EBADFD		114	/* File descriptor in bad state */
+#define EREMCHG		115	/* Remote address changed */
+
+#define EUCLEAN		117	/* Structure needs cleaning */
+#define ENOTNAM		118	/* Not a XENIX named type file */
+#define ENAVAIL		119	/* No XENIX semaphores available */
+#define EISNAM		120	/* Is a named type file */
+#define EREMOTEIO	121	/* Remote I/O error */
+
+#define ELIBACC		122	/* Can not access a needed shared library */
+#define ELIBBAD		123	/* Accessing a corrupted shared library */
+#define ELIBSCN		124	/* .lib section in a.out corrupted */
+#define ELIBMAX		125	/* Attempting to link in too many shared libraries */
+#define ELIBEXEC	126	/* Cannot exec a shared library directly */
+#define ERESTART	127	/* Interrupted system call should be restarted */
+#define ESTRPIPE	128	/* Streams pipe error */
 
 #define ENOMEDIUM	129	/* No medium found */
 #define EMEDIUMTYPE	130	/* Wrong medium type */
-#define	ECANCELED	131	/* Operation Cancelled */
-#define	ENOKEY		132	/* Required key not available */
-#define	EKEYEXPIRED	133	/* Key has expired */
-#define	EKEYREVOKED	134	/* Key has been revoked */
-#define	EKEYREJECTED	135	/* Key was rejected by service */
+#define ECANCELED	131	/* Operation Cancelled */
+#define ENOKEY		132	/* Required key not available */
+#define EKEYEXPIRED	133	/* Key has expired */
+#define EKEYREVOKED	134	/* Key has been revoked */
+#define EKEYREJECTED	135	/* Key was rejected by service */
 
 /* for robust mutexes */
-#define	EOWNERDEAD	136	/* Owner died */
-#define	ENOTRECOVERABLE	137	/* State not recoverable */
+#define EOWNERDEAD	136	/* Owner died */
+#define ENOTRECOVERABLE	137	/* State not recoverable */
 
-#define	ERFKILL		138	/* Operation not possible due to RF-kill */
+#define ERFKILL		138	/* Operation not possible due to RF-kill */
 
 #define EHWPOISON	139	/* Memory page has hardware error */
 
-#endif
+#endif /* _UAPI_ASM_SW64_ERRNO_H */
diff --git a/arch/sw_64/include/uapi/asm/fcntl.h b/arch/sw_64/include/uapi/asm/fcntl.h
index 99e1a31c5e8606000808bd62c6a6db25c0982501..be2daae2cc4dc1a1f7d9a8b653ade486e7329db9 100644
--- a/arch/sw_64/include/uapi/asm/fcntl.h
+++ b/arch/sw_64/include/uapi/asm/fcntl.h
@@ -55,4 +55,4 @@
 
 #include <asm-generic/fcntl.h>
 
-#endif
+#endif /* _UAPI_ASM_SW64_FCNTL_H */
diff --git a/arch/sw_64/include/uapi/asm/hmcall.h b/arch/sw_64/include/uapi/asm/hmcall.h
index f10378ba99c8042db62524c013ba64a44a5c2d48..6867fb7b4d244ee325b1d6294d1cae7453246af2 100644
--- a/arch/sw_64/include/uapi/asm/hmcall.h
+++ b/arch/sw_64/include/uapi/asm/hmcall.h
@@ -7,9 +7,11 @@
 #define HMC_bpt			0x80
 #define HMC_callsys		0x83
 #define HMC_imb			0x86
-#define HMC_rdunique		0x9E
-#define HMC_wrunique		0x9F
+#define HMC_rdtp		0x9E
+#define HMC_wrtp		0x9F
+#define HMC_rdunique		HMC_rdtp
+#define HMC_wrunique		HMC_wrtp
 #define HMC_gentrap		0xAA
 #define HMC_wrperfmon		0xB0
 
-#endif
+#endif /* _UAPI_ASM_SW64_HMCALL_H */
diff --git a/arch/sw_64/include/uapi/asm/ioctls.h b/arch/sw_64/include/uapi/asm/ioctls.h
index db8e456290e6592da5e15e7d68b4449729b9ffb9..751a07fd0303d29f0979a2c9acb051b4329612e0 100644
--- a/arch/sw_64/include/uapi/asm/ioctls.h
+++ b/arch/sw_64/include/uapi/asm/ioctls.h
@@ -34,8 +34,8 @@
 
 #define TIOCSWINSZ	_IOW('t', 103, struct winsize)
 #define TIOCGWINSZ	_IOR('t', 104, struct winsize)
-#define	TIOCSTART	_IO('t', 110)		/* start output, like ^Q */
-#define	TIOCSTOP	_IO('t', 111)		/* stop output, like ^S */
+#define TIOCSTART	_IO('t', 110)		/* start output, like ^Q */
+#define TIOCSTOP	_IO('t', 111)		/* stop output, like ^S */
 #define TIOCOUTQ	_IOR('t', 115, int)	/* output queue size */
 
 #define TIOCGLTC	_IOR('t', 116, struct ltchars)
diff --git a/arch/sw_64/include/uapi/asm/kvm.h b/arch/sw_64/include/uapi/asm/kvm.h
index 126c2a1d74110ef6d8eb12b2314e2193c2d5e823..254d6cbf1eb146c53c766c715f2980fb3017ecf5 100644
--- a/arch/sw_64/include/uapi/asm/kvm.h
+++ b/arch/sw_64/include/uapi/asm/kvm.h
@@ -114,4 +114,4 @@ struct kvm_sync_regs {
 struct kvm_sregs {
 };
 
-#endif  /* _UAPI_ASM_SW64_KVM_H */
+#endif /* _UAPI_ASM_SW64_KVM_H */
diff --git a/arch/sw_64/include/uapi/asm/perf_regs.h b/arch/sw_64/include/uapi/asm/perf_regs.h
index 1378a7397951d6ba3085cf64bdda6ef0453a3932..febde5fd72fb7f95b442060352e6c4a38a7e8fbc 100644
--- a/arch/sw_64/include/uapi/asm/perf_regs.h
+++ b/arch/sw_64/include/uapi/asm/perf_regs.h
@@ -1,7 +1,7 @@
 /* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
 
-#ifndef _ASM_SW64_PERF_REGS_H
-#define _ASM_SW64_PERF_REGS_H
+#ifndef _UAPI_ASM_SW64_PERF_REGS_H
+#define _UAPI_ASM_SW64_PERF_REGS_H
 
 enum perf_event_sw64_regs {
 	PERF_REG_SW64_R0,
@@ -42,4 +42,4 @@ enum perf_event_sw64_regs {
 	PERF_REG_SW64_R18,
 	PERF_REG_SW64_MAX,
 };
-#endif /* _ASM_SW64_PERF_REGS_H */
+#endif /* _UAPI_ASM_SW64_PERF_REGS_H */
diff --git a/arch/sw_64/include/uapi/asm/ptrace.h b/arch/sw_64/include/uapi/asm/ptrace.h
index 80bad067fc15523e92a61f1cfed0a159e7803677..5cf3ca1d3dd843b0dc18b7c7452e9b7220d9359b 100644
--- a/arch/sw_64/include/uapi/asm/ptrace.h
+++ b/arch/sw_64/include/uapi/asm/ptrace.h
@@ -36,7 +36,8 @@ struct user_fpsimd_state {
 #define FPREG_END		62
 #define FPCR			63
 #define PC			64
-#define UNIQUE			65
+#define TP			65
+#define UNIQUE			TP
 #define VECREG_BASE		67
 #define VECREG_END		161
 #define F31_V1			98
diff --git a/arch/sw_64/include/uapi/asm/regdef.h b/arch/sw_64/include/uapi/asm/regdef.h
index ad4475b7943517e62b8f0935c442a260bed9fa22..7460a987c7267b85ffbe5238cf258944e7c0309c 100644
--- a/arch/sw_64/include/uapi/asm/regdef.h
+++ b/arch/sw_64/include/uapi/asm/regdef.h
@@ -13,14 +13,14 @@
 #define t6	$7
 #define t7	$8
 
-#define	s0	$9	/* saved-registers (callee-saved registers) */
-#define	s1	$10
-#define	s2	$11
-#define	s3	$12
-#define	s4	$13
-#define	s5	$14
-#define	s6	$15
-#define	fp	s6	/* frame-pointer (s6 in frame-less procedures) */
+#define s0	$9	/* saved-registers (callee-saved registers) */
+#define s1	$10
+#define s2	$11
+#define s3	$12
+#define s4	$13
+#define s5	$14
+#define s6	$15
+#define fp	s6	/* frame-pointer (s6 in frame-less procedures) */
 
 #define a0	$16	/* argument registers (caller-saved) */
 #define a1	$17
diff --git a/arch/sw_64/include/uapi/asm/setup.h b/arch/sw_64/include/uapi/asm/setup.h
index 10ce5dba9c3066a3334f8ecfe59f569403024178..e6cca45250491ca64bfc6a6c473da19f01984bb9 100644
--- a/arch/sw_64/include/uapi/asm/setup.h
+++ b/arch/sw_64/include/uapi/asm/setup.h
@@ -4,4 +4,4 @@
 
 #define COMMAND_LINE_SIZE   2048
 
-#endif
+#endif /* _UAPI_ASM_SW64_SETUP_H */
diff --git a/arch/sw_64/include/uapi/asm/sigcontext.h b/arch/sw_64/include/uapi/asm/sigcontext.h
index facbf34e920d4f57b2ba51069e760920f8cde730..08a0814703830370efae670d59f4262b7da50dbd 100644
--- a/arch/sw_64/include/uapi/asm/sigcontext.h
+++ b/arch/sw_64/include/uapi/asm/sigcontext.h
@@ -2,15 +2,13 @@
 #ifndef _UAPI_ASM_SW64_SIGCONTEXT_H
 #define _UAPI_ASM_SW64_SIGCONTEXT_H
 
+/*
+ * Signal context structure
+ *
+ * The context is saved before a signal handler is invoked, and it is
+ * restored by sys_sigreturn / sys_rt_sigreturn.
+ */
 struct sigcontext {
-	/*
-	 * What should we have here? I'd probably better use the same
-	 * stack layout as DEC Unix, just in case we ever want to try
-	 * running their binaries..
-	 *
-	 * This is the basic layout, but I don't know if we'll ever
-	 * actually fill in all the values..
-	 */
 	long		sc_onstack;
 	long		sc_mask;
 	long		sc_pc;
@@ -19,6 +17,7 @@ struct sigcontext {
 	long		sc_ownedfp;
 	long		sc_fpregs[128];	/* SIMD-FP */
 	unsigned long	sc_fpcr;
+	/* TODO: Following are unused, to be removed and synced with libc */
 	unsigned long	sc_fp_control;
 	unsigned long	sc_reserved1, sc_reserved2;
 	unsigned long	sc_ssize;
@@ -32,4 +31,4 @@ struct sigcontext {
 };
 
 
-#endif
+#endif /* _UAPI_ASM_SW64_SIGCONTEXT_H */
diff --git a/arch/sw_64/include/uapi/asm/siginfo.h b/arch/sw_64/include/uapi/asm/siginfo.h
index 4a58eea9b67c9ab254f7142aeb37bf70758b5d8a..28c656c283132e1771179391d0c5a95144b470e4 100644
--- a/arch/sw_64/include/uapi/asm/siginfo.h
+++ b/arch/sw_64/include/uapi/asm/siginfo.h
@@ -8,4 +8,4 @@
 #include <asm-generic/siginfo.h>
 
 
-#endif
+#endif /* _UAPI_ASM_SW64_SIGINFO_H */
diff --git a/arch/sw_64/include/uapi/asm/stat.h b/arch/sw_64/include/uapi/asm/stat.h
index d2b21128c56947bfeef183113ba6a51e149a91a1..25aad21f4d31c7beda0f023f8a0dd7e21db52fac 100644
--- a/arch/sw_64/include/uapi/asm/stat.h
+++ b/arch/sw_64/include/uapi/asm/stat.h
@@ -48,4 +48,4 @@ struct stat64 {
 	long		__unused[3];
 };
 
-#endif
+#endif /* _UAPI_ASM_SW64_STAT_H */
diff --git a/arch/sw_64/include/uapi/asm/termbits.h b/arch/sw_64/include/uapi/asm/termbits.h
index 83de6ff63234f69a7ddf3b169c4e204961640d3d..aaadb1d54f92e585db9b843444c1d6a198a20fe9 100644
--- a/arch/sw_64/include/uapi/asm/termbits.h
+++ b/arch/sw_64/include/uapi/asm/termbits.h
@@ -87,73 +87,73 @@ struct ktermios {
 #define OFILL		00000100
 #define OFDEL		00000200
 #define NLDLY		00001400
-#define   NL0		00000000
-#define   NL1		00000400
-#define   NL2		00001000
-#define   NL3		00001400
+#define NL0		00000000
+#define NL1		00000400
+#define NL2		00001000
+#define NL3		00001400
 #define TABDLY		00006000
-#define   TAB0		00000000
-#define   TAB1		00002000
-#define   TAB2		00004000
-#define   TAB3		00006000
-#define	CRDLY		00030000
-#define   CR0		00000000
-#define   CR1		00010000
-#define   CR2		00020000
-#define   CR3		00030000
+#define TAB0		00000000
+#define TAB1		00002000
+#define TAB2		00004000
+#define TAB3		00006000
+#define CRDLY		00030000
+#define CR0		00000000
+#define CR1		00010000
+#define CR2		00020000
+#define CR3		00030000
 #define FFDLY		00040000
-#define   FF0		00000000
-#define   FF1		00040000
+#define FF0		00000000
+#define FF1		00040000
 #define BSDLY		00100000
-#define   BS0		00000000
-#define   BS1		00100000
+#define BS0		00000000
+#define BS1		00100000
 #define VTDLY		00200000
-#define   VT0		00000000
-#define   VT1		00200000
+#define VT0		00000000
+#define VT1		00200000
 #define XTABS		01000000	/* Hmm.. Linux/i386 considers this part of TABDLY.. */
 
 /* c_cflag bit meaning */
 #define CBAUD		0000037
-#define  B0		0000000		/* hang up */
-#define  B50		0000001
-#define  B75		0000002
-#define  B110		0000003
-#define  B134		0000004
-#define  B150		0000005
-#define  B200		0000006
-#define  B300		0000007
-#define  B600		0000010
-#define  B1200		0000011
-#define  B1800		0000012
-#define  B2400		0000013
-#define  B4800		0000014
-#define  B9600		0000015
-#define  B19200		0000016
-#define  B38400		0000017
+#define B0		0000000		/* hang up */
+#define B50		0000001
+#define B75		0000002
+#define B110		0000003
+#define B134		0000004
+#define B150		0000005
+#define B200		0000006
+#define B300		0000007
+#define B600		0000010
+#define B1200		0000011
+#define B1800		0000012
+#define B2400		0000013
+#define B4800		0000014
+#define B9600		0000015
+#define B19200		0000016
+#define B38400		0000017
 #define EXTA		B19200
 #define EXTB		B38400
 #define CBAUDEX		0000000
-#define  B57600		00020
-#define  B115200	00021
-#define  B230400	00022
-#define  B460800	00023
-#define  B500000	00024
-#define  B576000	00025
-#define  B921600	00026
-#define  B1000000	00027
-#define  B1152000	00030
-#define  B1500000	00031
-#define  B2000000	00032
-#define  B2500000	00033
-#define  B3000000	00034
-#define  B3500000	00035
-#define  B4000000	00036
+#define B57600		00020
+#define B115200	00021
+#define B230400	00022
+#define B460800	00023
+#define B500000	00024
+#define B576000	00025
+#define B921600	00026
+#define B1000000	00027
+#define B1152000	00030
+#define B1500000	00031
+#define B2000000	00032
+#define B2500000	00033
+#define B3000000	00034
+#define B3500000	00035
+#define B4000000	00036
 
 #define CSIZE		00001400
-#define   CS5		00000000
-#define   CS6		00000400
-#define   CS7		00001000
-#define   CS8		00001400
+#define CS5		00000000
+#define CS6		00000400
+#define CS7		00001000
+#define CS8		00001400
 
 #define CSTOPB		00002000
 #define CREAD		00004000
@@ -184,19 +184,19 @@ struct ktermios {
 #define EXTPROC		0x10000000
 
 /* Values for the ACTION argument to `tcflow'.  */
-#define	TCOOFF		0
-#define	TCOON		1
-#define	TCIOFF		2
-#define	TCION		3
+#define TCOOFF		0
+#define TCOON		1
+#define TCIOFF		2
+#define TCION		3
 
 /* Values for the QUEUE_SELECTOR argument to `tcflush'.  */
-#define	TCIFLUSH	0
-#define	TCOFLUSH	1
-#define	TCIOFLUSH	2
+#define TCIFLUSH	0
+#define TCOFLUSH	1
+#define TCIOFLUSH	2
 
 /* Values for the OPTIONAL_ACTIONS argument to `tcsetattr'.  */
-#define	TCSANOW		0
-#define	TCSADRAIN	1
-#define	TCSAFLUSH	2
+#define TCSANOW		0
+#define TCSADRAIN	1
+#define TCSAFLUSH	2
 
 #endif /* _UAPI_ASM_SW64_TERMBITS_H */
diff --git a/arch/sw_64/kernel/Makefile b/arch/sw_64/kernel/Makefile
index 8cc09dd8fdbc4c95dd60391cd405606d2cde48a1..02facabae2d9f736d121b1f8b628748af501b484 100644
--- a/arch/sw_64/kernel/Makefile
+++ b/arch/sw_64/kernel/Makefile
@@ -17,7 +17,7 @@ obj-y    := entry.o fpu.o traps.o process.o sys_sw64.o irq.o \
 	    irq_sw64.o signal.o setup.o ptrace.o time.o \
 	    systbls.o dup_print.o tc.o timer.o \
 	    insn.o early_init.o topology.o cacheinfo.o \
-	    vdso.o vdso/
+	    vdso.o vdso/ hmcall.o
 
 obj-$(CONFIG_ACPI) += acpi.o
 obj-$(CONFIG_STACKTRACE) += stacktrace.o
diff --git a/arch/sw_64/kernel/acpi.c b/arch/sw_64/kernel/acpi.c
index a0b5c4a57a07e698a8fd6e05c171e16c14a16c7e..61f2948f17818ef4d5fee71d3bef177792436c03 100644
--- a/arch/sw_64/kernel/acpi.c
+++ b/arch/sw_64/kernel/acpi.c
@@ -97,7 +97,7 @@ int acpi_unmap_lsapic(int cpu)
 	return 0;
 }
 EXPORT_SYMBOL(acpi_unmap_lsapic);
-#endif				/* CONFIG_ACPI_HOTPLUG_CPU */
+#endif /* CONFIG_ACPI_HOTPLUG_CPU */
 
 u8 acpi_checksum(u8 *table, u32 length)
 {
@@ -361,7 +361,7 @@ int acpi_unmap_cpu(int cpu)
 	return 0;
 }
 EXPORT_SYMBOL(acpi_unmap_cpu);
-#endif				/* CONFIG_ACPI_HOTPLUG_CPU */
+#endif /* CONFIG_ACPI_HOTPLUG_CPU */
 
 void __init acpi_boot_table_init(void)
 
diff --git a/arch/sw_64/kernel/asm-offsets.c b/arch/sw_64/kernel/asm-offsets.c
index 9e6c338a5edd8bc0ebef67b2ebc1ca3fdbdfc996..12b3311c1bcb75f25a825de2bf0d74083b5051c8 100644
--- a/arch/sw_64/kernel/asm-offsets.c
+++ b/arch/sw_64/kernel/asm-offsets.c
@@ -33,9 +33,8 @@ void foo(void)
 	OFFSET(PSTATE_FPREGS, processor_state, fpregs);
 	OFFSET(PSTATE_FPCR, processor_state, fpcr);
 #ifdef CONFIG_HIBERNATION
-	OFFSET(PSTATE_PCB, processor_state, pcb);
+	OFFSET(PSTATE_SP, processor_state, sp);
 #endif
-	OFFSET(PCB_KSP, pcb_struct, ksp);
 	OFFSET(PBE_ADDR, pbe, address);
 	OFFSET(PBE_ORIG_ADDR, pbe, orig_address);
 	OFFSET(PBE_NEXT, pbe, next);
@@ -89,9 +88,6 @@ void foo(void)
 	DEFINE(PT_REGS_R26, offsetof(struct pt_regs, r26));
 	DEFINE(PT_REGS_R27, offsetof(struct pt_regs, r27));
 	DEFINE(PT_REGS_R28, offsetof(struct pt_regs, r28));
-	DEFINE(PT_REGS_TRAP_A0, offsetof(struct pt_regs, trap_a0));
-	DEFINE(PT_REGS_TRAP_A1, offsetof(struct pt_regs, trap_a1));
-	DEFINE(PT_REGS_TRAP_A2, offsetof(struct pt_regs, trap_a2));
 	DEFINE(PT_REGS_PS, offsetof(struct pt_regs, ps));
 	DEFINE(PT_REGS_PC, offsetof(struct pt_regs, pc));
 	DEFINE(PT_REGS_GP, offsetof(struct pt_regs, gp));
@@ -222,4 +218,5 @@ void foo(void)
 	OFFSET(TASK_THREAD_S5, task_struct, thread.s[5]);
 	OFFSET(TASK_THREAD_S6, task_struct, thread.s[6]);
 	BLANK();
+	DEFINE(ASM_THREAD_SIZE, THREAD_SIZE);
 }
diff --git a/arch/sw_64/kernel/clock.c b/arch/sw_64/kernel/clock.c
index f31f596a00521e4b8ea193eb8adc69896f272c92..32f01d4b8255507b897873da790b7f98ba241d56 100644
--- a/arch/sw_64/kernel/clock.c
+++ b/arch/sw_64/kernel/clock.c
@@ -109,14 +109,21 @@ struct clk *sw64_clk_get(struct device *dev, const char *id)
 }
 EXPORT_SYMBOL(sw64_clk_get);
 
-unsigned long sw64_clk_get_rate(struct clk *clk)
+unsigned int __sw64_cpufreq_get(struct cpufreq_policy *policy)
 {
-	if (!clk)
-		return 0;
+	int i;
+	u64 val;
 
-	return (unsigned long)clk->rate;
+	val = sw64_io_read(0, CLK_CTL);
+	val = val >> CORE_PLL2_CFG_SHIFT;
+
+	for (i = 0; i < sizeof(cpu_freq)/sizeof(int); i++) {
+		if (cpu_freq[val] == cpu_freq[i])
+			return cpu_freq[i];
+	}
+	return 0;
 }
-EXPORT_SYMBOL(sw64_clk_get_rate);
+EXPORT_SYMBOL(__sw64_cpufreq_get);
 
 void sw64_store_policy(struct cpufreq_policy *policy)
 {
@@ -124,15 +131,17 @@ void sw64_store_policy(struct cpufreq_policy *policy)
 }
 EXPORT_SYMBOL_GPL(sw64_store_policy);
 
-int sw64_set_rate(int index, unsigned long rate)
+void sw64_set_rate(unsigned long rate)
 {
 	unsigned int i, val;
+	int index = -1;
 
 	rate /= 1000000;
 
 	for (i = 0; i < sizeof(cpu_freq)/sizeof(int); i++) {
 		if (rate == cpu_freq[i]) {
 			index = i;
+			update_cpu_freq(cpu_freq[i]);
 			break;
 		}
 	}
@@ -178,7 +187,5 @@ int sw64_set_rate(int index, unsigned long rate)
 	/* LV1 select PLL0/PLL1 */
 	sw64_io_write(0, CLU_LV1_SEL, CLK_LV1_SEL_MUXB | CLK_LV1_SEL_PRT);
 	sw64_io_write(1, CLU_LV1_SEL, CLK_LV1_SEL_MUXB | CLK_LV1_SEL_PRT);
-
-	return index;
 }
 EXPORT_SYMBOL_GPL(sw64_set_rate);
diff --git a/arch/sw_64/kernel/early_init.c b/arch/sw_64/kernel/early_init.c
index 392627bef8bb14708e56860f456ba53fd1bf5866..2f38719cc216ce2c84f0e1dfdd17d977b1324f02 100644
--- a/arch/sw_64/kernel/early_init.c
+++ b/arch/sw_64/kernel/early_init.c
@@ -23,6 +23,7 @@ static void __init sw64_setup_platform_ops(void)
 
 asmlinkage __visible void __init sw64_start_kernel(void)
 {
+	fixup_hmcall();
 	sw64_setup_chip_ops();
 	sw64_setup_platform_ops();
 	sw64_platform->ops_fixup();
diff --git a/arch/sw_64/kernel/entry-ftrace.S b/arch/sw_64/kernel/entry-ftrace.S
index 3f88a9fe2e3eb08ccd671f70e60aaa23249e33b8..223dd5fc0808bdef3c26cb90547ffe5c2b6eb38e 100644
--- a/arch/sw_64/kernel/entry-ftrace.S
+++ b/arch/sw_64/kernel/entry-ftrace.S
@@ -79,7 +79,7 @@ ftrace_graph_call:				/* ftrace_graph_caller(); */
 						/* "br ftrace_graph_caller" */
 #endif
 	mcount_end
-	ret $31, ($28), 1
+	ret	$31, ($28), 1
 	.end ftrace_caller
 #else /* !CONFIG_DYNAMIC_FTRACE */
 
diff --git a/arch/sw_64/kernel/entry.S b/arch/sw_64/kernel/entry.S
index f79c9a6ddf3692f4c5427c7b83a555ac9d68fd71..cfb8e7c6d7fba2cd9493734cfa7261e7dd2f4e11 100644
--- a/arch/sw_64/kernel/entry.S
+++ b/arch/sw_64/kernel/entry.S
@@ -14,11 +14,10 @@
 /*
  * This defines the normal kernel pt-regs layout.
  *
- * regs 9-15 preserved by C code
+ * regs 9-15 preserved by C code, saving to pt_regs will make
+ * them easier to be accessed in an unified way.
  * regs 16-18 saved by HMcode
  * regs 29-30 saved and set up by HMcode
- * JRP - Save regs 16-18 in a special area of the stack, so that
- * the hmcode-provided values are available to the signal handler.
  */
 
 	.macro SAVE_COMMON_REGS
@@ -42,9 +41,6 @@
 	stl	$25, PT_REGS_R25($sp)
 	stl	$26, PT_REGS_R26($sp)
 	stl	$27, PT_REGS_R27($sp)
-	stl	$16, PT_REGS_TRAP_A0($sp)
-	stl	$17, PT_REGS_TRAP_A1($sp)
-	stl	$18, PT_REGS_TRAP_A2($sp)
 	.endm
 
 	.macro RESTORE_COMMON_REGS
@@ -384,11 +380,10 @@ $syscall_trace_failed:
  * Integer register context switch
  * The callee-saved registers must be saved and restored.
  *
- *   a0: physical address of next task's pcb, used by hmcode
- *   a1: previous task_struct (must be preserved across the switch)
- *   a2: next task_struct
+ *   a0: previous task_struct (must be preserved across the switch)
+ *   a1: next task_struct
  *
- * The value of a1  must be preserved by this function, as that's how
+ * The value of a0  must be preserved by this function, as that's how
  * arguments are passed to schedule_tail.
  */
 	.align 4
@@ -397,33 +392,28 @@ $syscall_trace_failed:
 __switch_to:
 	.prologue 0
 	/* Save context into prev->thread */
-	stl	$26, TASK_THREAD_RA($17)
-	stl	$30, TASK_THREAD_SP($17)
-	stl	$9, TASK_THREAD_S0($17)
-	stl	$10, TASK_THREAD_S1($17)
-	stl	$11, TASK_THREAD_S2($17)
-	stl	$12, TASK_THREAD_S3($17)
-	stl	$13, TASK_THREAD_S4($17)
-	stl	$14, TASK_THREAD_S5($17)
-	stl	$15, TASK_THREAD_S6($17)
+	stl	$26, TASK_THREAD_RA($16)
+	stl	$30, TASK_THREAD_SP($16)
+	stl	$9, TASK_THREAD_S0($16)
+	stl	$10, TASK_THREAD_S1($16)
+	stl	$11, TASK_THREAD_S2($16)
+	stl	$12, TASK_THREAD_S3($16)
+	stl	$13, TASK_THREAD_S4($16)
+	stl	$14, TASK_THREAD_S5($16)
+	stl	$15, TASK_THREAD_S6($16)
 	/* Restore context from next->thread */
-	ldl	$26, TASK_THREAD_RA($18)
-	ldl	$9, TASK_THREAD_S0($18)
-	ldl	$10, TASK_THREAD_S1($18)
-	ldl	$11, TASK_THREAD_S2($18)
-	ldl	$12, TASK_THREAD_S3($18)
-	ldl	$13, TASK_THREAD_S4($18)
-	ldl	$14, TASK_THREAD_S5($18)
-	ldl	$15, TASK_THREAD_S6($18)
-	sys_call HMC_swpctx
-	/*
-	 * SP has been saved and restored by HMC_swpctx,
-	 * and restore it again here for future expansion.
-	 */
-	ldl     $30, TASK_THREAD_SP($18)
+	ldl	$26, TASK_THREAD_RA($17)
+	ldl	$30, TASK_THREAD_SP($17)
+	ldl	$9, TASK_THREAD_S0($17)
+	ldl	$10, TASK_THREAD_S1($17)
+	ldl	$11, TASK_THREAD_S2($17)
+	ldl	$12, TASK_THREAD_S3($17)
+	ldl	$13, TASK_THREAD_S4($17)
+	ldl	$14, TASK_THREAD_S5($17)
+	ldl	$15, TASK_THREAD_S6($17)
 	ldi	$8, 0x3fff
 	bic	$sp, $8, $8
-	mov	$17, $0
+	mov	$16, $0
 	ret
 	.end __switch_to
 
@@ -436,8 +426,7 @@ __switch_to:
 	.ent ret_from_fork
 ret_from_fork:
 	ldi	$26, ret_from_sys_call
-	mov	$17, $16
-	jmp	$31, schedule_tail
+	call	$31, schedule_tail
 	.end ret_from_fork
 
 /*
@@ -447,7 +436,6 @@ ret_from_fork:
 	.globl ret_from_kernel_thread
 	.ent ret_from_kernel_thread
 ret_from_kernel_thread:
-	mov	$17, $16
 	call	$26, schedule_tail
 	mov	$9, $27
 	mov	$10, $16
diff --git a/arch/sw_64/kernel/head.S b/arch/sw_64/kernel/head.S
index 5fff0f33c9e2af000d361f899281bd8f7bc626a6..7cce2a8859e566f74142a15ec54024faa484c967 100644
--- a/arch/sw_64/kernel/head.S
+++ b/arch/sw_64/kernel/head.S
@@ -24,25 +24,13 @@ __start:
 	/* We need to get current_task_info loaded up...  */
 	ldi	$8, init_thread_union
 	/* ... and find our stack ... */
-	ldi	$30, 0x4000 - PT_REGS_SIZE($8)
+	ldi	$30, ASM_THREAD_SIZE($8)
 	/* ... and then we can clear bss data.  */
-	ldi	$2, __bss_start
-	ldi	$3, __bss_stop
-	/* 8 bytes alignment */
-1:	and	$2, 0x7, $1	# align check
-	bne	$1, 3f
-2:	subl	$3, $2, $1	# align clear
-	ble	$1, 4f
-	subl	$1, 0x8, $1
-	ble	$1, 3f
-	stl	$31, 0($2)
-	addl	$2, 8, $2
-	br	$31, 2b
-3:	stb	$31, 0($2)	# non align clear
-	addl	$2, 1, $2
-	subl	$3, $2, $1
-	bgt	$1, 1b
-4:# finish clear
+	ldi	$16, __bss_start
+	ldi	$18, __bss_stop
+	subl	$18, $16, $18
+	mov	$31, $17
+	call	$26, __constant_c_memset
 #ifdef CONFIG_RELOCATABLE
 	ldi	$30, -8($30)
 	stl	$29, 0($30)
@@ -51,7 +39,7 @@ __start:
 	ldl	$29, 0($30)
 	addl	$29, $0, $29
 	/* Repoint the sp into the new kernel image */
-	ldi	$30, 0x4000 - PT_REGS_SIZE($8)
+	ldi	$30, ASM_THREAD_SIZE($8)
 #endif
 	/* ... and then we can start the kernel.  */
 	call	$26, sw64_start_kernel
@@ -71,24 +59,20 @@ __smp_callin:
 	br	$27, 2f		# we copy this from above "br $27 1f"
 2:	ldgp	$29, 0($27)	# First order of business, load the GP.
 
-	subl	$31, 2, $16
+	bis	$31, $31, $16	# invalidate all TLB with current VPN
 	sys_call HMC_tbi
 
 	sys_call HMC_whami	# Get hard cid
 
-	sll	$0, 2, $0
 	ldi	$1, __rcid_to_cpu
-	addl	$1, $0, $1
+	s4addl	$0, $1, $1
 	ldw	$0, 0($1)	# Get logical cpu number
 
-	sll	$0, 3, $0
-	ldi	$1, tidle_pcb
-	addl	$1, $0, $1
-	ldl	$16, 0($1)	# Get PCBB of idle thread
+	ldi	$2, tidle_ksp
+	s8addl	$0, $2, $2
+	ldl	$30, 0($2)	# Get ksp of idle thread
 
-	sys_call HMC_swpctx
-	ldi	$8, 0x3fff	# Find "current".
-	bic	$30, $8, $8
+	ldi	$8, -ASM_THREAD_SIZE($30) # Find "current"
 
 	call	$26, smp_callin
 	sys_call HMC_halt
diff --git a/arch/sw_64/kernel/hibernate.c b/arch/sw_64/kernel/hibernate.c
index 33426e3ed305a60fdf5fd1088e0c799d946d84ea..0e7e860c507e7576b1bdb895d94a374aa584918e 100644
--- a/arch/sw_64/kernel/hibernate.c
+++ b/arch/sw_64/kernel/hibernate.c
@@ -14,7 +14,7 @@ void save_processor_state(void)
 
 	vcb->ksp = rdksp();
 	vcb->usp = rdusp();
-	vcb->pcbb = rdpcbb();
+	vcb->tid = rtid();
 	vcb->ptbr = rdptbr();
 }
 
@@ -24,11 +24,10 @@ void restore_processor_state(void)
 
 	wrksp(vcb->ksp);
 	wrusp(vcb->usp);
-	wrpcbb(vcb->pcbb);
+	wrtp(vcb->tid);
 	wrptbr(vcb->ptbr);
 	sflush();
-	tbia();
-	imb();
+	tbiv();
 }
 
 int swsusp_arch_resume(void)
diff --git a/arch/sw_64/kernel/hibernate_asm.S b/arch/sw_64/kernel/hibernate_asm.S
index 3acbcdbae0b3df4ce838ff81b91f6ffc73f9e31f..23bab0d6edd87567c02ef218d7d9305e7c4efdc6 100644
--- a/arch/sw_64/kernel/hibernate_asm.S
+++ b/arch/sw_64/kernel/hibernate_asm.S
@@ -30,8 +30,7 @@ ENTRY(swsusp_arch_suspend)
 	rfpcr	$f0
 	fstd	$f0, PSTATE_FPCR($16)
 
-	ldi	$1, PSTATE_PCB($16)
-	stl	sp, PCB_KSP($1)
+	stl	sp, PSTATE_SP($16)
 	call	swsusp_save
 	ldi	$16, hibernate_state
 	ldi	$1, PSTATE_REGS($16)
@@ -112,8 +111,7 @@ $hibernate_setfpec_over:
 	vldd	$f8, CALLEE_F8($1)
 	vldd	$f9, CALLEE_F9($1)
 
-	ldi	$1, PSTATE_PCB($16)
-	ldl	sp, PCB_KSP($1)
+	ldl	sp, PSTATE_SP($16)
 
 	ldi	$8, 0x3fff
 	bic	sp, $8, $8
diff --git a/arch/sw_64/kernel/hmcall.c b/arch/sw_64/kernel/hmcall.c
new file mode 100644
index 0000000000000000000000000000000000000000..b81d7fff1c347d58f3c03935f76d532eb35d093c
--- /dev/null
+++ b/arch/sw_64/kernel/hmcall.c
@@ -0,0 +1,54 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * arch/sw_64/kernel/hmcall.c
+ *
+ * Copyright (C) 2022 WXIAT
+ * Author: He Sheng
+ */
+
+#include <asm/hmcall.h>
+#include <asm/page.h>
+
+#define A0(func)	(((HMC_##func & 0xFF) >> 6) & 0x1)
+#define A1(func)	((((HMC_##func & 0xFF)>>6) & 0x2) >> 1)
+#define A2(func)	((HMC_##func & 0x3F) << 7)
+
+#define T(func)		((A0(func) ^ A1(func)) & 0x1)
+#define B0(func)	((T(func) | A0(func)) << 13)
+#define B1(func)	(((~T(func) & 1) | A1(func)) << 14)
+
+#define PRI_BASE	0x10000UL
+
+#define HMCALL_ENTRY(func)	(PRI_BASE | B1(func) | B0(func) | A2(func))
+
+
+static inline void fixup_rdtp(void)
+{
+	unsigned int *entry = __va(HMCALL_ENTRY(rdtp));
+
+	entry[0] = 0x181ffec7;	/* pri_rcsr $0, CSR__TID */
+	entry[1] = 0x1ee00000;	/* pri_ret $23 */
+}
+
+static inline void fixup_wrtp(void)
+{
+	unsigned int *entry = __va(HMCALL_ENTRY(wrtp));
+
+	entry[0] = 0x1a1fffc7;	/* pri_wcsr $16, CSR__TID */
+	entry[1] = 0x1ee00000;	/* pri_ret $23 */
+}
+
+void __init fixup_hmcall(void)
+{
+#if defined(CONFIG_SUBARCH_C3A) || defined(CONFIG_SUBARCH_C3B)
+	fixup_rdtp();
+	fixup_wrtp();
+#endif
+}
+
+#undef A0
+#undef A1
+#undef A2
+#undef T
+#undef B0
+#undef B1
diff --git a/arch/sw_64/kernel/kgdb.c b/arch/sw_64/kernel/kgdb.c
index ac2f397f16096b39454c766c48e7054c221474c3..95970b293de0773234137d29b9ca263a15bac8d1 100644
--- a/arch/sw_64/kernel/kgdb.c
+++ b/arch/sw_64/kernel/kgdb.c
@@ -95,7 +95,7 @@ struct dbg_reg_def_t dbg_reg_def[DBG_MAX_REG_NUM] = {
 
 	{ "pc", 8, offsetof(struct pt_regs, pc)},
 	{ "", 8, -1 },
-	{ "unique", 8, -1},
+	{ "tp", 8, -1},
 };
 
 char *dbg_get_reg(int regno, void *mem, struct pt_regs *regs)
diff --git a/arch/sw_64/kernel/machine_kexec.c b/arch/sw_64/kernel/machine_kexec.c
index c9ca7a728bd458f323575ceae67657b379d6d925..950998476cdaced4b7368cb4712a1d7081e11047 100644
--- a/arch/sw_64/kernel/machine_kexec.c
+++ b/arch/sw_64/kernel/machine_kexec.c
@@ -204,9 +204,6 @@ void machine_kexec(struct kimage *image)
 
 	pr_info("Will call new kernel at %08lx\n", image->start);
 	pr_info("Bye ...\n");
-	//flush_cache_all();
-	//sflush();
-	//tbia();
 	smp_wmb();
 	((noretfun_t) reboot_code_buffer)();
 }
diff --git a/arch/sw_64/kernel/pci.c b/arch/sw_64/kernel/pci.c
index fcc6e0f02a93aa93c7cfad18ff960973c08dbb50..6cc872ba9ca54786c9bd44db3bd4a7f9665afde0 100644
--- a/arch/sw_64/kernel/pci.c
+++ b/arch/sw_64/kernel/pci.c
@@ -614,7 +614,8 @@ void __init sw64_init_arch(void)
 		cpu_num = sw64_chip->get_cpu_num();
 
 		for (node = 0; node < cpu_num; node++) {
-			set_devint_wken(node);
+			if (is_in_host())
+				set_devint_wken(node);
 			rc_enable = sw64_chip_init->pci_init.get_rc_enable(node);
 			if (rc_enable == 0) {
 				printk("PCIe is disabled on node %ld\n", node);
diff --git a/arch/sw_64/kernel/pci_impl.h b/arch/sw_64/kernel/pci_impl.h
index 6025145cb1c5c31ae749d892a92028f75d987e38..4262ba94f44b92935e44b46c6de94f0050c2a390 100644
--- a/arch/sw_64/kernel/pci_impl.h
+++ b/arch/sw_64/kernel/pci_impl.h
@@ -4,7 +4,7 @@
  * with the PCI initialization routines.
  */
 #ifndef _SW64_KERNEL_PCI_IMPL_H
-#define	_SW64_KERNEL_PCI_IMPL_H
+#define _SW64_KERNEL_PCI_IMPL_H
 
 #include <asm/sw64io.h>
 
@@ -25,4 +25,4 @@ extern const struct dma_map_ops sw64_dma_direct_ops;
 
 extern struct cma *sw64_kvm_cma;
 extern struct gen_pool *sw64_kvm_pool;
-#endif
+#endif /* _SW64_KERNEL_PCI_IMPL_H */
diff --git a/arch/sw_64/kernel/perf_event.c b/arch/sw_64/kernel/perf_event.c
index 6e344239917b8d703db5870d33930056aac9fe1e..f1f74a968cbc7f3dcf22e12cd334c00ccc2e9ceb 100644
--- a/arch/sw_64/kernel/perf_event.c
+++ b/arch/sw_64/kernel/perf_event.c
@@ -385,7 +385,6 @@ static int sw64_pmu_add(struct perf_event *event, int flags)
 	int err = 0;
 	unsigned long irq_flags;
 
-	perf_pmu_disable(event->pmu);
 	local_irq_save(irq_flags);
 
 	if (cpuc->pmcs[hwc->idx] == PMC_IN_USE) {
@@ -408,7 +407,6 @@ static int sw64_pmu_add(struct perf_event *event, int flags)
 
 out:
 	local_irq_restore(irq_flags);
-	perf_pmu_enable(event->pmu);
 
 	return err;
 }
@@ -422,24 +420,17 @@ static void sw64_pmu_del(struct perf_event *event, int flags)
 	struct hw_perf_event *hwc = &event->hw;
 	unsigned long irq_flags;
 
-	perf_pmu_disable(event->pmu);
 	local_irq_save(irq_flags);
 
-	if (cpuc->event[hwc->idx] != event)
-		goto out;
-
+	sw64_pmu_stop(event, PERF_EF_UPDATE);
 	cpuc->event[hwc->idx] = NULL;
 	cpuc->pmcs[hwc->idx] = PMC_NOT_USE;
 	cpuc->n_events--;
 
-	sw64_pmu_stop(event, PERF_EF_UPDATE);
-
 	/* Absorb the final count and turn off the event. */
 	perf_event_update_userpage(event);
 
-out:
 	local_irq_restore(irq_flags);
-	perf_pmu_enable(event->pmu);
 }
 
 /*
@@ -478,6 +469,9 @@ static void sw64_pmu_stop(struct perf_event *event, int flags)
 	struct hw_perf_event *hwc = &event->hw;
 
 	if (!(hwc->state & PERF_HES_STOPPED)) {
+		wrperfmon(PERFMON_CMD_DISABLE, hwc->idx == 0 ?
+				PERFMON_DISABLE_ARGS_PC0 :
+				PERFMON_DISABLE_ARGS_PC1);
 		hwc->state |= PERF_HES_STOPPED;
 		barrier();
 	}
@@ -486,12 +480,6 @@ static void sw64_pmu_stop(struct perf_event *event, int flags)
 		sw64_perf_event_update(event, hwc, hwc->idx, 0);
 		hwc->state |= PERF_HES_UPTODATE;
 	}
-
-	if (hwc->idx == 0)
-		wrperfmon(PERFMON_CMD_DISABLE, PERFMON_DISABLE_ARGS_PC0);
-	else
-		wrperfmon(PERFMON_CMD_DISABLE, PERFMON_DISABLE_ARGS_PC1);
-
 }
 
 /*
@@ -659,10 +647,7 @@ static void sw64_perf_event_irq_handler(unsigned long perfmon_num,
 	event = cpuc->event[idx];
 
 	if (unlikely(!event)) {
-		/* This should never occur! */
 		irq_err_count++;
-		pr_warn("PMI: No event at index %d!\n", idx);
-		wrperfmon(PERFMON_CMD_ENABLE, idx == 0 ? PERFMON_DISABLE_ARGS_PC0 : PERFMON_DISABLE_ARGS_PC1);
 		return;
 	}
 
@@ -775,6 +760,38 @@ void perf_callchain_kernel(struct perf_callchain_entry_ctx *entry,
 	walk_stackframe(NULL, regs, callchain_trace, entry);
 }
 
+/*
+ * Gets the perf_instruction_pointer and perf_misc_flags for guest os.
+ */
+#undef is_in_guest
+
+unsigned long perf_instruction_pointer(struct pt_regs *regs)
+{
+	if (perf_guest_cbs && perf_guest_cbs->is_in_guest())
+		return perf_guest_cbs->get_guest_ip();
+
+	return instruction_pointer(regs);
+}
+
+unsigned long perf_misc_flags(struct pt_regs *regs)
+{
+	int misc = 0;
+
+	if (perf_guest_cbs && perf_guest_cbs->is_in_guest()) {
+		if (perf_guest_cbs->is_user_mode())
+			misc |= PERF_RECORD_MISC_GUEST_USER;
+		else
+			misc |= PERF_RECORD_MISC_GUEST_KERNEL;
+	} else {
+		if (user_mode(regs))
+			misc |= PERF_RECORD_MISC_USER;
+		else
+			misc |= PERF_RECORD_MISC_KERNEL;
+	}
+
+	return misc;
+}
+
 /*
  * Init call to initialise performance events at kernel startup.
  */
diff --git a/arch/sw_64/kernel/perf_regs.c b/arch/sw_64/kernel/perf_regs.c
index 4c12a2cdf912020c4e49df19f4c9fa99e2ffae36..b036f213936bc6d79214c9b7bdf1ab9a82a40b69 100644
--- a/arch/sw_64/kernel/perf_regs.c
+++ b/arch/sw_64/kernel/perf_regs.c
@@ -28,6 +28,6 @@ u64 perf_reg_abi(struct task_struct *task)
 void perf_get_regs_user(struct perf_regs *regs_user,
 			struct pt_regs *regs)
 {
-	regs_user->regs = NULL;
-	regs_user->abi = PERF_SAMPLE_REGS_ABI_NONE;
+	regs_user->regs = task_pt_regs(current);
+	regs_user->abi = perf_reg_abi(current);
 }
diff --git a/arch/sw_64/kernel/process.c b/arch/sw_64/kernel/process.c
index 4d223a7255bbf2579c7059000ef3edbb29a4767e..e1689d25f77d25300164ae39a6f4d039eae77069 100644
--- a/arch/sw_64/kernel/process.c
+++ b/arch/sw_64/kernel/process.c
@@ -125,7 +125,7 @@ flush_thread(void)
 	wrfpcr(FPCR_DYN_NORMAL | ieee_swcr_to_fpcr(0));
 
 	/* Clean slate for TLS.  */
-	current_thread_info()->pcb.unique = 0;
+	current_thread_info()->pcb.tp = 0;
 }
 
 void
@@ -135,7 +135,11 @@ release_thread(struct task_struct *dead_task)
 
 int arch_dup_task_struct(struct task_struct *dst, struct task_struct *src)
 {
-	fpstate_save(src);
+	/*
+	 * aux_save() has to read the current TLS pointer from CSR:TID as it
+	 * may be out-of-sync with the saved value.
+	 */
+	aux_save(src);
 	*dst = *src;
 	return 0;
 }
@@ -156,8 +160,6 @@ copy_thread(unsigned long clone_flags, unsigned long usp,
 	struct pt_regs *childregs = task_pt_regs(p);
 	struct pt_regs *regs = current_pt_regs();
 
-	childti->pcb.ksp = (unsigned long) childregs;
-	childti->pcb.flags = 7;	/* set FEN, clear everything else */
 	p->thread.sp = (unsigned long) childregs;
 
 	if (unlikely(p->flags & PF_KTHREAD)) {
@@ -169,6 +171,7 @@ copy_thread(unsigned long clone_flags, unsigned long usp,
 		childti->pcb.usp = 0;
 		return 0;
 	}
+
 	/*
 	 * Note: if CLONE_SETTLS is not set, then we must inherit the
 	 * value from the parent, which will have been set by the block
@@ -177,10 +180,11 @@ copy_thread(unsigned long clone_flags, unsigned long usp,
 	 * application calling fork.
 	 */
 	if (clone_flags & CLONE_SETTLS)
-		childti->pcb.unique = tls;
+		childti->pcb.tp = regs->r20;
 	else
 		regs->r20 = 0;
-	childti->pcb.usp = usp ?: rdusp();
+	if (usp)
+		childti->pcb.usp = usp;
 	*childregs = *regs;
 	childregs->r0 = 0;
 	childregs->r19 = 0;
@@ -203,7 +207,7 @@ void sw64_elf_core_copy_regs(elf_greg_t *dest, struct pt_regs *regs)
 		dest[i] = *(__u64 *)((void *)regs + regoffsets[i]);
 	dest[30] = ti == current_thread_info() ? rdusp() : ti->pcb.usp;
 	dest[31] = regs->pc;
-	dest[32] = ti->pcb.unique;
+	dest[32] = ti->pcb.tp;
 }
 EXPORT_SYMBOL(sw64_elf_core_copy_regs);
 
diff --git a/arch/sw_64/kernel/proto.h b/arch/sw_64/kernel/proto.h
index f84629ec05ea67adfd098305de86e637869dc84b..f908263f925a76c6acee373cf213ccb541bf029b 100644
--- a/arch/sw_64/kernel/proto.h
+++ b/arch/sw_64/kernel/proto.h
@@ -7,10 +7,6 @@
 #include <asm/pgtable.h>
 #include <asm/sw64io.h>
 
-/* ptrace.c */
-extern int ptrace_set_bpt(struct task_struct *child);
-extern int ptrace_cancel_bpt(struct task_struct *child);
-
 /* traps.c */
 extern void show_regs(struct pt_regs *regs);
 extern void die(char *str, struct pt_regs *regs, long err);
@@ -23,4 +19,4 @@ extern void __init setup_sched_clock(void);
 extern void __init sw64_sched_clock_init(void);
 #endif
 
-#endif /* _SW64_PROTO_H */
+#endif /* _SW64_KERNEL_PROTO_H */
diff --git a/arch/sw_64/kernel/ptrace.c b/arch/sw_64/kernel/ptrace.c
index 064296711b2f8e8cdb60fcf583ace19cfb09f9ed..e98679d10fae16adee87ae65b08a241f55d986a6 100644
--- a/arch/sw_64/kernel/ptrace.c
+++ b/arch/sw_64/kernel/ptrace.c
@@ -46,16 +46,6 @@
  * zero have no stack-slot and need to be treated specially (see
  * get_reg/put_reg below).
  */
-enum {
-	REG_R0 = 0,
-	REG_F0 = 32,
-	REG_FPCR = 63,
-	REG_PC = 64,
-	REG_SP = 30,
-	REG_PS = 31,
-	REG_GP = 29
-};
-
 #define R(x)	((size_t) &((struct pt_regs *)0)->x)
 
 short regoffsets[32] = {
@@ -72,7 +62,7 @@ short regoffsets[32] = {
 
 static int pcboff[] = {
 	[USP] = PCB_OFF(usp),
-	[UNIQUE] = PCB_OFF(unique),
+	[TP] = PCB_OFF(tp),
 	[DA_MATCH] = PCB_OFF(da_match),
 	[DA_MASK] = PCB_OFF(da_mask),
 	[DV_MATCH] = PCB_OFF(dv_match),
@@ -154,119 +144,12 @@ put_reg(struct task_struct *task, unsigned long regno, unsigned long data)
 	return 0;
 }
 
-static inline int
-read_int(struct task_struct *task, unsigned long addr, int *data)
-{
-	int copied = access_process_vm(task, addr, data, sizeof(int), FOLL_FORCE);
-
-	return (copied == sizeof(int)) ? 0 : -EIO;
-}
-
-static inline int
-write_int(struct task_struct *task, unsigned long addr, int data)
-{
-	int copied = access_process_vm(task, addr, &data, sizeof(int),
-			FOLL_FORCE | FOLL_WRITE);
-	return (copied == sizeof(int)) ? 0 : -EIO;
-}
-
 /*
- * Set breakpoint.
- */
-int
-ptrace_set_bpt(struct task_struct *child)
-{
-	int displ, i, res, reg_b, nsaved = 0;
-	unsigned int insn, op_code;
-	unsigned long pc;
-
-	pc = get_reg(child, REG_PC);
-	res = read_int(child, pc, (int *)&insn);
-	if (res < 0)
-		return res;
-
-	op_code = insn >> 26;
-	/* br bsr beq bne blt ble bgt bge blbc blbs fbeq fbne fblt fble fbgt fbge */
-	if ((1UL << op_code) & 0x3fff000000000030UL) {
-		/*
-		 * It's a branch: instead of trying to figure out
-		 * whether the branch will be taken or not, we'll put
-		 * a breakpoint at either location.  This is simpler,
-		 * more reliable, and probably not a whole lot slower
-		 * than the alternative approach of emulating the
-		 * branch (emulation can be tricky for fp branches).
-		 */
-		displ = ((s32)(insn << 11)) >> 9;
-		task_thread_info(child)->bpt_addr[nsaved++] = pc + 4;
-		if (displ) /* guard against unoptimized code */
-			task_thread_info(child)->bpt_addr[nsaved++]
-				= pc + 4 + displ;
-		/*call ret jmp*/
-	} else if (op_code >= 0x1 && op_code <= 0x3) {
-		reg_b = (insn >> 16) & 0x1f;
-		task_thread_info(child)->bpt_addr[nsaved++] = get_reg(child, reg_b);
-	} else {
-		task_thread_info(child)->bpt_addr[nsaved++] = pc + 4;
-	}
-
-	/* install breakpoints: */
-	for (i = 0; i < nsaved; ++i) {
-		res = read_int(child, task_thread_info(child)->bpt_addr[i],
-				(int *)&insn);
-		if (res < 0)
-			return res;
-		task_thread_info(child)->bpt_insn[i] = insn;
-		res = write_int(child, task_thread_info(child)->bpt_addr[i],
-				BREAKINST);
-		if (res < 0)
-			return res;
-	}
-	task_thread_info(child)->bpt_nsaved = nsaved;
-	return 0;
-}
-
-/*
- * Ensure no single-step breakpoint is pending.  Returns non-zero
- * value if child was being single-stepped.
- */
-int
-ptrace_cancel_bpt(struct task_struct *child)
-{
-	int i, nsaved = task_thread_info(child)->bpt_nsaved;
-
-	task_thread_info(child)->bpt_nsaved = 0;
-
-	if (nsaved > 2) {
-		printk("%s: bogus nsaved: %d!\n", __func__, nsaved);
-		nsaved = 2;
-	}
-
-	for (i = 0; i < nsaved; ++i) {
-		write_int(child, task_thread_info(child)->bpt_addr[i],
-				task_thread_info(child)->bpt_insn[i]);
-	}
-	return (nsaved != 0);
-}
-
-void user_enable_single_step(struct task_struct *child)
-{
-	/* Mark single stepping.  */
-	task_thread_info(child)->bpt_nsaved = -1;
-}
-
-void user_disable_single_step(struct task_struct *child)
-{
-	ptrace_cancel_bpt(child);
-}
-
-/*
- * Called by kernel/ptrace.c when detaching..
- *
- * Make sure the single step bit is not set.
+ * Called by ptrace_detach
  */
 void ptrace_disable(struct task_struct *child)
 {
-	user_disable_single_step(child);
+	/**/
 }
 
 static int gpr_get(struct task_struct *target,
@@ -611,10 +494,6 @@ static const struct pt_regs_offset regoffset_table[] = {
 	REG_OFFSET_NAME(r26),
 	REG_OFFSET_NAME(r27),
 	REG_OFFSET_NAME(r28),
-	REG_OFFSET_NAME(hae),
-	REG_OFFSET_NAME(trap_a0),
-	REG_OFFSET_NAME(trap_a1),
-	REG_OFFSET_NAME(trap_a2),
 	REG_OFFSET_NAME(ps),
 	REG_OFFSET_NAME(pc),
 	REG_OFFSET_NAME(gp),
diff --git a/arch/sw_64/kernel/relocate.c b/arch/sw_64/kernel/relocate.c
index fe403f9c70c74f35be4fa575ac3d3e743ff5b0e5..792ee1a9c2b1a261706e1e339c7c6ff85e5de20d 100644
--- a/arch/sw_64/kernel/relocate.c
+++ b/arch/sw_64/kernel/relocate.c
@@ -1,10 +1,8 @@
 // SPDX-License-Identifier: GPL-2.0
 /*
- * This file is subject to the terms and conditions of the GNU General Public
- * License.  See the file "COPYING" in the main directory of this archive
- * for more details.
+ * Support for kernel relocation at boot time.
  *
- * Support for Kernel relocation at boot time
+ * Based on arch/mips/kernel/relocate.c
  *
  * Copyright (C) 2019 He Sheng
  * Authors: He Sheng (hesheng05@gmail.com)
@@ -15,7 +13,6 @@
 
 #include <asm/sections.h>
 
-#define INITRD_ADDR  0x3000000UL
 #define KTEXT_MAX    0xffffffffa0000000UL
 #define RELOCATED(x) ((void *)((unsigned long)x + offset))
 
@@ -30,8 +27,6 @@ extern unsigned long __start___ex_table;	/* Start exception table */
 extern unsigned long __stop___ex_table;	/* End exception table */
 extern union thread_union init_thread_union;
 
-extern void __weak plat_fdt_relocated(void *new_location);
-
 /*
  * This function may be defined for a platform to perform any post-relocation
  * fixup necessary.
@@ -42,13 +37,6 @@ int __weak plat_post_relocation(long offset)
 	return 0;
 }
 
-
-static void __init sync_icache(void)
-{
-	// IC_FLUSH
-	imb();
-}
-
 static int __init apply_r_sw64_refquad(unsigned long *loc_orig, unsigned long *loc_new, unsigned int offset)
 {
 	*(unsigned long *)loc_new += offset;
@@ -166,13 +154,14 @@ static unsigned long __init determine_relocation_offset(void)
 	if (offset < kernel_length)
 		offset += ALIGN(kernel_length, 0x10000);
 
-	/* TODO: 119MB is for test */
-	offset = (119 << 20);
+	/*
+	 * TODO:new location should not overlaps initrd, dtb, acpi
+	 * tables, etc.
+	 */
+
 	if ((KTEXT_MAX - (unsigned long)_end) < offset)
 		offset = 0;
 
-	// TODO:new location should not overlaps initrd
-
 	return offset;
 }
 
@@ -216,9 +205,7 @@ unsigned int __init relocate_kernel(void)
 	bss_length = (unsigned long)&__bss_stop - (long)&__bss_start;
 
 	offset = determine_relocation_offset();
-
 	/* Reset the command line now so we don't end up with a duplicate */
-	//arcs_cmdline[0] = '\0';
 
 	/* Sanity check relocation address */
 	if (offset && relocation_offset_valid(offset)) {
@@ -232,9 +219,6 @@ unsigned int __init relocate_kernel(void)
 		if (res < 0)
 			goto out;
 
-		/* Sync the caches ready for execution of new kernel */
-		sync_icache();
-
 		res = relocate_got(offset);
 		if (res < 0)
 			goto out;
@@ -259,7 +243,6 @@ unsigned int __init relocate_kernel(void)
 		__current_thread_info = RELOCATED(&init_thread_union);
 
 		/* Return the new kernel's offset */
-		//printk("loc_new:%p, start_kernel: %p, gp:%p\n", loc_new, kernel_entry, kgp);
 		return offset;
 	}
 out:
diff --git a/arch/sw_64/kernel/setup.c b/arch/sw_64/kernel/setup.c
index 39103e4edee43781e6c78833b5d97e1fd17a9508..f68d93b5a7b77d2ab9631fa45f52a27a701cabb1 100644
--- a/arch/sw_64/kernel/setup.c
+++ b/arch/sw_64/kernel/setup.c
@@ -143,10 +143,7 @@ EXPORT_SYMBOL(screen_info);
  */
 void store_cpu_data(int cpu)
 {
-	cpu_data[cpu].loops_per_jiffy = loops_per_jiffy;
 	cpu_data[cpu].last_asn = ASN_FIRST_VERSION;
-	cpu_data[cpu].need_new_asn = 0;
-	cpu_data[cpu].asn_lock = 0;
 }
 
 #ifdef CONFIG_KEXEC
@@ -788,7 +785,7 @@ setup_arch(char **cmdline_p)
 		strlcat(boot_command_line, " ", COMMAND_LINE_SIZE);
 		strlcat(boot_command_line, builtin_cmdline, COMMAND_LINE_SIZE);
 	}
-#endif	/* CMDLINE_EXTEND */
+#endif /* CMDLINE_EXTEND */
 #endif
 	if (IS_ENABLED(CONFIG_SW64_CHIP3_ASIC_DEBUG) &&
 			IS_ENABLED(CONFIG_SW64_CHIP3)) {
diff --git a/arch/sw_64/kernel/signal.c b/arch/sw_64/kernel/signal.c
index 6a6203ccb04f489ef0b6b1bbf59b4635c3f88d50..b80cf0e56224485e41e27ffe7ef49c093f559f76 100644
--- a/arch/sw_64/kernel/signal.c
+++ b/arch/sw_64/kernel/signal.c
@@ -38,6 +38,36 @@ SYSCALL_DEFINE2(odd_sigprocmask, int, how, unsigned long, newmask)
 	return res;
 }
 
+SYSCALL_DEFINE3(odd_sigaction, int, sig,
+		const struct odd_sigaction __user *, act,
+		struct odd_sigaction __user *, oact)
+{
+	struct k_sigaction new_ka, old_ka;
+	old_sigset_t mask;
+	int ret;
+
+	if (act) {
+		if (!access_ok(act, sizeof(*act)) ||
+		    __get_user(new_ka.sa.sa_handler, &act->sa_handler) ||
+		    __get_user(new_ka.sa.sa_flags, &act->sa_flags) ||
+		    __get_user(mask, &act->sa_mask))
+			return -EFAULT;
+		siginitset(&new_ka.sa.sa_mask, mask);
+	}
+
+	ret = do_sigaction(sig, act ? &new_ka : NULL, oact ? &old_ka : NULL);
+
+	if (!ret && oact) {
+		if (!access_ok(oact, sizeof(*oact)) ||
+		    __put_user(old_ka.sa.sa_handler, &oact->sa_handler) ||
+		    __put_user(old_ka.sa.sa_flags, &oact->sa_flags) ||
+		    __put_user(old_ka.sa.sa_mask.sig[0], &oact->sa_mask))
+			return -EFAULT;
+	}
+
+	return ret;
+}
+
 /*
  * Do a signal return; undo the signal stack.
  */
@@ -133,11 +163,6 @@ do_sigreturn(struct sigcontext __user *sc)
 	if (restore_sigcontext(sc, regs))
 		goto give_sigsegv;
 
-	/* Send SIGTRAP if we're single-stepping: */
-	if (ptrace_cancel_bpt(current)) {
-		force_sig_fault(SIGTRAP, TRAP_BRKPT,
-				(void __user *)regs->pc, 0);
-	}
 	return;
 
 give_sigsegv:
@@ -164,11 +189,6 @@ do_rt_sigreturn(struct rt_sigframe __user *frame)
 	if (restore_altstack(&frame->uc.uc_stack))
 		goto give_sigsegv;
 
-	/* Send SIGTRAP if we're single-stepping: */
-	if (ptrace_cancel_bpt(current)) {
-		force_sig_fault(SIGTRAP, TRAP_BRKPT,
-				(void __user *)regs->pc, 0);
-	}
 	return;
 
 give_sigsegv:
@@ -235,17 +255,13 @@ setup_sigcontext(struct sigcontext __user *sc, struct pt_regs *regs,
 				offsetof(struct user_fpsimd_state, fpcr));
 	err |= __put_user(current->thread.fpstate.fpcr, &sc->sc_fpcr);
 
-	err |= __put_user(regs->trap_a0, &sc->sc_traparg_a0);
-	err |= __put_user(regs->trap_a1, &sc->sc_traparg_a1);
-	err |= __put_user(regs->trap_a2, &sc->sc_traparg_a2);
-
 	return err;
 }
 
 static int
 setup_rt_frame(struct ksignal *ksig, sigset_t *set, struct pt_regs *regs)
 {
-	unsigned long oldsp, r26, err = 0;
+	unsigned long oldsp, err = 0;
 	struct rt_sigframe __user *frame;
 
 	oldsp = rdusp();
@@ -267,13 +283,8 @@ setup_rt_frame(struct ksignal *ksig, sigset_t *set, struct pt_regs *regs)
 	if (err)
 		return -EFAULT;
 
-	/* Set up to return from userspace.  If provided, use a stub
-	 * already in userspace.
-	 */
-	r26 = VDSO_SYMBOL(current->mm->context.vdso, rt_sigreturn);
-
 	/* "Return" to the handler */
-	regs->r26 = r26;
+	regs->r26 = VDSO_SYMBOL(current->mm->context.vdso, rt_sigreturn);
 	regs->r27 = regs->pc = (unsigned long) ksig->ka.sa.sa_handler;
 	regs->r16 = ksig->sig;                    /* a0: signal number */
 	if (ksig->ka.sa.sa_flags & SA_SIGINFO) {
@@ -351,19 +362,15 @@ syscall_restart(unsigned long r0, unsigned long r19,
 static void
 do_signal(struct pt_regs *regs, unsigned long r0, unsigned long r19)
 {
-	unsigned long single_stepping = ptrace_cancel_bpt(current);
 	struct ksignal ksig;
 
 	/* This lets the debugger run, ... */
 	if (get_signal(&ksig)) {
-		/* ... so re-check the single stepping. */
-		single_stepping |= ptrace_cancel_bpt(current);
 		/* Whee!  Actually deliver the signal.  */
 		if (r0)
 			syscall_restart(r0, r19, regs, &ksig.ka);
 		handle_signal(&ksig, regs);
 	} else {
-		single_stepping |= ptrace_cancel_bpt(current);
 		if (r0) {
 			switch (regs->r0) {
 			case ERESTARTNOHAND:
@@ -383,8 +390,6 @@ do_signal(struct pt_regs *regs, unsigned long r0, unsigned long r19)
 		}
 		restore_saved_sigmask();
 	}
-	if (single_stepping)
-		ptrace_set_bpt(current);        /* re-set breakpoint */
 }
 
 void
diff --git a/arch/sw_64/kernel/smp.c b/arch/sw_64/kernel/smp.c
index 1004e9e3be27f19f6b6dbabce4186802d200f127..b467562bce9ec9fca35981033655cc8300be6c8b 100644
--- a/arch/sw_64/kernel/smp.c
+++ b/arch/sw_64/kernel/smp.c
@@ -34,7 +34,7 @@ EXPORT_SYMBOL(__cpu_to_rcid);
 int __rcid_to_cpu[NR_CPUS];		/* Map physical to logical */
 EXPORT_SYMBOL(__rcid_to_cpu);
 
-unsigned long tidle_pcb[NR_CPUS];
+void *tidle_ksp[NR_CPUS];
 
 /* State of each CPU */
 DEFINE_PER_CPU(int, cpu_state) = { 0 };
@@ -110,6 +110,8 @@ void smp_callin(void)
 	/* All kernel threads share the same mm context.  */
 	mmgrab(&init_mm);
 	current->active_mm = &init_mm;
+	/* update csr:ptbr */
+	wrptbr(virt_to_phys(init_mm.pgd));
 
 	/* inform the notifiers about the new cpu */
 	notify_cpu_starting(cpuid);
@@ -153,23 +155,11 @@ static inline void set_secondary_ready(int cpuid)
  */
 static int secondary_cpu_start(int cpuid, struct task_struct *idle)
 {
-	struct pcb_struct *ipcb;
 	unsigned long timeout;
-
-	ipcb = &task_thread_info(idle)->pcb;
-
 	/*
-	 * Initialize the idle's PCB to something just good enough for
-	 * us to get started.  Immediately after starting, we'll swpctx
-	 * to the target idle task's pcb.  Reuse the stack in the mean
-	 * time.  Precalculate the target PCBB.
+	 * Precalculate the target ksp.
 	 */
-	ipcb->ksp = (unsigned long)ipcb + sizeof(union thread_union) - 16;
-	ipcb->usp = 0;
-	ipcb->pcc = 0;
-	ipcb->asn = 0;
-	tidle_pcb[cpuid] = ipcb->unique = virt_to_phys(ipcb);
-	ipcb->dv_match = ipcb->dv_mask = 0;
+	tidle_ksp[cpuid] = idle->stack + THREAD_SIZE;
 
 	DBGS("Starting secondary cpu %d: state 0x%lx\n", cpuid, idle->state);
 
@@ -384,18 +374,8 @@ int native_cpu_up(unsigned int cpu, struct task_struct *tidle)
 
 void __init native_smp_cpus_done(unsigned int max_cpus)
 {
-	int cpu;
-	unsigned long bogosum = 0;
-
-	for (cpu = 0; cpu < NR_CPUS; cpu++)
-		if (cpu_online(cpu))
-			bogosum += cpu_data[cpu].loops_per_jiffy;
-
 	smp_booted = 1;
-	pr_info("SMP: Total of %d processors activated (%lu.%02lu BogoMIPS).\n",
-		num_online_cpus(),
-		(bogosum + 2500) / (500000/HZ),
-		((bogosum + 2500) / (5000/HZ)) % 100);
+	pr_info("SMP: Total of %d processors activated.\n", num_online_cpus());
 }
 
 int setup_profiling_timer(unsigned int multiplier)
@@ -496,22 +476,9 @@ void native_send_call_func_single_ipi(int cpu)
 	send_ipi_message(cpumask_of(cpu), IPI_CALL_FUNC);
 }
 
-static void
-ipi_imb(void *ignored)
-{
-	imb();
-}
-
-void smp_imb(void)
-{
-	/* Must wait other processors to flush their icache before continue. */
-	on_each_cpu(ipi_imb, NULL, 1);
-}
-EXPORT_SYMBOL(smp_imb);
-
 static void ipi_flush_tlb_all(void *ignored)
 {
-	tbia();
+	tbiv();
 }
 
 void flush_tlb_all(void)
@@ -522,8 +489,6 @@ void flush_tlb_all(void)
 	on_each_cpu(ipi_flush_tlb_all, NULL, 1);
 }
 
-#define asn_locked() (cpu_data[smp_processor_id()].asn_lock)
-
 static void ipi_flush_tlb_mm(void *x)
 {
 	struct mm_struct *mm = (struct mm_struct *) x;
@@ -628,50 +593,6 @@ void flush_tlb_range(struct vm_area_struct *vma, unsigned long start, unsigned l
 }
 EXPORT_SYMBOL(flush_tlb_range);
 
-static void ipi_flush_icache_page(void *x)
-{
-	struct mm_struct *mm = (struct mm_struct *) x;
-
-	if (mm == current->mm)
-		__load_new_mm_context(mm);
-	else
-		flush_tlb_other(mm);
-}
-
-void flush_icache_user_page(struct vm_area_struct *vma, struct page *page,
-			unsigned long addr, int len)
-{
-	struct mm_struct *mm = vma->vm_mm;
-
-	if ((vma->vm_flags & VM_EXEC) == 0)
-		return;
-	if (!icache_is_vivt_no_ictag())
-		return;
-
-	preempt_disable();
-
-	if (mm == current->mm) {
-		__load_new_mm_context(mm);
-		if (atomic_read(&mm->mm_users) == 1) {
-			int cpu, this_cpu = smp_processor_id();
-
-			for (cpu = 0; cpu < NR_CPUS; cpu++) {
-				if (!cpu_online(cpu) || cpu == this_cpu)
-					continue;
-				if (mm->context.asid[cpu])
-					mm->context.asid[cpu] = 0;
-			}
-			preempt_enable();
-			return;
-		}
-	} else
-		flush_tlb_other(mm);
-
-	smp_call_function(ipi_flush_icache_page, mm, 1);
-
-	preempt_enable();
-}
-
 int native_cpu_disable(void)
 {
 	int cpu = smp_processor_id();
@@ -695,6 +616,7 @@ void native_cpu_die(unsigned int cpu)
 		if (per_cpu(cpu_state, cpu) == CPU_DEAD) {
 			if (system_state == SYSTEM_RUNNING)
 				pr_info("CPU %u is now offline\n", cpu);
+			smp_rcb->ready = 0;
 			return;
 		}
 		msleep(100);
diff --git a/arch/sw_64/kernel/suspend.c b/arch/sw_64/kernel/suspend.c
index 369bc1e19b85713cb0ebe9a0719fd3a7a68ec358..b9798baa246745c68bcd21ce98f2d4a8fdaac66e 100644
--- a/arch/sw_64/kernel/suspend.c
+++ b/arch/sw_64/kernel/suspend.c
@@ -23,6 +23,8 @@ void disable_local_timer(void)
 	wrtimer(0);
 }
 
+extern struct pci_controller *hose_head;
+
 /*
  * Boot Core will enter suspend stat here.
  */
@@ -32,7 +34,13 @@ void sw64_suspend_enter(void)
 	 * After wake up  boot processor, pc will go here
 	 */
 
+#ifdef CONFIG_SW64_SUPPORT_S3_SLEEPING_STATE
+	if (sw64_chip->suspend)
+		sw64_chip->suspend(false);
+#endif
+
 	disable_local_timer();
+	current_thread_info()->pcb.tp = rtid();
 
 #ifdef CONFIG_SW64_SUSPEND_DEEPSLEEP_BOOTCORE
 	sw64_suspend_deep_sleep(&suspend_state);
@@ -40,6 +48,12 @@ void sw64_suspend_enter(void)
 	mtinten();
 	asm("halt");
 #endif
+	wrtp(current_thread_info()->pcb.tp);
+
+#ifdef CONFIG_SW64_SUPPORT_S3_SLEEPING_STATE
+	if (sw64_chip->suspend)
+		sw64_chip->suspend(true);
+#endif
 
 	disable_local_timer();
 }
@@ -55,6 +69,7 @@ static const struct platform_suspend_ops native_suspend_ops = {
 	.valid = native_suspend_state_valid,
 	.enter = native_suspend_enter,
 };
+
 static int __init sw64_pm_init(void)
 {
 	suspend_set_ops(&native_suspend_ops);
diff --git a/arch/sw_64/kernel/syscalls/syscall.tbl b/arch/sw_64/kernel/syscalls/syscall.tbl
index 42a179422b6b22fbe14fa10237dfb93ba6dc9261..35d108b49a61ca7ac1058272e89dc2ecacf61923 100644
--- a/arch/sw_64/kernel/syscalls/syscall.tbl
+++ b/arch/sw_64/kernel/syscalls/syscall.tbl
@@ -163,7 +163,7 @@
 #153 is unused
 #154 is unused
 #155 is unused
-156	common	sigaction			sys_sigaction
+156	common	sigaction			sys_odd_sigaction
 #157 is unused
 #158 is unused
 #159 is unused
diff --git a/arch/sw_64/kernel/time.c b/arch/sw_64/kernel/time.c
index 6a4c8a31465c615d212d0e33ce48c72f33c85a2f..32690e78849b8fb54afa7ad22f7f3c59bd9a87f4 100644
--- a/arch/sw_64/kernel/time.c
+++ b/arch/sw_64/kernel/time.c
@@ -60,7 +60,7 @@ void arch_irq_work_raise(void)
 	set_irq_work_pending_flag();
 }
 
-#else  /* CONFIG_IRQ_WORK */
+#else /* CONFIG_IRQ_WORK */
 
 #define test_irq_work_pending()      0
 #define clear_irq_work_pending()
@@ -111,14 +111,8 @@ time_init(void)
 	of_clk_init(NULL);
 	/* Startup the timer source. */
 	setup_timer();
-}
-
-void calibrate_delay(void)
-{
-	loops_per_jiffy = get_cpu_freq() / HZ;
-	pr_info("Clock rate yields %lu.%02lu BogoMIPS (lpj=%lu)\n",
-			loops_per_jiffy / (500000 / HZ),
-			(loops_per_jiffy / (5000 / HZ)) % 100, loops_per_jiffy);
+	/* Calibrate the delay loop directly */
+	lpj_fine = cycle_freq / HZ;
 }
 
 static void __init calibrate_sched_clock(void)
diff --git a/arch/sw_64/kernel/traps.c b/arch/sw_64/kernel/traps.c
index 9915160d95d2da442ce9f65c82ed95d6822963d3..f01b88e53ff2dfacbb1b0490c98f5ca6780a2695 100644
--- a/arch/sw_64/kernel/traps.c
+++ b/arch/sw_64/kernel/traps.c
@@ -31,6 +31,14 @@
 
 #include "proto.h"
 
+enum SW64_IF_TYPES {
+	IF_BREAKPOINT = 0,
+	IF_RESERVED,
+	IF_GENTRAP,
+	IF_FEN,
+	IF_OPDEC,
+};
+
 void show_regs(struct pt_regs *regs)
 {
 	show_regs_print_info(KERN_DEFAULT);
@@ -155,6 +163,10 @@ do_entArith(unsigned long summary, unsigned long write_mask,
 	force_sig_fault(SIGFPE, si_code, (void __user *)regs->pc, 0);
 }
 
+/*
+ * BPT/GENTRAP/OPDEC make regs->pc = exc_pc + 4. debugger should
+ * do something necessary to handle it correctly.
+ */
 asmlinkage void
 do_entIF(unsigned long inst_type, struct pt_regs *regs)
 {
@@ -164,35 +176,23 @@ do_entIF(unsigned long inst_type, struct pt_regs *regs)
 	type = inst_type & 0xffffffff;
 	inst = inst_type >> 32;
 
-	if (!user_mode(regs) && type != 4) {
-		if (type == 1) {
-			const unsigned int *data
-				= (const unsigned int *) regs->pc;
-			printk("Kernel bug at %s:%d\n",
-				(const char *)(data[1] | (long)data[2] << 32),
-				data[0]);
-		} else if (type == 0) {
+	if (!user_mode(regs) && type != IF_OPDEC) {
+		if (type == IF_BREAKPOINT) {
 			/* support kgdb */
 			notify_die(0, "kgdb trap", regs, 0, 0, SIGTRAP);
 			return;
 		}
-		die((type == 1 ? "Kernel Bug" : "Instruction fault"),
+		die((type == IF_RESERVED ? "Kernel Bug" : "Instruction fault"),
 				regs, type);
 	}
 
 	switch (type) {
-	case 0: /* breakpoint */
-		if (ptrace_cancel_bpt(current))
-			regs->pc -= 4;	/* make pc point to former bpt */
-
+	case IF_BREAKPOINT: /* gdb do pc-4 for sigtrap */
 		force_sig_fault(SIGTRAP, TRAP_BRKPT, (void __user *)regs->pc, 0);
 		return;
 
-	case 1: /* bugcheck */
-		force_sig_fault(SIGTRAP, TRAP_UNK, (void __user *)regs->pc, 0);
-		return;
-
-	case 2: /* gentrap */
+	case IF_GENTRAP:
+		regs->pc -= 4;
 		switch ((long)regs->r16) {
 		case GEN_INTOVF:
 			signo = SIGFPE;
@@ -245,6 +245,7 @@ do_entIF(unsigned long inst_type, struct pt_regs *regs)
 		case GEN_SUBRNG6:
 		case GEN_SUBRNG7:
 		default:
+			regs->pc += 4;
 			signo = SIGTRAP;
 			code = TRAP_UNK;
 			break;
@@ -253,7 +254,11 @@ do_entIF(unsigned long inst_type, struct pt_regs *regs)
 		force_sig_fault(signo, code, (void __user *)regs->pc, regs->r16);
 		return;
 
-	case 4: /* opDEC */
+	case IF_FEN:
+		fpu_enable();
+		return;
+
+	case IF_OPDEC:
 		switch (inst) {
 		case BREAK_KPROBE:
 			if (notify_die(DIE_BREAK, "kprobe", regs, 0, 0, SIGTRAP) == NOTIFY_STOP)
@@ -268,27 +273,15 @@ do_entIF(unsigned long inst_type, struct pt_regs *regs)
 			if (notify_die(DIE_UPROBE_XOL, "uprobe_xol", regs, 0, 0, SIGTRAP) == NOTIFY_STOP)
 				return;
 		}
-		if (!user_mode(regs))
+
+		if (user_mode(regs))
+			regs->pc -= 4;
+		else
 			die("Instruction fault", regs, type);
 		break;
 
-	case 3: /* FEN fault */
-		/*
-		 * Irritating users can call HMC_clrfen to disable the
-		 * FPU for the process. The kernel will then trap to
-		 * save and restore the FP registers.
-
-		 * Given that GCC by default generates code that uses the
-		 * FP registers, HMC_clrfen is not useful except for DoS
-		 * attacks. So turn the bleeding FPU back on and be done
-		 * with it.
-		 */
-		current_thread_info()->pcb.flags |= 1;
-		__reload_thread(&current_thread_info()->pcb);
-		return;
-
-	case 5: /* illoc */
 	default: /* unexpected instruction-fault type */
+		regs->pc -= 4;
 		break;
 	}
 
diff --git a/arch/sw_64/kernel/vdso/vgettimeofday.c b/arch/sw_64/kernel/vdso/vgettimeofday.c
index b9c9a137f9d3438d500db5f511008652e1f371d6..49bb4e2e66ed40526f6154ae4649a4c5db824235 100644
--- a/arch/sw_64/kernel/vdso/vgettimeofday.c
+++ b/arch/sw_64/kernel/vdso/vgettimeofday.c
@@ -16,6 +16,7 @@
 
 #include <asm/unistd.h>
 #include <asm/vdso.h>
+#include <asm/hmcall.h>
 
 static __always_inline int syscall_fallback(clockid_t clkid, struct timespec64 *ts)
 {
@@ -25,8 +26,8 @@ static __always_inline int syscall_fallback(clockid_t clkid, struct timespec64 *
 	"	mov		%0, $16\n"
 	"	mov		%1, $17\n"
 	"	ldi		$0, %2\n"
-	"	sys_call	0x83\n"
-	:: "r"(clkid), "r"(ts), "i"(__NR_clock_gettime)
+	"	sys_call	%3\n"
+	:: "r"(clkid), "r"(ts), "i"(__NR_clock_gettime), "i"(HMC_callsys)
 	: "$0", "$16", "$17", "$19");
 	if (unlikely(r19))
 		return -r0;
@@ -78,9 +79,7 @@ static __always_inline u64 read_longtime(void)
 	register unsigned long __r0 __asm__("$0");
 
 	__asm__ __volatile__(
-		"sys_call 0xB1"
-		: "=r"(__r0)
-		::"memory");
+		"sys_call %1" : "=r"(__r0) : "i" (HMC_longtime));
 
 	return __r0;
 }
diff --git a/arch/sw_64/kernel/vdso/vrt_sigreturn.S b/arch/sw_64/kernel/vdso/vrt_sigreturn.S
index 6aa7aa300b4d119a844ea1fcd75a1985b1685573..d2d7295ffa7ac13d2c0fea96cb56b50eb72af6b3 100644
--- a/arch/sw_64/kernel/vdso/vrt_sigreturn.S
+++ b/arch/sw_64/kernel/vdso/vrt_sigreturn.S
@@ -19,6 +19,7 @@
 
 #include <linux/linkage.h>
 #include <asm/unistd.h>
+#include <uapi/asm/hmcall.h>
 
 #define RT_SIGFRAME_SIZE	1600
 #define RT_SIGFRAME_MCTX	176
@@ -64,6 +65,6 @@
 ENTRY(__vdso_rt_sigreturn)
 	mov		$sp, $16
 	ldi		$0, __NR_rt_sigreturn
-	sys_call	0x83
+	sys_call	HMC_callsys
 ENDPROC(__vdso_rt_sigreturn)
 	.cfi_endproc
diff --git a/arch/sw_64/kernel/vmlinux.lds.S b/arch/sw_64/kernel/vmlinux.lds.S
index a106be42121f58d8b6af8b8776944fe946b85a75..07bc3d8ee7e47bcc98c2c3de0635ab1f152b3662 100644
--- a/arch/sw_64/kernel/vmlinux.lds.S
+++ b/arch/sw_64/kernel/vmlinux.lds.S
@@ -33,7 +33,7 @@ SECTIONS
 	} :text
 	_etext = .;	/* End of text section */
 
-	RO_DATA(4096)
+	RO_DATA(PAGE_SIZE)
 
 	/* Will be freed after init */
 	__init_begin = ALIGN(PAGE_SIZE);
diff --git a/arch/sw_64/kvm/Kconfig b/arch/sw_64/kvm/Kconfig
index 4b6201ff5dc80bacfe731c0d3469d847caeaef62..8077ea4527654fe6e0329cd0a8942da9490f49f5 100644
--- a/arch/sw_64/kvm/Kconfig
+++ b/arch/sw_64/kvm/Kconfig
@@ -29,6 +29,7 @@ config KVM
 	select KVM_VFIO
 	select TUN
 	select GENERIC_ALLOCATOR
+	select KVM_GENERIC_DIRTYLOG_READ_PROTECT
 	help
 	  Support for hosting Guest kernels.
 	  We don't support KVM with 3-level page tables yet.
diff --git a/arch/sw_64/kvm/Makefile b/arch/sw_64/kvm/Makefile
index 48ae938faab7733e3f155490f3653e445bbc9068..43cea19215ffa91a6006631553cee9ce98098be9 100644
--- a/arch/sw_64/kvm/Makefile
+++ b/arch/sw_64/kvm/Makefile
@@ -8,6 +8,6 @@ KVM := ../../../virt/kvm
 ccflags-y += -Ivirt/kvm -Iarch/sw_64/kvm
 
 kvm-$(CONFIG_KVM_SW64_HOST) += $(KVM)/kvm_main.o $(KVM)/eventfd.o $(KVM)/irqchip.o $(KVM)/vfio.o
-kvm-$(CONFIG_KVM_SW64_HOST) += kvm-sw64.o entry.o emulate.o mmio.o kvm_timer.o handle_exit.o
+kvm-$(CONFIG_KVM_SW64_HOST) += kvm-sw64.o entry.o emulate.o mmio.o kvm_timer.o handle_exit.o perf.o
 
 obj-$(CONFIG_KVM_SW64_HOST) += kvm.o
diff --git a/arch/sw_64/kvm/handle_exit.c b/arch/sw_64/kvm/handle_exit.c
index 5016bc0eddc2f86fa99f3b9e3247c7f138344e54..52f40a4c5803bf5980308b18fbfb6459ab42bdec 100644
--- a/arch/sw_64/kvm/handle_exit.c
+++ b/arch/sw_64/kvm/handle_exit.c
@@ -13,9 +13,18 @@
 int handle_exit(struct kvm_vcpu *vcpu, struct kvm_run *run,
 		int exception_index, struct hcall_args *hargs)
 {
+	gfn_t gfn;
+
 	switch (exception_index) {
 	case SW64_KVM_EXIT_IO:
 		return io_mem_abort(vcpu, run, hargs);
+	case SW64_KVM_MIGRATION_SET_DIRTY_HM:
+	case SW64_KVM_MIGRATION_SET_DIRTY:
+		gfn = hargs->arg2 >> 24;
+		mutex_lock(&vcpu->kvm->slots_lock);
+		kvm_vcpu_mark_page_dirty(vcpu, gfn);
+		mutex_unlock(&vcpu->kvm->slots_lock);
+		return 1;
 	case SW64_KVM_EXIT_HALT:
 		vcpu->arch.halted = 1;
 		kvm_vcpu_block(vcpu);
diff --git a/arch/sw_64/kvm/irq.h b/arch/sw_64/kvm/irq.h
index ee56d9b97632f3be9962aa832fafb32d4cc35e41..9268ab6af4920818566e2d39e056be795429d20d 100644
--- a/arch/sw_64/kvm/irq.h
+++ b/arch/sw_64/kvm/irq.h
@@ -3,10 +3,10 @@
  * irq.h: in kernel interrupt controller related definitions
  */
 
-#ifndef __IRQ_H
-#define __IRQ_H
+#ifndef _SW64_KVM_IRQ_H
+#define _SW64_KVM_IRQ_H
 static inline int irqchip_in_kernel(struct kvm *kvm)
 {
 	return 1;
 }
-#endif
+#endif /* _SW64_KVM_IRQ_H */
diff --git a/arch/sw_64/kvm/kvm-sw64.c b/arch/sw_64/kvm/kvm-sw64.c
index 9d209141820cd4d6679662b30e6f26130a05aacf..ffcfdee58a4877f527b9ac2f41fa8fb348f71f6d 100644
--- a/arch/sw_64/kvm/kvm-sw64.c
+++ b/arch/sw_64/kvm/kvm-sw64.c
@@ -16,6 +16,9 @@
 #include <asm/kvm_timer.h>
 #include <asm/kvm_emulate.h>
 
+#define CREATE_TRACE_POINTS
+#include "trace.h"
+
 #include "../kernel/pci_impl.h"
 #include "vmem.c"
 
@@ -34,6 +37,13 @@ extern bool bind_vcpu_enabled;
 #define HARDWARE_VPN_MASK	((1UL << WIDTH_HARDWARE_VPN) - 1)
 #define VPN_SHIFT		(64 - WIDTH_HARDWARE_VPN)
 
+static DEFINE_PER_CPU(struct kvm_vcpu *, kvm_running_vcpu);
+
+static void kvm_set_running_vcpu(struct kvm_vcpu *vcpu)
+{
+	__this_cpu_write(kvm_running_vcpu, vcpu);
+}
+
 int vcpu_interrupt_line(struct kvm_vcpu *vcpu, int number, bool level)
 {
 	set_bit(number, (vcpu->arch.irqs_pending));
@@ -123,6 +133,19 @@ static void sw64_kvm_switch_vpn(struct kvm_vcpu *vcpu)
 	}
 }
 
+static void check_vcpu_requests(struct kvm_vcpu *vcpu)
+{
+	unsigned long vpn;
+	long cpu = smp_processor_id();
+
+	if (kvm_request_pending(vcpu)) {
+		if (kvm_check_request(KVM_REQ_TLB_FLUSH, vcpu)) {
+			vpn = vcpu->arch.vpnc[cpu] & HARDWARE_VPN_MASK;
+			tbivpn(0, 0, vpn);
+		}
+	}
+}
+
 struct kvm_stats_debugfs_item debugfs_entries[] = {
 	{ NULL }
 };
@@ -167,12 +190,47 @@ int kvm_arch_vcpu_should_kick(struct kvm_vcpu *vcpu)
 	return kvm_vcpu_exiting_guest_mode(vcpu) == IN_GUEST_MODE;
 }
 
+/*
+ * kvm_mark_migration write the mark on every vcpucbs of the kvm, which tells
+ * the system to do migration while the mark is on, and flush all vcpu's tlbs
+ * at the beginning of the migration.
+ */
+void kvm_mark_migration(struct kvm *kvm, int mark)
+{
+	struct kvm_vcpu *vcpu;
+	int cpu;
+
+	kvm_for_each_vcpu(cpu, vcpu, kvm)
+		vcpu->arch.vcb.migration_mark = mark << 2;
+
+	kvm_flush_remote_tlbs(kvm);
+}
+
 void kvm_arch_commit_memory_region(struct kvm *kvm,
 		const struct kvm_userspace_memory_region *mem,
 		struct kvm_memory_slot *old,
 		const struct kvm_memory_slot *new,
 		enum kvm_mr_change change)
 {
+	/*
+	 * At this point memslot has been committed and there is an
+	 * allocated dirty_bitmap[], dirty pages will be be tracked while the
+	 * memory slot is write protected.
+	 */
+
+	/* If dirty logging has been stopped, do nothing for now. */
+	if ((change != KVM_MR_DELETE)
+			&& (old->flags & KVM_MEM_LOG_DIRTY_PAGES)
+			&& (!(new->flags & KVM_MEM_LOG_DIRTY_PAGES))) {
+		kvm_mark_migration(kvm, 0);
+		return;
+	}
+
+	/* If it's the first time dirty logging, flush all vcpu tlbs. */
+	if ((change == KVM_MR_FLAGS_ONLY)
+			&& (!(old->flags & KVM_MEM_LOG_DIRTY_PAGES))
+			&& (new->flags & KVM_MEM_LOG_DIRTY_PAGES))
+		kvm_mark_migration(kvm, 1);
 }
 
 int kvm_vm_ioctl_check_extension(struct kvm *kvm, long ext)
@@ -183,6 +241,7 @@ int kvm_vm_ioctl_check_extension(struct kvm *kvm, long ext)
 	case KVM_CAP_IRQCHIP:
 	case KVM_CAP_IOEVENTFD:
 	case KVM_CAP_SYNC_MMU:
+	case KVM_CAP_IMMEDIATE_EXIT:
 		r = 1;
 		break;
 	case KVM_CAP_NR_VCPUS:
@@ -196,9 +255,10 @@ int kvm_vm_ioctl_check_extension(struct kvm *kvm, long ext)
 	return r;
 }
 
-int kvm_vm_ioctl_get_dirty_log(struct kvm *kvm, struct kvm_dirty_log *log)
+void kvm_arch_mmu_enable_log_dirty_pt_masked(struct kvm *kvm,
+		struct kvm_memory_slot *slot, gfn_t gfn_offset,
+		unsigned long mask)
 {
-	return 0;
 }
 
 int kvm_sw64_pending_timer(struct kvm_vcpu *vcpu)
@@ -308,6 +368,12 @@ int kvm_arch_prepare_memory_region(struct kvm *kvm,
 	if (change == KVM_MR_FLAGS_ONLY || change == KVM_MR_DELETE)
 		return 0;
 
+	if (test_bit(IO_MARK_BIT, &(mem->guest_phys_addr)))
+		return 0;
+
+	if (test_bit(IO_MARK_BIT + 1, &(mem->guest_phys_addr)))
+		return 0;
+
 #ifndef CONFIG_KVM_MEMHOTPLUG
 	if (mem->guest_phys_addr) {
 		pr_info("%s, No KVM MEMHOTPLUG support!\n", __func__);
@@ -315,12 +381,6 @@ int kvm_arch_prepare_memory_region(struct kvm *kvm,
 	}
 #endif
 
-	if (test_bit(IO_MARK_BIT, &(mem->guest_phys_addr)))
-		return 0;
-
-	if (test_bit(IO_MARK_BIT + 1, &(mem->guest_phys_addr)))
-		return 0;
-
 	if (!sw64_kvm_pool)
 		return -ENOMEM;
 
@@ -411,6 +471,7 @@ int kvm_arch_vcpu_reset(struct kvm_vcpu *vcpu)
 {
 	unsigned long addr = vcpu->kvm->arch.host_phys_addr;
 
+	hrtimer_cancel(&vcpu->arch.hrt);
 	vcpu->arch.vcb.whami = vcpu->vcpu_id;
 	vcpu->arch.vcb.vcpu_irq_disabled = 1;
 	vcpu->arch.pcpu_id = -1; /* force flush tlb for the first time */
@@ -461,6 +522,7 @@ int kvm_arch_vcpu_setup(struct kvm_vcpu *vcpu)
 void kvm_arch_vcpu_load(struct kvm_vcpu *vcpu, int cpu)
 {
 	vcpu->cpu = cpu;
+	kvm_set_running_vcpu(vcpu);
 }
 
 void kvm_arch_vcpu_put(struct kvm_vcpu *vcpu)
@@ -471,6 +533,7 @@ void kvm_arch_vcpu_put(struct kvm_vcpu *vcpu)
 	 * optimized make_all_cpus_request path.
 	 */
 	vcpu->cpu = -1;
+	kvm_set_running_vcpu(NULL);
 }
 
 int kvm_arch_vcpu_ioctl_get_mpstate(struct kvm_vcpu *vcpu,
@@ -534,6 +597,9 @@ int kvm_arch_vcpu_ioctl_run(struct kvm_vcpu *vcpu)
 	bool more;
 	sigset_t sigsaved;
 
+	if (run->immediate_exit)
+		return -EINTR;
+
 	/* Set guest vcb */
 	/* vpn will update later when vcpu is running */
 	if (vcpu->arch.vcb.vpcr == 0) {
@@ -560,6 +626,9 @@ int kvm_arch_vcpu_ioctl_run(struct kvm_vcpu *vcpu)
 		vcpu->arch.vcb.upcr = 0x7;
 	}
 
+#ifdef CONFIG_PERF_EVENTS
+	vcpu_load(vcpu);
+#endif
 	if (vcpu->sigset_active)
 		sigprocmask(SIG_SETMASK, &vcpu->sigset, &sigsaved);
 
@@ -597,9 +666,11 @@ int kvm_arch_vcpu_ioctl_run(struct kvm_vcpu *vcpu)
 		vcpu->arch.halted = 0;
 
 		sw64_kvm_switch_vpn(vcpu);
+		check_vcpu_requests(vcpu);
 		guest_enter_irqoff();
 
 		/* Enter the guest */
+		trace_kvm_sw64_entry(vcpu->vcpu_id, vcpu->arch.regs.pc);
 		vcpu->mode = IN_GUEST_MODE;
 
 		ret = __sw64_vcpu_run((struct vcpucb *)__phys_addr((unsigned long)vcb), &(vcpu->arch.regs), &hargs);
@@ -609,6 +680,9 @@ int kvm_arch_vcpu_ioctl_run(struct kvm_vcpu *vcpu)
 
 		local_irq_enable();
 		guest_exit_irqoff();
+
+		trace_kvm_sw64_exit(ret, vcpu->arch.regs.pc);
+
 		preempt_enable();
 
 		/* ret = 0 indicate interrupt in guest mode, ret > 0 indicate hcall */
@@ -618,12 +692,16 @@ int kvm_arch_vcpu_ioctl_run(struct kvm_vcpu *vcpu)
 	if (vcpu->sigset_active)
 		sigprocmask(SIG_SETMASK, &sigsaved, NULL);
 
+#ifdef CONFIG_PERF_EVENTS
+	vcpu_put(vcpu);
+#endif
 	return ret;
 }
 
 long kvm_arch_vcpu_ioctl(struct file *filp,
 		unsigned int ioctl, unsigned long arg)
 {
+	unsigned long result;
 	struct kvm_vcpu *vcpu = filp->private_data;
 	struct vcpucb *kvm_vcb;
 
@@ -631,12 +709,32 @@ long kvm_arch_vcpu_ioctl(struct file *filp,
 	case KVM_SW64_VCPU_INIT:
 		return kvm_arch_vcpu_reset(vcpu);
 	case KVM_SW64_GET_VCB:
+		if (vcpu->arch.vcb.migration_mark) {
+			result = sw64_io_read(0, LONG_TIME);
+			vcpu->arch.vcb.guest_longtime = result;
+			vcpu->arch.vcb.guest_irqs_pending = vcpu->arch.irqs_pending[0];
+		}
+
 		if (copy_to_user((void __user *)arg, &(vcpu->arch.vcb), sizeof(struct vcpucb)))
 			return -EINVAL;
 		break;
 	case KVM_SW64_SET_VCB:
 		kvm_vcb = memdup_user((void __user *)arg, sizeof(*kvm_vcb));
 		memcpy(&(vcpu->arch.vcb), kvm_vcb, sizeof(struct vcpucb));
+
+		if (vcpu->arch.vcb.migration_mark) {
+			/* updated vpcr needed by destination vm */
+			vcpu->arch.vcb.vpcr
+				= get_vpcr(vcpu->kvm->arch.host_phys_addr, vcpu->kvm->arch.size, 0);
+
+			result = sw64_io_read(0, LONG_TIME);
+
+			/* synchronize the longtime of source and destination */
+			vcpu->arch.vcb.guest_longtime_offset = vcpu->arch.vcb.guest_longtime - result;
+
+			set_timer(vcpu, 200000000);
+			vcpu->arch.vcb.migration_mark = 0;
+		}
 		break;
 	default:
 		return -EINVAL;
@@ -666,17 +764,25 @@ long kvm_arch_vm_ioctl(struct file *filp, unsigned int ioctl, unsigned long arg)
 
 int kvm_arch_init(void *opaque)
 {
+	kvm_sw64_perf_init();
 	return 0;
 }
 
 void kvm_arch_exit(void)
 {
+	kvm_sw64_perf_teardown();
 }
 
 void kvm_arch_sync_dirty_log(struct kvm *kvm, struct kvm_memory_slot *memslot)
 {
 }
 
+void kvm_arch_flush_remote_tlbs_memslot(struct kvm *kvm,
+					struct kvm_memory_slot *memslot)
+{
+	kvm_flush_remote_tlbs(kvm);
+}
+
 int kvm_arch_vcpu_precreate(struct kvm *kvm, unsigned int id)
 {
 	return 0;
diff --git a/arch/sw_64/kvm/perf.c b/arch/sw_64/kvm/perf.c
new file mode 100644
index 0000000000000000000000000000000000000000..8d90d79643de025a1fd815a6eee992be6daae892
--- /dev/null
+++ b/arch/sw_64/kvm/perf.c
@@ -0,0 +1,53 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Performance events support for KVM.
+ */
+
+#include <linux/perf_event.h>
+#include <linux/kvm_host.h>
+
+#include <asm/kvm_emulate.h>
+
+static int kvm_is_in_guest(void)
+{
+	return kvm_get_running_vcpu() != NULL;
+}
+
+static int kvm_is_user_mode(void)
+{
+	struct kvm_vcpu *vcpu;
+
+	vcpu = kvm_get_running_vcpu();
+
+	if (vcpu)
+		return (vcpu->arch.regs.ps & 8) != 0;
+
+	return 0;
+}
+
+static unsigned long kvm_get_guest_ip(void)
+{
+	struct kvm_vcpu *vcpu;
+
+	vcpu = kvm_get_running_vcpu();
+
+	if (vcpu)
+		return vcpu->arch.regs.pc;
+	return 0;
+}
+
+static struct perf_guest_info_callbacks kvm_guest_cbs = {
+	.is_in_guest	= kvm_is_in_guest,
+	.is_user_mode	= kvm_is_user_mode,
+	.get_guest_ip	= kvm_get_guest_ip,
+};
+
+int kvm_sw64_perf_init(void)
+{
+	return perf_register_guest_info_callbacks(&kvm_guest_cbs);
+}
+
+int kvm_sw64_perf_teardown(void)
+{
+	return perf_unregister_guest_info_callbacks(&kvm_guest_cbs);
+}
diff --git a/arch/sw_64/kvm/trace.h b/arch/sw_64/kvm/trace.h
new file mode 100644
index 0000000000000000000000000000000000000000..2611df3d3fa57658881319c3384979aad2ea302c
--- /dev/null
+++ b/arch/sw_64/kvm/trace.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#if !defined(_SW64_KVM_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _SW64_KVM_TRACE_H
+
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM kvm
+
+/*
+ * Tracepoint for guest mode entry.
+ */
+TRACE_EVENT(kvm_sw64_entry,
+	TP_PROTO(unsigned int vcpu_id, unsigned int vcpu_pc),
+	TP_ARGS(vcpu_id, vcpu_pc),
+
+	TP_STRUCT__entry(
+		__field(unsigned int,   vcpu_id)
+		__field(unsigned int,	vcpu_pc)
+	),
+
+	TP_fast_assign(
+		__entry->vcpu_id = vcpu_id;
+		__entry->vcpu_pc = vcpu_pc;
+	),
+
+	TP_printk("VCPU %u: PC: 0x%08x", __entry->vcpu_id, __entry->vcpu_pc)
+);
+
+/*
+ * Tracepoint for guest mode exit.
+ */
+
+TRACE_EVENT(kvm_sw64_exit,
+	TP_PROTO(unsigned int exit_reason, unsigned long vcpu_pc),
+	TP_ARGS(exit_reason, vcpu_pc),
+
+	TP_STRUCT__entry(
+		__field(unsigned int,	exit_reason)
+		__field(unsigned long,	vcpu_pc)
+	),
+
+	TP_fast_assign(
+		__entry->exit_reason = exit_reason;
+		__entry->vcpu_pc = vcpu_pc;
+	),
+
+	TP_printk("exit_reason: 0x%04x (%11s),  PC: 0x%08lx",
+		__entry->exit_reason,
+		__print_symbolic(__entry->exit_reason, kvm_sw64_exception_type),
+		__entry->vcpu_pc)
+);
+
+#endif /* _SW64_KVM_TRACE_H */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/arch/sw_64/lib/deep-copy_template.S b/arch/sw_64/lib/deep-copy_template.S
index 8355ecf8a905faf58b8da992d70be04efc05f8d0..7705eb3f36d4edab9e09c49bafbe3129086b6252 100644
--- a/arch/sw_64/lib/deep-copy_template.S
+++ b/arch/sw_64/lib/deep-copy_template.S
@@ -3,9 +3,15 @@
 /*
  * template for memcpy and copy_user with SIMD
  *
- * $16: current store address
- * $17: current load address
- * $18: current bytes left to copy
+ * $4:	8-byte misalignment of src when dest is 8-byte aligned
+ * $5:	32-byte misalignment of src when dest is 32-byte aligned
+ * $7:	SIMD status
+ *	0: not in simd loop
+ *	1: in simd loop
+ *	2: in simd_u loop
+ * $16:	latest dest, clobbered
+ * $17:	latest src, clobbered
+ * $18:	bytes left to copy
  *
  */
 
@@ -16,27 +22,27 @@
 	addl	$sp, 0x1f, $23; \
 	bic	$23, 0x1f, $23; \
 	vstd	$f1, 0($23); \
-	vstd	$f2, 0x20($23)
+	vstd	$f2, 0x20($23); \
+	ldi	$7, 1
 
 #define RESTORE_SIMD_REGS \
 	addl	$sp, 0x1f, $23; \
 	bic	$23, 0x1f, $23; \
 	vldd	$f1, 0($23); \
 	vldd	$f2, 0x20($23); \
-	ldi	$sp, 0x60($sp)
+	ldi	$sp, 0x60($sp); \
+	bis	$31, $31, $7
 
 #define SAVE_SIMD_U_REGS \
-	ldi	$sp, -0x120($sp); \
+	ldi	$sp, -0xc0($sp); \
 	addl	$sp, 0x1f, $23; \
 	bic	$23, 0x1f, $23; \
 	vstd	$f1, 0($23); \
 	vstd	$f2, 0x20($23); \
 	vstd	$f4, 0x40($23); \
 	vstd	$f5, 0x60($23); \
-	vstd	$f10, 0x80($23); \
-	vstd	$f11, 0xa0($23); \
-	vstd	$f20, 0xc0($23); \
-	vstd	$f21, 0xe0($23)
+	vstd	$f3, 0x80($23); \
+	ldi	$7, 2
 
 #define RESTORE_SIMD_U_REGS \
 	addl	$sp, 0x1f, $23; \
@@ -45,22 +51,19 @@
 	vldd	$f2, 0x20($23); \
 	vldd	$f4, 0x40($23); \
 	vldd	$f5, 0x60($23); \
-	vldd	$f10, 0x80($23); \
-	vldd	$f11, 0xa0($23); \
-	vldd	$f20, 0xc0($23); \
-	vldd	$f21, 0xe0($23); \
-	ldi	$sp, 0x120($sp)
+	vldd	$f3, 0x80($23); \
+	ldi	$sp, 0xc0($sp); \
+	bis	$31, $31, $7
 
 	ble	$18, $out
 	and	$16, 7, $1
 	beq	$1, $dest_aligned_8
 
-	.align 4
 $byte_loop_head:
 	FIXUP_LDST( ldbu $2, 0($17) )
+	FIXUP_LDST( stb $2, 0($16) )
 	subl	$18, 1, $18
 	addl	$17, 1, $17
-	FIXUP_LDST( stb $2, 0($16) )
 	addl	$16, 1, $16
 	ble	$18, $out
 	and	$16, 7, $1
@@ -68,27 +71,27 @@ $byte_loop_head:
 
 $dest_aligned_8:
 	and	$17, 7, $4
-	subl	$18, 16, $18
-	blt	$18, $quad_end
-	subl	$18, 64, $18
-	blt	$18, $simd_end
+	cmplt	$18, 16, $1
+	bne	$1, $quad_loop_end
 	and	$16, 31, $1
 	beq	$1, $dest_aligned_32
+	cmplt	$18, 64, $1
+	bne	$1, $simd_end
 	bne	$4, $quad_u_loop_head
 
-	.align 5
 $quad_loop_head:
 	FIXUP_LDST( ldl $2, 0($17) )
-	subl	$18, 8, $18
-	addl	$17, 8, $17
 	FIXUP_LDST( stl $2, 0($16) )
 	addl	$16, 8, $16
+	addl	$17, 8, $17
+	subl	$18, 8, $18
 	and	$16, 31, $1
-	blt	$18, $simd_end
-	beq	$16, $dest_aligned_32
+	beq	$1, $dest_aligned_32
 	br	$31, $quad_loop_head
 
 $dest_aligned_32:
+	cmplt	$18, 64, $1
+	bne	$1, $simd_end
 	and	$17, 31, $5
 	bne	$5, $prep_simd_u_loop
 
@@ -98,63 +101,63 @@ $prep_simd_loop:
 	cmple	$18, $1, $1
 	bne	$1, $simd_loop
 
-	.align 5
+	.align 4
 $simd_loop_nc:
-	fillcs	128 * 5($17)
 	FIXUP_LDST( vldd $f1, 0($17) )
 	FIXUP_LDST( vldd $f2, 32($17) )
-	subl	$18, 64, $18
-	addl	$17, 64, $17
 	FIXUP_LDST( vstd_nc $f1, 0($16) )
 	FIXUP_LDST( vstd_nc $f2, 32($16) )
+	subl	$18, 64, $18
+	addl	$17, 64, $17
 	addl	$16, 64, $16
-	bge	$18, $simd_loop_nc
+	cmplt	$18, 64, $1
+	beq	$1, $simd_loop_nc
 	memb			# required for _nc store instructions
 	br	$31, $simd_loop_end
 
-	.align 5
+	.align 4
 $simd_loop:
-	fillcs	128 * 5($17)
 	FIXUP_LDST( vldd $f1, 0($17) )
 	FIXUP_LDST( vldd $f2, 32($17) )
-	subl	$18, 64, $18
-	addl	$17, 64, $17
 	FIXUP_LDST( vstd $f1, 0($16) )
 	FIXUP_LDST( vstd $f2, 32($16) )
+	subl	$18, 64, $18
+	addl	$17, 64, $17
 	addl	$16, 64, $16
-	bge	$18, $simd_loop
+	cmplt	$18, 64, $1
+	beq	$1, $simd_loop
 
 $simd_loop_end:
-	addl	$18, 64, $1
-	cmplt	$1, 32, $1
+	cmplt	$18, 32, $1
 	bne	$1, $no_more_simd
 	FIXUP_LDST( vldd $f1, 0($17) )
+	FIXUP_LDST( vstd $f1, 0($16) )
 	subl	$18, 32, $18
 	addl	$17, 32, $17
-	FIXUP_LDST( vstd $f1, 0($16) )
 	addl	$16, 32, $16
 
 $no_more_simd:
 	RESTORE_SIMD_REGS
 
 $simd_end:
-	addl	$18, 64, $18
-	blt	$18, $quad_end
+	ble	$18, $out
+	cmplt	$18, 16, $1
+	bne	$1, $quad_loop_end
 	bne	$4, $prep_quad_u_loop_tail
 
 	.align 4
 $quad_loop_tail:
 	FIXUP_LDST( ldl $2, 0($17) )
 	FIXUP_LDST( ldl $3, 8($17) )
-	subl	$18, 16, $18
-	addl	$17, 16, $17
 	FIXUP_LDST( stl $2, 0($16) )
 	FIXUP_LDST( stl $3, 8($16) )
+	subl	$18, 16, $18
+	addl	$17, 16, $17
 	addl	$16, 16, $16
-	bge	$18, $quad_loop_tail
+	cmplt	$18, 16, $1
+	beq	$1, $quad_loop_tail
 
-$quad_end:
-	addl	$18, 16, $18
+$quad_loop_end:
 	ble	$18, $out
 	cmplt	$18, 8, $1
 	bne	$1, $byte_loop_tail
@@ -162,36 +165,35 @@ $quad_end:
 
 $move_one_quad:
 	FIXUP_LDST( ldl $2, 0($17) )
+	FIXUP_LDST( stl $2, 0($16) )
 	subl	$18, 8, $18
 	addl	$17, 8, $17
-	FIXUP_LDST( stl $2, 0($16) )
 	addl	$16, 8, $16
 	ble	$18, $out
 
-	.align 4
+	.align 3
 $byte_loop_tail:
 	FIXUP_LDST( ldbu $2, 0($17) )
+	FIXUP_LDST( stb $2, 0($16) )
 	subl	$18, 1, $18
 	addl	$17, 1, $17
-	FIXUP_LDST( stb $2, 0($16) )
 	addl	$16, 1, $16
 	bgt	$18, $byte_loop_tail
 	br	$31, $out
 
 /* misaligned src and dst */
-	.align 5
 $quad_u_loop_head:
 	FIXUP_LDST( ldl_u $2, 0($17) )
 	FIXUP_LDST( ldl_u $3, 7($17) )
-	subl	$18, 8, $18
-	addl	$17, 8, $17
 	extll	$2, $4, $2
 	exthl	$3, $4, $3
 	bis	$2, $3, $2
 	FIXUP_LDST( stl $2, 0($16) )
 	addl	$16, 8, $16
-	blt	$18, $simd_end
-	beq	$16, $dest_aligned_32
+	addl	$17, 8, $17
+	subl	$18, 8, $18
+	and	$16, 31, $1
+	beq	$1, $dest_aligned_32
 	br	$31, $quad_u_loop_head
 
 $prep_simd_u_loop:
@@ -209,53 +211,52 @@ $prep_simd_u_loop:
 	cmple	$18, $1, $1
 	bne	$1, $simd_u_loop
 
-	.align 5
+	.align 4
 $simd_u_loop_nc:
 	FIXUP_LDST( vldd $f5, 32($3) )
-	fillcs	128 * 5($3)
-	srlow	$f4, $f1, $f10
-	sllow	$f5, $f2, $f11
-	vlogfc	$f10, $f11, $f31, $f10
+	srlow	$f4, $f1, $f4
+	sllow	$f5, $f2, $f3
+	vlogfc	$f3, $f4, $f31, $f3
+	FIXUP_LDST( vstd_nc $f3, 0($16) )
 	FIXUP_LDST( vldd $f4, 64($3) )
-	srlow	$f5, $f1, $f20
-	sllow	$f4, $f2, $f21
-	vlogfc	$f20, $f21, $f31, $f20
-	FIXUP_LDST( vstd_nc $f10, 0($16) )
-	FIXUP_LDST( vstd_nc $f20, 32($16) )
+	srlow	$f5, $f1, $f5
+	sllow	$f4, $f2, $f3
+	vlogfc	$f5, $f3, $f31, $f5
+	FIXUP_LDST( vstd_nc $f5, 32($16) )
 	subl	$18, 64, $18
 	addl	$3, 64, $3
 	addl	$16, 64, $16
-	bge	$18, $simd_u_loop_nc
+	cmplt	$18, 64, $1
+	beq	$1, $simd_u_loop_nc
 	memb			# required for _nc store instructions
 	br	$31, $simd_u_loop_end
 
-	.align 5
+	.align 4
 $simd_u_loop:
 	FIXUP_LDST( vldd $f5, 32($3) )
-	fillcs	128 * 5($3)
-	srlow	$f4, $f1, $f10
-	sllow	$f5, $f2, $f11
-	vlogfc	$f10, $f11, $f31, $f10
+	srlow	$f4, $f1, $f4
+	sllow	$f5, $f2, $f3
+	vlogfc	$f4, $f3, $f31, $f3
+	FIXUP_LDST( vstd $f3, 0($16) )
 	FIXUP_LDST( vldd $f4, 64($3) )
-	srlow	$f5, $f1, $f20
-	sllow	$f4, $f2, $f21
-	vlogfc	$f20, $f21, $f31, $f20
-	FIXUP_LDST( vstd $f10, 0($16) )
-	FIXUP_LDST( vstd $f20, 32($16) )
+	srlow	$f5, $f1, $f5
+	sllow	$f4, $f2, $f3
+	vlogfc	$f5, $f3, $f31, $f3
+	FIXUP_LDST( vstd $f3, 32($16) )
 	subl	$18, 64, $18
 	addl	$3, 64, $3
 	addl	$16, 64, $16
-	bge	$18, $simd_u_loop
+	cmplt	$18, 64, $1
+	beq	$1, $simd_u_loop
 
 $simd_u_loop_end:
-	addl	$18, 64, $1
-	cmplt	$1, 32, $1
+	cmplt	$18, 32, $1
 	bne	$1, $no_more_simd_u
 	FIXUP_LDST( vldd $f5, 32($3) )
-	srlow	$f4, $f1, $f10
-	sllow	$f5, $f2, $f11
-	vlogfc	$f10, $f11, $f31, $f10
-	FIXUP_LDST( vstd $f10, 0($16) )
+	srlow	$f4, $f1, $f4
+	sllow	$f5, $f2, $f3
+	vlogfc	$f4, $f3, $f31, $f3
+	FIXUP_LDST( vstd $f3, 0($16) )
 	subl	$18, 32, $18
 	addl	$3, 32, $3
 	addl	$16, 32, $16
@@ -267,7 +268,7 @@ $no_more_simd_u:
 
 $prep_quad_u_loop_tail:
 	FIXUP_LDST( ldl_u $2, 0($17) )
-	.align 5
+	.align 4
 $quad_u_loop_tail:
 	FIXUP_LDST( ldl_u $3, 8($17) )
 	extll	$2, $4, $22
@@ -282,18 +283,19 @@ $quad_u_loop_tail:
 	subl	$18, 16, $18
 	addl	$17, 16, $17
 	addl	$16, 16, $16
-	bge	$18, $quad_u_loop_tail
-	br	$31, $quad_end
+	cmplt	$18, 16, $1
+	beq	$1, $quad_u_loop_tail
+	br	$31, $quad_loop_end
 
 $move_one_quad_u:
 	FIXUP_LDST( ldl_u $2, 0($17) )
 	FIXUP_LDST( ldl_u $3, 8($17) )
-	subl	$18, 8, $18
-	addl	$17, 8, $17
 	extll	$2, $4, $22
 	exthl	$3, $4, $23
 	bis	$22, $23, $22
 	FIXUP_LDST( stl $22, 0($16) )
+	subl	$18, 8, $18
+	addl	$17, 8, $17
 	addl	$16, 8, $16
 	ble	$18, $out
 	br	$31, $byte_loop_tail
diff --git a/arch/sw_64/lib/deep-copy_user.S b/arch/sw_64/lib/deep-copy_user.S
index 145e1cc6ba18505f518401e73d112c6b7f2e3e0d..327cab322765ab2f4758812cd178d437726eb44d 100644
--- a/arch/sw_64/lib/deep-copy_user.S
+++ b/arch/sw_64/lib/deep-copy_user.S
@@ -10,13 +10,34 @@
 	ldi $31, $out-99b($31);		\
 	.previous
 
+/*
+ * $7:	SIMD status
+ *	0: not in simd loop
+ *	1: in simd loop
+ *	2: in simd_u loop
+ * $18:	bytes left to copy
+ *
+ */
 	.globl __copy_user
 	.ent __copy_user
 __copy_user:
 	.prologue 0
+	bis	$31, $31, $7
 #include "deep-copy_template.S"
 $out:
 	bis	$31, $18, $0
+	beq	$7, $return
+	subl	$7, 1, $7
+	beq	$7, $restore_simd
+
+$restore_simd_u:
+	RESTORE_SIMD_U_REGS
+	br	$31, $return
+
+$restore_simd:
+	RESTORE_SIMD_REGS
+
+$return:
 	ret
 	.end __copy_user
 	EXPORT_SYMBOL(__copy_user)
diff --git a/arch/sw_64/lib/iomap_copy.c b/arch/sw_64/lib/iomap_copy.c
index 10e756fffff5ec8a97671ad06d9abe2a9e8f4aaf..1c75bd602d7e7fcd01591cdcbdd73b8e6d258aec 100644
--- a/arch/sw_64/lib/iomap_copy.c
+++ b/arch/sw_64/lib/iomap_copy.c
@@ -41,15 +41,12 @@ void __iowrite64_copy(void __iomem *to,
 		      const void *from,
 		      size_t count)
 {
-#ifdef CONFIG_64BIT
 	u64 __iomem *dst = to;
 	const u64 *src = from;
 	const u64 *end = src + count;
 
-	while (src < end)
+	while (src < end) {
 		__raw_writeq(*src++, dst++);
 		mb();
-#else
-	__iowrite32_copy(to, from, count * 2);
-#endif
+	}
 }
diff --git a/arch/sw_64/lib/udelay.c b/arch/sw_64/lib/udelay.c
index 48356ab8872f89f6f3fb75189c4fa9760f14b1bc..59ca8a97d748895a49e4fbaf83a85eee99f0d459 100644
--- a/arch/sw_64/lib/udelay.c
+++ b/arch/sw_64/lib/udelay.c
@@ -28,12 +28,6 @@ void __delay(unsigned long loops)
 }
 EXPORT_SYMBOL(__delay);
 
-#ifdef CONFIG_SMP
-#define LPJ	 cpu_data[smp_processor_id()].loops_per_jiffy
-#else
-#define LPJ	 loops_per_jiffy
-#endif
-
 void udelay(unsigned long usecs)
 {
 	unsigned long loops = usecs * get_cpu_freq() / 1000000;
diff --git a/arch/sw_64/math-emu/math.c b/arch/sw_64/math-emu/math.c
index 9f281d82ad83cc76632901f9f2f96aaac7a17b47..6da3aadcff88dffb2498ed7beb39fb6fb04b390b 100644
--- a/arch/sw_64/math-emu/math.c
+++ b/arch/sw_64/math-emu/math.c
@@ -188,14 +188,14 @@ void write_fp_reg_s(unsigned long reg, unsigned long val_p0,
 		unsigned long p1, unsigned long p2, unsigned long p3);
 void write_fp_reg_d(unsigned long reg, unsigned long val_p0,
 		unsigned long p1, unsigned long p2, unsigned long p3);
-#define	LOW_64_WORKING	1
+#define LOW_64_WORKING	1
 #define HIGH_64_WORKING	2
 
 /*
  * End for sw64
  */
 
-#define	OPC_HMC		0x00
+#define OPC_HMC		0x00
 #define OPC_INTA	0x10
 #define OPC_INTL	0x11
 #define OPC_INTS	0x12
@@ -205,7 +205,7 @@ void write_fp_reg_d(unsigned long reg, unsigned long val_p0,
 #define OPC_FLTI	0x16
 #define OPC_FLTL	0x17
 #define OPC_MISC	0x18
-#define	OPC_JSR		0x1a
+#define OPC_JSR		0x1a
 
 #define FOP_SRC_S	0
 #define FOP_SRC_T	2
@@ -295,9 +295,9 @@ void cleanup_module(void)
 	sw64_fp_emul = save_emul;
 }
 
-#undef  sw64_fp_emul_imprecise
+#undef sw64_fp_emul_imprecise
 #define sw64_fp_emul_imprecise		do_sw_fp_emul_imprecise
-#undef  sw64_fp_emul
+#undef sw64_fp_emul
 #define sw64_fp_emul			do_sw_fp_emul
 
 #endif /* MODULE */
diff --git a/arch/sw_64/math-emu/sfp-util.h b/arch/sw_64/math-emu/sfp-util.h
index 63f9685999f3df9cd34edc1e0a300d74b4fec494..0769c0223e0d7e43e0b73fbd45882333e95f765f 100644
--- a/arch/sw_64/math-emu/sfp-util.h
+++ b/arch/sw_64/math-emu/sfp-util.h
@@ -1,4 +1,7 @@
 /* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _SW64_MATH_EMU_SFP_UTIL_H
+#define _SW64_MATH_EMU_SFP_UTIL_H
+
 #include <linux/kernel.h>
 #include <linux/sched.h>
 #include <linux/types.h>
@@ -34,3 +37,5 @@ extern unsigned long __udiv_qrnnd(unsigned long *, unsigned long,
 #define __LITTLE_ENDIAN -1
 #endif
 #define __BYTE_ORDER __LITTLE_ENDIAN
+
+#endif /* _SW64_MATH_EMU_SFP_UTIL_H */
diff --git a/arch/sw_64/mm/fault.c b/arch/sw_64/mm/fault.c
index 541e30b4b84ccbcdf234e3e853d928ef1b50b5f8..574fe7930aacd97d3830d6e3347770f9dab4eac6 100644
--- a/arch/sw_64/mm/fault.c
+++ b/arch/sw_64/mm/fault.c
@@ -61,24 +61,6 @@ void show_all_vma(void)
 	}
 }
 
-/*
- * Force a new ASN for a task.
- */
-void __load_new_mm_context(struct mm_struct *next_mm)
-{
-	unsigned long mmc;
-	struct pcb_struct *pcb;
-
-	mmc = __get_new_mm_context(next_mm, smp_processor_id());
-	next_mm->context.asid[smp_processor_id()] = mmc;
-
-	pcb = &current_thread_info()->pcb;
-	pcb->asn = mmc & HARDWARE_ASN_MASK;
-	pcb->ptbr = virt_to_pfn(next_mm->pgd);
-
-	__reload_thread(pcb);
-}
-
 /*
  * This routine handles page faults.  It determines the address,
  * and the problem, and then passes it off to handle_mm_fault().
diff --git a/arch/sw_64/mm/init.c b/arch/sw_64/mm/init.c
index e0096a0b432a4ab8cdfd49e30b9d8f869b213e72..93ec3ecdf4f1a6593dd22fab13cb3ef674a47b54 100644
--- a/arch/sw_64/mm/init.c
+++ b/arch/sw_64/mm/init.c
@@ -77,34 +77,14 @@ pgd_alloc(struct mm_struct *mm)
 	return ret;
 }
 
-static inline unsigned long
-load_PCB(struct pcb_struct *pcb)
-{
-	register unsigned long sp __asm__("$30");
-	pcb->ksp = sp;
-	return __reload_thread(pcb);
-}
-
 /* Set up initial PCB, VPTB, and other such nicities.  */
 
 static inline void
 switch_to_system_map(void)
 {
-	unsigned long newptbr;
-	unsigned long original_pcb_ptr;
-
-	/*
-	 * Initialize the kernel's page tables.  Linux puts the vptb in
-	 * the last slot of the L1 page table.
-	 */
 	memset(swapper_pg_dir, 0, PAGE_SIZE);
-	newptbr = virt_to_pfn(swapper_pg_dir);
-
-	/* Also set up the real kernel PCB while we're at it.  */
-	init_thread_info.pcb.ptbr = newptbr;
-	init_thread_info.pcb.flags = 1;	/* set FEN, clear everything else */
-	original_pcb_ptr = load_PCB(&init_thread_info.pcb);
-	tbia();
+	wrptbr(virt_to_phys(swapper_pg_dir));
+	tbiv();
 }
 
 void __init callback_init(void)
diff --git a/arch/sw_64/net/bpf_jit.h b/arch/sw_64/net/bpf_jit.h
index 2bf3ca6f3abdb1db1d55ad846a91eba25133061f..929036d8ea6b10daec13166c1e87f63165d99f1a 100644
--- a/arch/sw_64/net/bpf_jit.h
+++ b/arch/sw_64/net/bpf_jit.h
@@ -18,83 +18,85 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef _SW64_BPF_JIT_H
-#define _SW64_BPF_JIT_H
+#ifndef _SW64_NET_BPF_JIT_H
+#define _SW64_NET_BPF_JIT_H
 
+/* SW64 instruction field shift */
 #define SW64_BPF_OPCODE_OFFSET		26
 #define SW64_BPF_RA_OFFSET		21
 #define SW64_BPF_RB_OFFSET		16
 #define SW64_BPF_SIMPLE_ALU_IMM_OFFSET	13
 #define SW64_BPF_SIMPLE_ALU_FUNC_OFFSET	5
 #define SW64_BPF_SIMPLE_ALU_RC_OFFSET	0
+#define SW64_BPF_LS_FUNC_OFFSET		12
 
-#define SW64_BPF_OPCODE_BR_CALL		0x01
-#define SW64_BPF_OPCODE_BR_RET		0x02
-#define SW64_BPF_OPCODE_BR_JMP		0x03
-#define SW64_BPF_OPCODE_BR_BR		0x04
-#define SW64_BPF_OPCODE_BR_BSR		0x05
-#define SW64_BPF_OPCODE_BR_BEQ		0x30
-#define SW64_BPF_OPCODE_BR_BNE		0x31
-#define SW64_BPF_OPCODE_BR_BLT		0x32
-#define SW64_BPF_OPCODE_BR_BLE		0x33
-#define SW64_BPF_OPCODE_BR_BGT		0x34
-#define SW64_BPF_OPCODE_BR_BGE		0x35
-#define SW64_BPF_OPCODE_BR_BLBC		0x36
-#define SW64_BPF_OPCODE_BR_BLBS		0x37
-
-#define SW64_BPF_OPCODE_LS_LDBU		0x20
-#define SW64_BPF_OPCODE_LS_LDHU		0x21
-#define SW64_BPF_OPCODE_LS_LDW		0x22
-#define SW64_BPF_OPCODE_LS_LDL		0x23
-#define SW64_BPF_OPCODE_LS_STB		0x28
-#define SW64_BPF_OPCODE_LS_STH		0x29
-#define SW64_BPF_OPCODE_LS_STW		0x2A
-#define SW64_BPF_OPCODE_LS_STL		0x2B
-#define SW64_BPF_OPCODE_LS_LDI		0x3E
-#define SW64_BPF_OPCODE_LS_LDIH		0x3F
-
+/* SW64 instruction opcodes */
+#define SW64_BPF_OPCODE_CALL		0x01
+#define SW64_BPF_OPCODE_RET		0x02
+#define SW64_BPF_OPCODE_JMP		0x03
+#define SW64_BPF_OPCODE_BR		0x04
+#define SW64_BPF_OPCODE_BSR		0x05
+#define SW64_BPF_OPCODE_MISC		0x06
+#define SW64_BPF_OPCODE_LOCK		0x08
 #define SW64_BPF_OPCODE_ALU_REG		0x10
 #define SW64_BPF_OPCODE_ALU_IMM		0x12
+#define SW64_BPF_OPCODE_LDBU		0x20
+#define SW64_BPF_OPCODE_LDHU		0x21
+#define SW64_BPF_OPCODE_LDW		0x22
+#define SW64_BPF_OPCODE_LDL		0x23
+#define SW64_BPF_OPCODE_STB		0x28
+#define SW64_BPF_OPCODE_STH		0x29
+#define SW64_BPF_OPCODE_STW		0x2A
+#define SW64_BPF_OPCODE_STL		0x2B
+#define SW64_BPF_OPCODE_BEQ		0x30
+#define SW64_BPF_OPCODE_BNE		0x31
+#define SW64_BPF_OPCODE_BLT		0x32
+#define SW64_BPF_OPCODE_BLE		0x33
+#define SW64_BPF_OPCODE_BGT		0x34
+#define SW64_BPF_OPCODE_BGE		0x35
+#define SW64_BPF_OPCODE_BLBC		0x36
+#define SW64_BPF_OPCODE_BLBS		0x37
+#define SW64_BPF_OPCODE_LDI		0x3E
+#define SW64_BPF_OPCODE_LDIH		0x3F
+
+/* SW64 MISC instructions function codes */
+#define SW64_BPF_FUNC_MISC_RD_F		0x1000
+#define SW64_BPF_FUNC_MISC_WR_F		0x1020
 
+/* SW64 LOCK instructions function codes */
+#define SW64_BPF_FUNC_LOCK_LLDW		0x0
+#define SW64_BPF_FUNC_LOCK_LLDL		0x1
+#define SW64_BPF_FUNC_LOCK_LSTW		0x8
+#define SW64_BPF_FUNC_LOCK_LSTL		0x9
+
+/* SW64 ALU instructions function codes */
 #define SW64_BPF_FUNC_ALU_ADDW		0x00
 #define SW64_BPF_FUNC_ALU_SUBW		0x01
 #define SW64_BPF_FUNC_ALU_ADDL		0x08
 #define SW64_BPF_FUNC_ALU_SUBL		0x09
 #define SW64_BPF_FUNC_ALU_MULW		0x10
 #define SW64_BPF_FUNC_ALU_MULL		0x18
+#define SW64_BPF_FUNC_ALU_CMPEQ		0x28
+#define SW64_BPF_FUNC_ALU_CMPLT		0x29
+#define SW64_BPF_FUNC_ALU_CMPLE		0x2A
+#define SW64_BPF_FUNC_ALU_CMPULT	0x2B
+#define SW64_BPF_FUNC_ALU_CMPULE	0x2C
+#define SW64_BPF_FUNC_ALU_AND		0x38
+#define SW64_BPF_FUNC_ALU_BIC		0x39
+#define SW64_BPF_FUNC_ALU_BIS		0x3A
+#define SW64_BPF_FUNC_ALU_ORNOT		0x3B
+#define SW64_BPF_FUNC_ALU_XOR		0x3C
+#define SW64_BPF_FUNC_ALU_EQV		0x3D
+#define SW64_BPF_FUNC_ALU_SLL		0x48
+#define SW64_BPF_FUNC_ALU_SRL		0x49
+#define SW64_BPF_FUNC_ALU_SRA		0x4A
 #define SW64_BPF_FUNC_ALU_ZAP		0x68
 #define SW64_BPF_FUNC_ALU_ZAPNOT	0x69
 #define SW64_BPF_FUNC_ALU_SEXTB		0x6A
 #define SW64_BPF_FUNC_ALU_SEXTH		0x6B
 
-#define SW64_BPF_OPCODE_BS_REG		0x10
-#define SW64_BPF_OPCODE_BS_IMM		0x12
-
-#define SW64_BPF_FUNC_BS_SLL		0x48
-#define SW64_BPF_FUNC_BS_SRL		0x49
-#define SW64_BPF_FUNC_BS_SRA		0x4A
-
-#define SW64_BPF_OPCODE_LOGIC_REG	0x10
-#define SW64_BPF_OPCODE_LOGIC_IMM	0x12
-
-#define SW64_BPF_FUNC_LOGIC_AND		0x38
-#define SW64_BPF_FUNC_LOGIC_BIC		0x39
-#define SW64_BPF_FUNC_LOGIC_BIS		0x3A
-#define SW64_BPF_FUNC_LOGIC_ORNOT	0x3B
-#define SW64_BPF_FUNC_LOGIC_XOR		0x3C
-#define SW64_BPF_FUNC_LOGIC_EQV		0x3D
-
-#define SW64_BPF_OPCODE_CMP_REG		0x10
-#define SW64_BPF_OPCODE_CMP_IMM		0x12
-
-#define SW64_BPF_FUNC_CMP_EQ		0x28
-#define SW64_BPF_FUNC_CMP_LT		0x29
-#define SW64_BPF_FUNC_CMP_LE		0x2A
-#define SW64_BPF_FUNC_CMP_ULT		0x2B
-#define SW64_BPF_FUNC_CMP_ULE		0x2C
-
 /* special instuction used in jit_fill_hole() */
-#define SW64_BPF_ILLEGAL_INSN	((1 << 25) | 0x80)
+#define SW64_BPF_ILLEGAL_INSN	(0x1ff00000)	/* pri_ret/b	$31 */
 
 enum sw64_bpf_registers {
 	SW64_BPF_REG_V0		= 0,	/* keep return value */
@@ -135,25 +137,45 @@ enum sw64_bpf_registers {
 
 /* SW64 load and store instructions */
 #define SW64_BPF_LDBU(dst, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_LDBU, dst, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LDBU, dst, rb, offset16)
 #define SW64_BPF_LDHU(dst, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_LDHU, dst, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LDHU, dst, rb, offset16)
 #define SW64_BPF_LDW(dst, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_LDW, dst, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LDW, dst, rb, offset16)
 #define SW64_BPF_LDL(dst, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_LDL, dst, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LDL, dst, rb, offset16)
 #define SW64_BPF_STB(src, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_STB, src, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_STB, src, rb, offset16)
 #define SW64_BPF_STH(src, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_STH, src, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_STH, src, rb, offset16)
 #define SW64_BPF_STW(src, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_STW, src, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_STW, src, rb, offset16)
 #define SW64_BPF_STL(src, rb, offset16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_STL, src, rb, offset16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_STL, src, rb, offset16)
 #define SW64_BPF_LDI(dst, rb, imm16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_LDI, dst, rb, imm16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LDI, dst, rb, imm16)
 #define SW64_BPF_LDIH(dst, rb, imm16) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LS_LDIH, dst, rb, imm16)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_LDIH, dst, rb, imm16)
+
+/* SW64 lock instructions */
+#define SW64_BPF_LLDW(ra, rb, offset16) \
+	sw64_bpf_gen_format_ls_func(SW64_BPF_OPCODE_LOCK, \
+			ra, rb, offset16, SW64_BPF_FUNC_LOCK_LLDW)
+#define SW64_BPF_LLDL(ra, rb, offset16) \
+	sw64_bpf_gen_format_ls_func(SW64_BPF_OPCODE_LOCK, \
+			ra, rb, offset16, SW64_BPF_FUNC_LOCK_LLDL)
+#define SW64_BPF_LSTW(ra, rb, offset16) \
+	sw64_bpf_gen_format_ls_func(SW64_BPF_OPCODE_LOCK, \
+			ra, rb, offset16, SW64_BPF_FUNC_LOCK_LSTW)
+#define SW64_BPF_LSTL(ra, rb, offset16) \
+	sw64_bpf_gen_format_ls_func(SW64_BPF_OPCODE_LOCK, \
+			ra, rb, offset16, SW64_BPF_FUNC_LOCK_LSTL)
+#define SW64_BPF_RD_F(ra) \
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_MISC, \
+			ra, SW64_BPF_REG_ZR, SW64_BPF_FUNC_MISC_RD_F)
+#define SW64_BPF_WR_F(ra) \
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_MISC, \
+			ra, SW64_BPF_REG_ZR, SW64_BPF_FUNC_MISC_WR_F)
 
 /* SW64 ALU instructions REG format */
 #define SW64_BPF_ADDW_REG(ra, rb, dst) \
@@ -182,10 +204,10 @@ enum sw64_bpf_registers {
 			ra, rb, dst, SW64_BPF_FUNC_ALU_ZAPNOT)
 #define SW64_BPF_SEXTB_REG(rb, dst) \
 	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
-			0, rb, dst, SW64_BPF_FUNC_ALU_SEXTB)
+			SW64_BPF_REG_ZR, rb, dst, SW64_BPF_FUNC_ALU_SEXTB)
 #define SW64_BPF_SEXTH_REG(rb, dst) \
 	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
-			0, rb, dst, SW64_BPF_FUNC_ALU_SEXTH)
+			SW64_BPF_REG_ZR, rb, dst, SW64_BPF_FUNC_ALU_SEXTH)
 
 /* SW64 ALU instructions IMM format */
 #define SW64_BPF_ADDW_IMM(ra, imm8, dst) \
@@ -214,130 +236,133 @@ enum sw64_bpf_registers {
 			ra, imm8, dst, SW64_BPF_FUNC_ALU_ZAPNOT)
 #define SW64_BPF_SEXTB_IMM(imm8, dst) \
 	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
-			0, imm8, dst, SW64_BPF_FUNC_ALU_SEXTB)
+			SW64_BPF_REG_ZR, imm8, dst, SW64_BPF_FUNC_ALU_SEXTB)
+#define SW64_BPF_SEXTH_IMM(imm8, dst) \
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			SW64_BPF_REG_ZR, imm8, dst, SW64_BPF_FUNC_ALU_SEXTH)
 
 /* SW64 bit shift instructions REG format */
 #define SW64_BPF_SLL_REG(src, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_BS_REG, \
-			src, rb, dst, SW64_BPF_FUNC_BS_SLL)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			src, rb, dst, SW64_BPF_FUNC_ALU_SLL)
 #define SW64_BPF_SRL_REG(src, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_BS_REG, \
-			src, rb, dst, SW64_BPF_FUNC_BS_SRL)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			src, rb, dst, SW64_BPF_FUNC_ALU_SRL)
 #define SW64_BPF_SRA_REG(src, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_BS_REG, \
-			src, rb, dst, SW64_BPF_FUNC_BS_SRA)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			src, rb, dst, SW64_BPF_FUNC_ALU_SRA)
 
 /* SW64 bit shift instructions IMM format */
 #define SW64_BPF_SLL_IMM(src, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_BS_IMM, \
-			src, imm8, dst, SW64_BPF_FUNC_BS_SLL)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			src, imm8, dst, SW64_BPF_FUNC_ALU_SLL)
 #define SW64_BPF_SRL_IMM(src, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_BS_IMM, \
-			src, imm8, dst, SW64_BPF_FUNC_BS_SRL)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			src, imm8, dst, SW64_BPF_FUNC_ALU_SRL)
 #define SW64_BPF_SRA_IMM(src, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_BS_IMM, \
-			src, imm8, dst, SW64_BPF_FUNC_BS_SRA)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			src, imm8, dst, SW64_BPF_FUNC_ALU_SRA)
 
 /* SW64 control instructions */
 #define SW64_BPF_CALL(ra, rb) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_BR_CALL, ra, rb, 0)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_CALL, ra, rb, 0)
 #define SW64_BPF_RET(rb) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_BR_RET, SW64_BPF_REG_ZR, rb, 0)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_RET, SW64_BPF_REG_ZR, rb, 0)
 #define SW64_BPF_JMP(ra, rb) \
-	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_BR_JMP, ra, rb, 0)
+	sw64_bpf_gen_format_ls(SW64_BPF_OPCODE_JMP, ra, rb, 0)
 #define SW64_BPF_BR(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BR, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR, ra, offset)
 #define SW64_BPF_BSR(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BSR, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BSR, ra, offset)
 #define SW64_BPF_BEQ(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BEQ, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BEQ, ra, offset)
 #define SW64_BPF_BNE(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BNE, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BNE, ra, offset)
 #define SW64_BPF_BLT(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BLT, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BLT, ra, offset)
 #define SW64_BPF_BLE(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BLE, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BLE, ra, offset)
 #define SW64_BPF_BGT(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BGT, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BGT, ra, offset)
 #define SW64_BPF_BGE(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BGE, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BGE, ra, offset)
 #define SW64_BPF_BLBC(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BLBC, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BLBC, ra, offset)
 #define SW64_BPF_BLBS(ra, offset) \
-	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BR_BLBS, ra, offset)
+	sw64_bpf_gen_format_br(SW64_BPF_OPCODE_BLBS, ra, offset)
 
 /* SW64 bit logic instructions REG format */
 #define SW64_BPF_AND_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_LOGIC_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_LOGIC_AND)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_AND)
 #define SW64_BPF_ANDNOT_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_LOGIC_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_LOGIC_BIC)
-#define SW64_BPF_OR_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_LOGIC_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_LOGIC_BIS)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_BIC)
+#define SW64_BPF_BIS_REG(ra, rb, dst) \
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_BIS)
 #define SW64_BPF_ORNOT_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_LOGIC_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_LOGIC_ORNOT)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_ORNOT)
 #define SW64_BPF_XOR_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_LOGIC_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_LOGIC_XOR)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_XOR)
 #define SW64_BPF_EQV_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_LOGIC_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_LOGIC_EQV)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_EQV)
 
 /* SW64 bit logic instructions IMM format */
 #define SW64_BPF_AND_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_LOGIC_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_LOGIC_AND)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_AND)
 #define SW64_BPF_ANDNOT_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_LOGIC_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_LOGIC_BIC)
-#define SW64_BPF_OR_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_LOGIC_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_LOGIC_BIS)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_BIC)
+#define SW64_BPF_BIS_IMM(ra, imm8, dst) \
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_BIS)
 #define SW64_BPF_ORNOT_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_LOGIC_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_LOGIC_ORNOT)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_ORNOT)
 #define SW64_BPF_XOR_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_LOGIC_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_LOGIC_XOR)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_XOR)
 #define SW64_BPF_EQV_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_LOGIC_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_LOGIC_EQV)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_EQV)
 
 /* SW64 compare instructions REG format */
 #define SW64_BPF_CMPEQ_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_CMP_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_CMP_EQ)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_CMPEQ)
 #define SW64_BPF_CMPLT_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_CMP_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_CMP_LT)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_CMPLT)
 #define SW64_BPF_CMPLE_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_CMP_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_CMP_LE)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_CMPLE)
 #define SW64_BPF_CMPULT_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_CMP_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_CMP_ULT)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_CMPULT)
 #define SW64_BPF_CMPULE_REG(ra, rb, dst) \
-	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_CMP_REG, \
-			ra, rb, dst, SW64_BPF_FUNC_CMP_ULE)
+	sw64_bpf_gen_format_simple_alu_reg(SW64_BPF_OPCODE_ALU_REG, \
+			ra, rb, dst, SW64_BPF_FUNC_ALU_CMPULE)
 
 /* SW64 compare instructions imm format */
 #define SW64_BPF_CMPEQ_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_CMP_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_CMP_EQ)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_CMPEQ)
 #define SW64_BPF_CMPLT_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_CMP_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_CMP_LT)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_CMPLT)
 #define SW64_BPF_CMPLE_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_CMP_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_CMP_LE)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_CMPLE)
 #define SW64_BPF_CMPULT_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_CMP_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_CMP_ULT)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_CMPULT)
 #define SW64_BPF_CMPULE_IMM(ra, imm8, dst) \
-	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_CMP_IMM, \
-			ra, imm8, dst, SW64_BPF_FUNC_CMP_ULE)
+	sw64_bpf_gen_format_simple_alu_imm(SW64_BPF_OPCODE_ALU_IMM, \
+			ra, imm8, dst, SW64_BPF_FUNC_ALU_CMPULE)
 
-#endif /* _SW64_BPF_JIT_H */
+#endif /* _SW64_NET_BPF_JIT_H */
diff --git a/arch/sw_64/net/bpf_jit_comp.c b/arch/sw_64/net/bpf_jit_comp.c
index 102de82d69e167d8e4ef2a6519743718befd3ce0..2c238c33e5740dcf4241de568644f7826c2a182b 100644
--- a/arch/sw_64/net/bpf_jit_comp.c
+++ b/arch/sw_64/net/bpf_jit_comp.c
@@ -29,46 +29,37 @@
 
 #include "bpf_jit.h"
 
-#define TMP_REG_1 (MAX_BPF_JIT_REG + 0)
-#define TMP_REG_2 (MAX_BPF_JIT_REG + 1)
-#define TCALL_CNT (MAX_BPF_JIT_REG + 2)
-
-/*
- * TO-DO List:
- *	DIV
- *	MOD
- */
+#define TCALL_CNT (MAX_BPF_JIT_REG + 0)
 
 static const int bpf2sw64[] = {
 	/* return value from in-kernel function, and exit value from eBPF */
 	[BPF_REG_0] = SW64_BPF_REG_V0,
 	/* arguments from eBPF program to in-kernel function */
-	[BPF_REG_1] = SW64_BPF_REG_A1,
-	[BPF_REG_2] = SW64_BPF_REG_A2,
-	[BPF_REG_3] = SW64_BPF_REG_A3,
-	[BPF_REG_4] = SW64_BPF_REG_A4,
-	[BPF_REG_5] = SW64_BPF_REG_A5,
+	[BPF_REG_1] = SW64_BPF_REG_A0,
+	[BPF_REG_2] = SW64_BPF_REG_A1,
+	[BPF_REG_3] = SW64_BPF_REG_A2,
+	[BPF_REG_4] = SW64_BPF_REG_A3,
+	[BPF_REG_5] = SW64_BPF_REG_A4,
 	/* callee saved registers that in-kernel function will preserve */
-	[BPF_REG_6] = SW64_BPF_REG_S1,
-	[BPF_REG_7] = SW64_BPF_REG_S2,
-	[BPF_REG_8] = SW64_BPF_REG_S3,
-	[BPF_REG_9] = SW64_BPF_REG_S4,
+	[BPF_REG_6] = SW64_BPF_REG_S0,
+	[BPF_REG_7] = SW64_BPF_REG_S1,
+	[BPF_REG_8] = SW64_BPF_REG_S2,
+	[BPF_REG_9] = SW64_BPF_REG_S3,
 	/* read-only frame pointer to access stack */
-	[BPF_REG_FP] = SW64_BPF_REG_S0,
-	/* temporary registers for internal BPF JIT */
-	[TMP_REG_1] = SW64_BPF_REG_T1,
-	[TMP_REG_2] = SW64_BPF_REG_T2,
+	[BPF_REG_FP] = SW64_BPF_REG_FP,
 	/* tail_call_cnt */
-	[TCALL_CNT] = SW64_BPF_REG_S5,
+	[TCALL_CNT] = SW64_BPF_REG_S4,
 	/* temporary register for blinding constants */
-	[BPF_REG_AX] = SW64_BPF_REG_T12,
+	[BPF_REG_AX] = SW64_BPF_REG_T11,
 };
 
 struct jit_ctx {
 	const struct bpf_prog *prog;
 	int idx;		// JITed instruction index
+	int current_tmp_reg;
 	int epilogue_offset;
 	int *insn_offset;	// [bpf_insn_idx] = jited_insn_idx
+	int exentry_idx;
 	u32 *image;		// JITed instruction
 	u32 stack_size;
 };
@@ -83,7 +74,7 @@ static inline u32 sw64_bpf_gen_format_br(int opcode, enum sw64_bpf_registers ra,
 {
 	opcode = opcode << SW64_BPF_OPCODE_OFFSET;
 	ra = ra << SW64_BPF_RA_OFFSET;
-	return opcode | ra | disp;
+	return opcode | ra | (disp & 0x1fffff);
 }
 
 static inline u32 sw64_bpf_gen_format_ls(int opcode, enum sw64_bpf_registers ra,
@@ -92,7 +83,17 @@ static inline u32 sw64_bpf_gen_format_ls(int opcode, enum sw64_bpf_registers ra,
 	opcode = opcode << SW64_BPF_OPCODE_OFFSET;
 	ra = ra << SW64_BPF_RA_OFFSET;
 	rb = rb << SW64_BPF_RB_OFFSET;
-	return opcode | ra | rb | disp;
+	return opcode | ra | rb | (disp & 0xffff);
+}
+
+static inline u32 sw64_bpf_gen_format_ls_func(int opcode, enum sw64_bpf_registers ra,
+		enum sw64_bpf_registers rb, u16 disp, int function)
+{
+	opcode = opcode << SW64_BPF_OPCODE_OFFSET;
+	ra = ra << SW64_BPF_RA_OFFSET;
+	rb = rb << SW64_BPF_RB_OFFSET;
+	function = function << SW64_BPF_LS_FUNC_OFFSET;
+	return opcode | ra | rb | function | (disp & 0xfff);
 }
 
 static inline u32 sw64_bpf_gen_format_simple_alu_reg(int opcode, enum sw64_bpf_registers ra,
@@ -107,12 +108,12 @@ static inline u32 sw64_bpf_gen_format_simple_alu_reg(int opcode, enum sw64_bpf_r
 }
 
 static inline u32 sw64_bpf_gen_format_simple_alu_imm(int opcode, enum sw64_bpf_registers ra,
-		enum sw64_bpf_registers rc, u8 imm, int function)
+		u32 imm, enum sw64_bpf_registers rc, int function)
 {
 	opcode = opcode << SW64_BPF_OPCODE_OFFSET;
 	ra = ra << SW64_BPF_RA_OFFSET;
+	imm = (imm & 0xff) << SW64_BPF_SIMPLE_ALU_IMM_OFFSET;
 	rc = rc << SW64_BPF_SIMPLE_ALU_RC_OFFSET;
-	imm = imm << SW64_BPF_SIMPLE_ALU_IMM_OFFSET;
 	function = function << SW64_BPF_SIMPLE_ALU_FUNC_OFFSET;
 	return opcode | ra | imm | function | rc;
 }
@@ -125,57 +126,85 @@ static inline void emit(const u32 insn, struct jit_ctx *ctx)
 	ctx->idx++;
 }
 
-static inline void emit_sw64_ldu64(const int dst, const u64 imm64, struct jit_ctx *ctx)
+static inline int get_tmp_reg(struct jit_ctx *ctx)
 {
-	u16 imm_tmp;
-	int reg_tmp = SW64_BPF_REG_T8;
-
-	imm_tmp = (imm64 >> 60) & 0xf;
-	emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm_tmp), ctx);
-	emit(SW64_BPF_SLL_IMM(dst, 60, dst), ctx);
-
-	imm_tmp = (imm64 >> 45) & 0x7fff;
-	emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
-	emit(SW64_BPF_SLL_IMM(reg_tmp, 45, reg_tmp), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
-
-	imm_tmp = (imm64 >> 30) & 0x7fff;
-	emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
-	emit(SW64_BPF_SLL_IMM(reg_tmp, 30, reg_tmp), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
-
-	imm_tmp = (imm64 >> 15) & 0x7fff;
-	emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
-	emit(SW64_BPF_SLL_IMM(reg_tmp, 15, reg_tmp), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
+	ctx->current_tmp_reg++;
+	/* Do not use 22-25. Should be more than enough. */
+	if (unlikely(ctx->current_tmp_reg == 8)) {
+		pr_err("eBPF JIT %s[%d]: not enough temporary registers!\n",
+				current->comm, current->pid);
+		return -1;
+	}
+	return ctx->current_tmp_reg;
+}
 
-	imm_tmp = imm64 & 0x7fff;
-	emit(SW64_BPF_LDI(dst, dst, imm_tmp), ctx);
+static inline void put_tmp_reg(struct jit_ctx *ctx)
+{
+	ctx->current_tmp_reg--;
+	if (ctx->current_tmp_reg == 21)
+		ctx->current_tmp_reg = 7;
 }
 
-static inline void emit_sw64_ldu32(const int dst, const u32 imm32, struct jit_ctx *ctx)
+static void emit_sw64_ldu32(const int dst, const u32 imm, struct jit_ctx *ctx)
 {
 	u16 imm_tmp;
-	int reg_tmp = SW64_BPF_REG_T8;
+	u8 reg_tmp = get_tmp_reg(ctx);
+
+	if (!imm) {
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_ZR, dst), ctx);
+		put_tmp_reg(ctx);
+		return;
+	}
+
+	if (imm <= S16_MAX) {
+		emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm), ctx);
+		put_tmp_reg(ctx);
+		return;
+	}
 
-	imm_tmp = (imm32 >> 30) & 3;
+	if (imm >= U32_MAX - S16_MAX) {
+		emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		put_tmp_reg(ctx);
+		return;
+	}
+
+	imm_tmp = (imm >> 30) & 3;
 	emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm_tmp), ctx);
-	emit(SW64_BPF_SLL_IMM(dst, 30, dst), ctx);
+	if (imm_tmp)
+		emit(SW64_BPF_SLL_IMM(dst, 30, dst), ctx);
 
-	imm_tmp = (imm32 >> 15) & 0x7fff;
-	emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
-	emit(SW64_BPF_SLL_IMM(reg_tmp, 15, reg_tmp), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
+	imm_tmp = (imm >> 15) & 0x7fff;
+	if (imm_tmp) {
+		emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
+		emit(SW64_BPF_SLL_IMM(reg_tmp, 15, reg_tmp), ctx);
+		emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
+	}
+
+	imm_tmp = imm & 0x7fff;
+	if (imm_tmp)
+		emit(SW64_BPF_LDI(dst, dst, imm_tmp), ctx);
 
-	imm_tmp = imm32 & 0x7fff;
-	emit(SW64_BPF_LDI(dst, dst, imm_tmp), ctx);
+	put_tmp_reg(ctx);
 }
 
-static inline void emit_sw64_lds32(const int dst, const s32 imm32, struct jit_ctx *ctx)
+static void emit_sw64_lds32(const int dst, const s32 imm, struct jit_ctx *ctx)
 {
-	s16 hi = imm32 >> 16;
-	s16 lo = imm32 & 0xffff;
-	int reg_tmp = SW64_BPF_REG_T8;
+	s16 hi = imm >> 16;
+	s16 lo = imm & 0xffff;
+	u8 reg_tmp = get_tmp_reg(ctx);
+
+	if (!imm) {
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_ZR, dst), ctx);
+		put_tmp_reg(ctx);
+		return;
+	}
+
+	if (imm >= S16_MIN && imm <= S16_MAX) {
+		emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm), ctx);
+		put_tmp_reg(ctx);
+		return;
+	}
 
 	emit(SW64_BPF_LDIH(dst, SW64_BPF_REG_ZR, hi), ctx);
 	if (lo & 0x8000) {	// sign bit is 1
@@ -183,214 +212,422 @@ static inline void emit_sw64_lds32(const int dst, const s32 imm32, struct jit_ct
 		emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, 1), ctx);
 		emit(SW64_BPF_SLL_IMM(reg_tmp, 15, reg_tmp), ctx);
 		emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
-		emit(SW64_BPF_LDI(dst, dst, lo), ctx);
+		if (lo)
+			emit(SW64_BPF_LDI(dst, dst, lo), ctx);
 	} else {	// sign bit is 0
-		emit(SW64_BPF_LDI(dst, dst, lo), ctx);
+		if (lo)
+			emit(SW64_BPF_LDI(dst, dst, lo), ctx);
 	}
+
+	put_tmp_reg(ctx);
 }
 
-/* dst = ra / rb */
-static void emit_sw64_div(const int ra, const int rb, const int dst, struct jit_ctx *ctx)
+static void emit_sw64_ldu64(const int dst, const u64 imm, struct jit_ctx *ctx)
 {
-	pr_err("DIV is not supported for now.\n");
+	u16 imm_tmp;
+	u8 reg_tmp = get_tmp_reg(ctx);
+
+	if (!imm) {
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_ZR, dst), ctx);
+		put_tmp_reg(ctx);
+		return;
+	}
+
+	if (imm <= U32_MAX) {
+		put_tmp_reg(ctx);
+		return emit_sw64_ldu32(dst, (u32)imm, ctx);
+	}
+
+	if (imm >= (U64_MAX - S16_MAX) || imm <= S16_MAX) {
+		emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm), ctx);
+		put_tmp_reg(ctx);
+		return;
+	}
+
+	imm_tmp = (imm >> 60) & 0xf;
+	emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm_tmp), ctx);
+	if (imm_tmp)
+		emit(SW64_BPF_SLL_IMM(dst, 60, dst), ctx);
+
+	imm_tmp = (imm >> 45) & 0x7fff;
+	if (imm_tmp) {
+		emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
+		emit(SW64_BPF_SLL_IMM(reg_tmp, 45, reg_tmp), ctx);
+		emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
+	}
+
+	imm_tmp = (imm >> 30) & 0x7fff;
+	if (imm_tmp) {
+		emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
+		emit(SW64_BPF_SLL_IMM(reg_tmp, 30, reg_tmp), ctx);
+		emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
+	}
+
+	imm_tmp = (imm >> 15) & 0x7fff;
+	if (imm_tmp) {
+		emit(SW64_BPF_LDI(reg_tmp, SW64_BPF_REG_ZR, imm_tmp), ctx);
+		emit(SW64_BPF_SLL_IMM(reg_tmp, 15, reg_tmp), ctx);
+		emit(SW64_BPF_ADDL_REG(dst, reg_tmp, dst), ctx);
+	}
+
+	imm_tmp = imm & 0x7fff;
+	if (imm_tmp)
+		emit(SW64_BPF_LDI(dst, dst, imm_tmp), ctx);
+
+	put_tmp_reg(ctx);
 }
 
-/* dst = ra % rb */
-static void emit_sw64_mod(const int ra, const int rb, const int dst, struct jit_ctx *ctx)
+/* Do not change!!! See arch/sw_64/lib/divide.S for more detail */
+#define REG(x)		"$"str(x)
+#define str(x)		#x
+#define DIVIDEND	24
+#define DIVISOR		25
+#define RESULT		27
+/* Make these functions noinline because we need their address at runtime */
+noinline void sw64_bpf_jit_helper_div32(void)
 {
-	pr_err("MOD is not supported for now.\n");
+	register u32 __dividend asm(REG(DIVIDEND));
+	register u32 __divisor asm(REG(DIVISOR));
+	u32 res = __dividend / __divisor;
+
+	asm volatile(
+	""
+	:: "r"(res));
+}
+
+noinline void sw64_bpf_jit_helper_mod32(void)
+{
+	register u32 __dividend asm(REG(DIVIDEND));
+	register u32 __divisor asm(REG(DIVISOR));
+	u32 res = __dividend % __divisor;
+
+	asm volatile(
+	""
+	:: "r"(res));
+}
+
+noinline void sw64_bpf_jit_helper_div64(void)
+{
+	register u64 __dividend asm(REG(DIVIDEND));
+	register u64 __divisor asm(REG(DIVISOR));
+	u64 res = __dividend / __divisor;
+
+	asm volatile(
+	""
+	:: "r"(res));
+}
+
+noinline void sw64_bpf_jit_helper_mod64(void)
+{
+	register u64 __dividend asm(REG(DIVIDEND));
+	register u64 __divisor asm(REG(DIVISOR));
+	u64 res = __dividend % __divisor;
+
+	asm volatile(
+	""
+	:: "r"(res));
+}
+
+static void emit_sw64_divmod(const int dst, const int src, struct jit_ctx *ctx, u8 code)
+{
+	emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, dst, DIVIDEND), ctx);
+	emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, src, DIVISOR), ctx);
+	switch (BPF_CLASS(code)) {
+	case BPF_ALU:
+		switch (BPF_OP(code)) {
+		case BPF_DIV:
+			emit_sw64_ldu64(SW64_BPF_REG_PV, (u64)sw64_bpf_jit_helper_div32, ctx);
+			break;
+		case BPF_MOD:
+			emit_sw64_ldu64(SW64_BPF_REG_PV, (u64)sw64_bpf_jit_helper_mod32, ctx);
+			break;
+		}
+		emit(SW64_BPF_CALL(SW64_BPF_REG_RA, SW64_BPF_REG_PV), ctx);
+		emit(SW64_BPF_ZAP_IMM(RESULT, 0xf0, dst), ctx);
+		break;
+	case BPF_ALU64:
+		switch (BPF_OP(code)) {
+		case BPF_DIV:
+			emit_sw64_ldu64(SW64_BPF_REG_PV, (u64)sw64_bpf_jit_helper_div64, ctx);
+			break;
+		case BPF_MOD:
+			emit_sw64_ldu64(SW64_BPF_REG_PV, (u64)sw64_bpf_jit_helper_mod64, ctx);
+			break;
+		}
+		emit(SW64_BPF_CALL(SW64_BPF_REG_RA, SW64_BPF_REG_PV), ctx);
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, RESULT, dst), ctx);
+		break;
+	}
+}
+
+#undef REG
+#undef str
+#undef DIVIDEND
+#undef DIVISOR
+#undef RESULT
+
+/* STX XADD: lock *(u32 *)(dst + off) += src */
+static void emit_sw64_xadd32(const int src, int dst, s16 off, struct jit_ctx *ctx)
+{
+	int atomic_start;
+	int atomic_end;
+	u8 tmp1 = get_tmp_reg(ctx);
+	u8 tmp2 = get_tmp_reg(ctx);
+	u8 tmp3 = get_tmp_reg(ctx);
+
+	if (off < -0x800 || off > 0x7ff) {
+		emit(SW64_BPF_LDI(tmp1, dst, off), ctx);
+		dst = tmp1;
+		off = 0;
+	}
+
+	atomic_start = ctx->idx;
+	emit(SW64_BPF_LLDW(tmp2, dst, off), ctx);
+	emit(SW64_BPF_LDI(tmp3, SW64_BPF_REG_ZR, 1), ctx);
+	emit(SW64_BPF_WR_F(tmp3), ctx);
+	emit(SW64_BPF_ADDW_REG(tmp2, src, tmp2), ctx);
+	if (ctx->idx & 1)
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_ZR, SW64_BPF_REG_ZR), ctx);
+	emit(SW64_BPF_LSTW(tmp2, dst, off), ctx);
+	emit(SW64_BPF_RD_F(tmp3), ctx);
+	atomic_end = ctx->idx;
+	emit(SW64_BPF_BEQ(tmp3, atomic_start - atomic_end - 1), ctx);
+
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
+}
+
+/* STX XADD: lock *(u64 *)(dst + off) += src */
+static void emit_sw64_xadd64(const int src, int dst, s16 off, struct jit_ctx *ctx)
+{
+	int atomic_start;
+	int atomic_end;
+	u8 tmp1 = get_tmp_reg(ctx);
+	u8 tmp2 = get_tmp_reg(ctx);
+	u8 tmp3 = get_tmp_reg(ctx);
+
+	if (off < -0x800 || off > 0x7ff) {
+		emit(SW64_BPF_LDI(tmp1, dst, off), ctx);
+		dst = tmp1;
+		off = 0;
+	}
+
+	atomic_start = ctx->idx;
+	emit(SW64_BPF_LLDL(tmp2, dst, off), ctx);
+	emit(SW64_BPF_LDI(tmp3, SW64_BPF_REG_ZR, 1), ctx);
+	emit(SW64_BPF_WR_F(tmp3), ctx);
+	emit(SW64_BPF_ADDL_REG(tmp2, src, tmp2), ctx);
+	if (ctx->idx & 1)
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_ZR, SW64_BPF_REG_ZR), ctx);
+	emit(SW64_BPF_LSTL(tmp2, dst, off), ctx);
+	emit(SW64_BPF_RD_F(tmp3), ctx);
+	atomic_end = ctx->idx;
+	emit(SW64_BPF_BEQ(tmp3, atomic_start - atomic_end - 1), ctx);
+
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
 }
 
 static void emit_sw64_htobe16(const int dst, struct jit_ctx *ctx)
 {
-	int tmp = SW64_BPF_REG_T8;
+	u8 tmp = get_tmp_reg(ctx);
 
-	emit(SW64_BPF_LDI(tmp, dst, 0), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp, 0x2, tmp), ctx);
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x2, tmp), ctx);
 	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x1, dst), ctx);
-	emit(SW64_BPF_SRL_REG(tmp, 8, tmp), ctx);
-	emit(SW64_BPF_SLL_REG(dst, 8, dst), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp, dst), ctx);
+	emit(SW64_BPF_SRL_IMM(tmp, 8, tmp), ctx);
+	emit(SW64_BPF_SLL_IMM(dst, 8, dst), ctx);
+	emit(SW64_BPF_BIS_REG(dst, tmp, dst), ctx);
+
+	put_tmp_reg(ctx);
 }
 
 static void emit_sw64_htobe32(const int dst, struct jit_ctx *ctx)
 {
-	int tmp1 = SW64_BPF_REG_T8;
-	int tmp2 = SW64_BPF_REG_T9;
+	u8 tmp1 = get_tmp_reg(ctx);
+	u8 tmp2 = get_tmp_reg(ctx);
 
-	emit(SW64_BPF_LDI(tmp1, dst, 0), ctx);
-	emit(SW64_BPF_LDI(tmp2, dst, 0), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp1, 0x1, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp2, 0x8, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x6, dst), ctx);
-	emit(SW64_BPF_SLL_IMM(tmp1, 24, tmp1), ctx);
-	emit(SW64_BPF_SRL_IMM(tmp2, 24, tmp2), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp2, dst), ctx);
-
-	emit(SW64_BPF_LDI(tmp1, dst, 0), ctx);
-	emit(SW64_BPF_LDI(tmp2, dst, 0), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp1, 0x2, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp2, 0x4, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x9, dst), ctx);
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x8, tmp1), ctx);
+	emit(SW64_BPF_SRL_IMM(tmp1, 24, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x4, tmp1), ctx);
+	emit(SW64_BPF_SRL_IMM(tmp1, 8, tmp1), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x2, tmp1), ctx);
 	emit(SW64_BPF_SLL_IMM(tmp1, 8, tmp1), ctx);
-	emit(SW64_BPF_SRL_IMM(tmp2, 8, tmp2), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp2, dst), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x1, dst), ctx);
+	emit(SW64_BPF_SLL_IMM(dst, 24, dst), ctx);
+	emit(SW64_BPF_BIS_REG(dst, tmp2, dst), ctx);
+
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
 }
 
 static void emit_sw64_htobe64(const int dst, struct jit_ctx *ctx)
 {
-	int tmp1 = SW64_BPF_REG_T8;
-	int tmp2 = SW64_BPF_REG_T9;
-
-	emit(SW64_BPF_LDI(tmp1, dst, 0), ctx);
-	emit(SW64_BPF_LDI(tmp2, dst, 0), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp1, 0x1, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp2, 0x80, tmp1), ctx);
-	emit(SW64_BPF_ZAP_IMM(dst, 0x81, dst), ctx);
-	emit(SW64_BPF_SLL_IMM(tmp1, 56, tmp1), ctx);
-	emit(SW64_BPF_SRL_IMM(tmp2, 56, tmp2), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp2, dst), ctx);
-
-	emit(SW64_BPF_LDI(tmp1, dst, 0), ctx);
-	emit(SW64_BPF_LDI(tmp2, dst, 0), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp1, 0x2, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp2, 0x40, tmp1), ctx);
-	emit(SW64_BPF_ZAP_IMM(dst, 0x42, dst), ctx);
-	emit(SW64_BPF_SLL_IMM(tmp1, 40, tmp1), ctx);
-	emit(SW64_BPF_SRL_IMM(tmp2, 40, tmp2), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp2, dst), ctx);
-
-	emit(SW64_BPF_LDI(tmp1, dst, 0), ctx);
-	emit(SW64_BPF_LDI(tmp2, dst, 0), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp1, 0x4, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp2, 0x20, tmp1), ctx);
-	emit(SW64_BPF_ZAP_IMM(dst, 0x24, dst), ctx);
-	emit(SW64_BPF_SLL_IMM(tmp1, 24, tmp1), ctx);
-	emit(SW64_BPF_SRL_IMM(tmp2, 24, tmp2), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp2, dst), ctx);
-
-	emit(SW64_BPF_LDI(tmp1, dst, 0), ctx);
-	emit(SW64_BPF_LDI(tmp2, dst, 0), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp1, 0x8, tmp1), ctx);
-	emit(SW64_BPF_ZAPNOT_IMM(tmp2, 0x10, tmp1), ctx);
-	emit(SW64_BPF_ZAP_IMM(dst, 0x18, dst), ctx);
+	u8 tmp1 = get_tmp_reg(ctx);
+	u8 tmp2 = get_tmp_reg(ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x80, tmp1), ctx);
+	emit(SW64_BPF_SRL_IMM(tmp1, 56, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x40, tmp1), ctx);
+	emit(SW64_BPF_SRL_IMM(tmp1, 40, tmp1), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x20, tmp1), ctx);
+	emit(SW64_BPF_SRL_IMM(tmp1, 24, tmp1), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x10, tmp1), ctx);
+	emit(SW64_BPF_SRL_IMM(tmp1, 8, tmp1), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x08, tmp1), ctx);
 	emit(SW64_BPF_SLL_IMM(tmp1, 8, tmp1), ctx);
-	emit(SW64_BPF_SRL_IMM(tmp2, 8, tmp2), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
-	emit(SW64_BPF_ADDL_REG(dst, tmp2, dst), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x04, tmp1), ctx);
+	emit(SW64_BPF_SLL_IMM(tmp1, 24, tmp1), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x02, tmp1), ctx);
+	emit(SW64_BPF_SLL_IMM(tmp1, 40, tmp1), ctx);
+	emit(SW64_BPF_BIS_REG(tmp2, tmp1, tmp2), ctx);
+
+	emit(SW64_BPF_ZAPNOT_IMM(dst, 0x01, dst), ctx);
+	emit(SW64_BPF_SLL_IMM(dst, 56, dst), ctx);
+	emit(SW64_BPF_BIS_REG(dst, tmp2, dst), ctx);
+
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
 }
 
 static void jit_fill_hole(void *area, unsigned int size)
 {
-	memset(area, SW64_BPF_ILLEGAL_INSN, size);
+	unsigned long c = SW64_BPF_ILLEGAL_INSN;
+
+	c |= c << 32;
+	__constant_c_memset(area, c, size);
+}
+
+static int offset_to_epilogue(const struct jit_ctx *ctx);
+static int bpf2sw64_offset(int bpf_idx, s32 off, const struct jit_ctx *ctx)
+{
+	int from = ctx->insn_offset[bpf_idx + 1];
+	int to = ctx->insn_offset[bpf_idx + 1 + off];
+
+	if (ctx->image == NULL)
+		return 0;
+
+	return to - from;
 }
 
 static int offset_to_epilogue(const struct jit_ctx *ctx)
 {
+	if (ctx->image == NULL)
+		return 0;
+
 	return ctx->epilogue_offset - ctx->idx;
 }
 
-/* For tail call to jump into */
-#define PROLOGUE_OFFSET 8
+/* For tail call, jump to set up function call stack */
+#define PROLOGUE_OFFSET	11
 
 static void build_prologue(struct jit_ctx *ctx, bool was_classic)
 {
-	const int r6 = bpf2sw64[BPF_REG_6];
-	const int r7 = bpf2sw64[BPF_REG_7];
-	const int r8 = bpf2sw64[BPF_REG_8];
-	const int r9 = bpf2sw64[BPF_REG_9];
-	const int fp = bpf2sw64[BPF_REG_FP];
-	const int tcc = bpf2sw64[TCALL_CNT];
-	const int tmp1 = bpf2sw64[TMP_REG_1];
+	const u8 r6 = bpf2sw64[BPF_REG_6];
+	const u8 r7 = bpf2sw64[BPF_REG_7];
+	const u8 r8 = bpf2sw64[BPF_REG_8];
+	const u8 r9 = bpf2sw64[BPF_REG_9];
+	const u8 fp = bpf2sw64[BPF_REG_FP];
+	const u8 tcc = bpf2sw64[TCALL_CNT];
 
 	/* Save callee-saved registers */
-	emit(SW64_BPF_SUBL_REG(SW64_BPF_REG_SP, 56, SW64_BPF_REG_SP), ctx);
-	emit(SW64_BPF_STL(r6, SW64_BPF_REG_SP, 0), ctx);
-	emit(SW64_BPF_STL(r7, SW64_BPF_REG_SP, 8), ctx);
-	emit(SW64_BPF_STL(r8, SW64_BPF_REG_SP, 16), ctx);
-	emit(SW64_BPF_STL(r9, SW64_BPF_REG_SP, 24), ctx);
-	emit(SW64_BPF_STL(fp, SW64_BPF_REG_SP, 32), ctx);
-	emit(SW64_BPF_STL(tcc, SW64_BPF_REG_SP, 40), ctx);
-	emit(SW64_BPF_STL(SW64_BPF_REG_RA, SW64_BPF_REG_SP, 48), ctx);
+	emit(SW64_BPF_LDI(SW64_BPF_REG_SP, SW64_BPF_REG_SP, -64), ctx);
+	emit(SW64_BPF_STL(SW64_BPF_REG_RA, SW64_BPF_REG_SP, 0), ctx);
+	emit(SW64_BPF_STL(fp, SW64_BPF_REG_SP, 8), ctx);
+	emit(SW64_BPF_STL(r6, SW64_BPF_REG_SP, 16), ctx);
+	emit(SW64_BPF_STL(r7, SW64_BPF_REG_SP, 24), ctx);
+	emit(SW64_BPF_STL(r8, SW64_BPF_REG_SP, 32), ctx);
+	emit(SW64_BPF_STL(r9, SW64_BPF_REG_SP, 40), ctx);
+	emit(SW64_BPF_STL(tcc, SW64_BPF_REG_SP, 48), ctx);
+	emit(SW64_BPF_STL(SW64_BPF_REG_GP, SW64_BPF_REG_SP, 56), ctx);
 
 	/* Set up BPF prog stack base register */
-	emit(SW64_BPF_LDI(fp, SW64_BPF_REG_SP, 0), ctx);
+	emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_SP, fp), ctx);
 	if (!was_classic)
 		/* Initialize tail_call_cnt */
-		emit(SW64_BPF_LDI(tcc, SW64_BPF_REG_ZR, 0), ctx);
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_ZR, tcc), ctx);
 
 	/* Set up function call stack */
-	ctx->stack_size = ctx->prog->aux->stack_depth;
-	emit_sw64_ldu32(tmp1, ctx->stack_size, ctx);
-	emit(SW64_BPF_SUBL_REG(SW64_BPF_REG_SP, tmp1, SW64_BPF_REG_SP), ctx);
+	ctx->stack_size = (ctx->prog->aux->stack_depth + 15) & (~15);
+	emit(SW64_BPF_LDI(SW64_BPF_REG_SP, SW64_BPF_REG_SP, -ctx->stack_size), ctx);
 }
 
 static void build_epilogue(struct jit_ctx *ctx)
 {
-	const int r6 = bpf2sw64[BPF_REG_6];
-	const int r7 = bpf2sw64[BPF_REG_7];
-	const int r8 = bpf2sw64[BPF_REG_8];
-	const int r9 = bpf2sw64[BPF_REG_9];
-	const int fp = bpf2sw64[BPF_REG_FP];
-	const int tcc = bpf2sw64[TCALL_CNT];
-	const int tmp1 = bpf2sw64[TMP_REG_1];
+	const u8 r6 = bpf2sw64[BPF_REG_6];
+	const u8 r7 = bpf2sw64[BPF_REG_7];
+	const u8 r8 = bpf2sw64[BPF_REG_8];
+	const u8 r9 = bpf2sw64[BPF_REG_9];
+	const u8 fp = bpf2sw64[BPF_REG_FP];
+	const u8 tcc = bpf2sw64[TCALL_CNT];
 
 	/* Destroy function call stack */
-	emit_sw64_ldu32(tmp1, ctx->stack_size, ctx);
-	emit(SW64_BPF_ADDL_REG(SW64_BPF_REG_SP, tmp1, SW64_BPF_REG_SP), ctx);
+	emit(SW64_BPF_LDI(SW64_BPF_REG_SP, SW64_BPF_REG_SP, ctx->stack_size), ctx);
 
 	/* Restore callee-saved registers */
-	emit(SW64_BPF_LDL(r6, SW64_BPF_REG_SP, 0), ctx);
-	emit(SW64_BPF_LDL(r7, SW64_BPF_REG_SP, 8), ctx);
-	emit(SW64_BPF_LDL(r8, SW64_BPF_REG_SP, 16), ctx);
-	emit(SW64_BPF_LDL(r9, SW64_BPF_REG_SP, 24), ctx);
-	emit(SW64_BPF_LDL(fp, SW64_BPF_REG_SP, 32), ctx);
-	emit(SW64_BPF_LDL(tcc, SW64_BPF_REG_SP, 40), ctx);
-	emit(SW64_BPF_LDL(SW64_BPF_REG_RA, SW64_BPF_REG_SP, 48), ctx);
-	emit(SW64_BPF_ADDL_REG(SW64_BPF_REG_SP, 56, SW64_BPF_REG_SP), ctx);
+	emit(SW64_BPF_LDL(SW64_BPF_REG_RA, SW64_BPF_REG_SP, 0), ctx);
+	emit(SW64_BPF_LDL(fp, SW64_BPF_REG_SP, 8), ctx);
+	emit(SW64_BPF_LDL(r6, SW64_BPF_REG_SP, 16), ctx);
+	emit(SW64_BPF_LDL(r7, SW64_BPF_REG_SP, 24), ctx);
+	emit(SW64_BPF_LDL(r8, SW64_BPF_REG_SP, 32), ctx);
+	emit(SW64_BPF_LDL(r9, SW64_BPF_REG_SP, 40), ctx);
+	emit(SW64_BPF_LDL(tcc, SW64_BPF_REG_SP, 48), ctx);
+	emit(SW64_BPF_LDL(SW64_BPF_REG_GP, SW64_BPF_REG_SP, 56), ctx);
+	emit(SW64_BPF_LDI(SW64_BPF_REG_SP, SW64_BPF_REG_SP, 64), ctx);
 
 	/* Return */
 	emit(SW64_BPF_RET(SW64_BPF_REG_RA), ctx);
 }
 
-static int out_offset = -1; /* initialized on the first pass of build_body() */
 static int emit_bpf_tail_call(struct jit_ctx *ctx)
 {
-	/* bpf_tail_call(void *prog_ctx, struct bpf_array *array, u64 index) */
+	/* bpf_tail_call(void *ctx, struct bpf_map *prog_array_map, u32 index) */
 	const u8 r2 = bpf2sw64[BPF_REG_2];	/* struct bpf_array *array */
-	const u8 r3 = bpf2sw64[BPF_REG_3];	/* u64 index */
+	const u8 r3 = bpf2sw64[BPF_REG_3];	/* u32 index */
 
-	const u8 tmp = bpf2sw64[TMP_REG_1];
-	const u8 prg = bpf2sw64[TMP_REG_2];
+	const u8 tmp = get_tmp_reg(ctx);
+	const u8 prg = get_tmp_reg(ctx);
 	const u8 tcc = bpf2sw64[TCALL_CNT];
-	const int idx0 = ctx->idx;
-#define cur_offset (ctx->idx - idx0)
-#define jmp_offset (out_offset - (cur_offset))
 	u64 offset;
+	static int out_idx;
+#define out_offset	(ctx->image ? (out_idx - ctx->idx - 1) : 0)
 
 	/* if (index >= array->map.max_entries)
 	 *     goto out;
 	 */
 	offset = offsetof(struct bpf_array, map.max_entries);
-	emit_sw64_ldu64(tmp, offset, ctx);		/* tmp = offset */
-	emit(SW64_BPF_ADDL_REG(r2, tmp, tmp), ctx);		/* tmp = r2 + tmp = &map.max_entries */
+	emit_sw64_ldu64(tmp, offset, ctx);
+	emit(SW64_BPF_ADDL_REG(r2, tmp, tmp), ctx);	/* tmp = r2 + tmp = &map.max_entries */
 	emit(SW64_BPF_LDW(tmp, tmp, 0), ctx);		/* tmp = *tmp = map.max_entries */
-	emit(SW64_BPF_ZAPNOT_IMM(tmp, 0xf, tmp), ctx);	/* map.max_entries is u32 */
-	emit(SW64_BPF_SUBL_REG(r3, tmp, tmp), ctx);		/* tmp = r3 - tmp = index - map.max_entries */
-	emit(SW64_BPF_BGE(tmp, jmp_offset), ctx);
+	emit(SW64_BPF_ZAP_IMM(tmp, 0xf0, tmp), ctx);	/* map.max_entries is u32 */
+	emit(SW64_BPF_ZAP_IMM(r3, 0xf0, r3), ctx);	/* index is u32 */
+	emit(SW64_BPF_CMPULE_REG(tmp, r3, tmp), ctx);
+	emit(SW64_BPF_BNE(tmp, out_offset), ctx);
 
 	/* if (tail_call_cnt > MAX_TAIL_CALL_CNT)
 	 *     goto out;
 	 * tail_call_cnt++;
 	 */
-	emit(SW64_BPF_LDI(tmp, SW64_BPF_REG_ZR, MAX_TAIL_CALL_CNT), ctx);
-	emit(SW64_BPF_SUBL_REG(tcc, tmp, tmp), ctx);
-	emit(SW64_BPF_BGT(tmp, jmp_offset), ctx);
+	emit_sw64_ldu64(tmp, MAX_TAIL_CALL_CNT, ctx);
+	emit(SW64_BPF_CMPULT_REG(tmp, tcc, tmp), ctx);
+	emit(SW64_BPF_BNE(tmp, out_offset), ctx);
 	emit(SW64_BPF_ADDL_IMM(tcc, 1, tcc), ctx);
 
 	/* prog = array->ptrs[index];
@@ -398,34 +635,66 @@ static int emit_bpf_tail_call(struct jit_ctx *ctx)
 	 *     goto out;
 	 */
 	offset = offsetof(struct bpf_array, ptrs);
-	emit_sw64_ldu64(tmp, offset, ctx);		/* tmp = offset of ptrs */
-	emit(SW64_BPF_ADDL_REG(r2, tmp, tmp), ctx);		/* tmp = r2 + tmp = &ptrs */
-	emit(SW64_BPF_SLL_IMM(r3, 3, prg), ctx);		/* prg = r3 * 8, ptrs is 8 bit aligned */
-	emit(SW64_BPF_ADDL_REG(tmp, prg, prg), ctx);	/* prg = tmp + prg = &prog */
-	emit(SW64_BPF_LDL(prg, prg, 0), ctx);		/* prg = *prg = prog */
-	emit(SW64_BPF_BEQ(prg, jmp_offset), ctx);
+	emit_sw64_ldu64(tmp, offset, ctx);
+	emit(SW64_BPF_ADDL_REG(r2, tmp, tmp), ctx);	/* tmp = r2 + tmp = &ptrs[0] */
+	emit(SW64_BPF_SLL_IMM(r3, 3, prg), ctx);	/* prg = r3 * 8, each entry is a pointer */
+	emit(SW64_BPF_ADDL_REG(tmp, prg, prg), ctx);	/* prg = tmp + prg = &ptrs[index] */
+	emit(SW64_BPF_LDL(prg, prg, 0), ctx);		/* prg = *prg = ptrs[index] = prog */
+	emit(SW64_BPF_BEQ(prg, out_offset), ctx);
 
 	/* goto *(prog->bpf_func + prologue_offset); */
 	offset = offsetof(struct bpf_prog, bpf_func);
-	emit_sw64_ldu64(tmp, offset, ctx);		/* tmp = offset */
+	emit_sw64_ldu64(tmp, offset, ctx);
 	emit(SW64_BPF_ADDL_REG(prg, tmp, tmp), ctx);	/* tmp = prg + tmp = &bpf_func */
-	emit(SW64_BPF_LDW(tmp, tmp, 0), ctx);		/* tmp = *tmp = bpf_func */
-	emit(SW64_BPF_ZAPNOT_IMM(tmp, 0xf, tmp), ctx);	/* bpf_func is unsigned int */
-	emit(SW64_BPF_ADDL_REG(tmp, sizeof(u32) * PROLOGUE_OFFSET, tmp), ctx);
-	emit(SW64_BPF_ADDL_REG(SW64_BPF_REG_SP, ctx->stack_size, SW64_BPF_REG_SP), ctx);
-	emit(SW64_BPF_BR(tmp, 0), ctx);
+	emit(SW64_BPF_LDL(tmp, tmp, 0), ctx);		/* tmp = *tmp = bpf_func */
+	emit(SW64_BPF_BEQ(tmp, out_offset), ctx);
+	emit(SW64_BPF_LDI(tmp, tmp, sizeof(u32) * PROLOGUE_OFFSET), ctx);
+	emit(SW64_BPF_LDI(SW64_BPF_REG_SP, SW64_BPF_REG_SP, ctx->stack_size), ctx);
+	emit(SW64_BPF_JMP(SW64_BPF_REG_ZR, tmp), ctx);
+
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
 
 	/* out */
-	if (out_offset == -1)
-		out_offset = cur_offset;
-	if (cur_offset != out_offset) {
-		pr_err("tail_call out_offset = %d, expected %d!\n",
-				cur_offset, out_offset);
+	if (ctx->image == NULL)
+		out_idx = ctx->idx;
+	if (ctx->image != NULL && out_idx <= 0)
 		return -1;
-	}
+#undef out_offset
+	return 0;
+}
+
+/* For accesses to BTF pointers, add an entry to the exception table */
+static int add_exception_handler(const struct bpf_insn *insn,
+				 struct jit_ctx *ctx,
+				 int dst_reg)
+{
+	off_t offset;
+	unsigned long pc;
+	struct exception_table_entry *ex;
+
+	if (!ctx->image)
+		/* First pass */
+		return 0;
+
+	if (!ctx->prog->aux->extable || BPF_MODE(insn->code) != BPF_PROBE_MEM)
+		return 0;
+
+	if (WARN_ON_ONCE(ctx->exentry_idx >= ctx->prog->aux->num_exentries))
+		return -EINVAL;
+
+	ex = &ctx->prog->aux->extable[ctx->exentry_idx];
+	pc = (unsigned long)&ctx->image[ctx->idx - 1];
+
+	offset = (long)&ex->insn - pc;
+	ex->insn = offset;
+
+	ex->fixup.bits.nextinsn = sizeof(u32);
+	ex->fixup.bits.valreg = dst_reg;
+	ex->fixup.bits.errreg = SW64_BPF_REG_ZR;
+
+	ctx->exentry_idx++;
 	return 0;
-#undef cur_offset
-#undef jmp_offset
 }
 
 /* JITs an eBPF instruction.
@@ -434,80 +703,110 @@ static int emit_bpf_tail_call(struct jit_ctx *ctx)
  * >0 - successfully JITed a 16-byte eBPF instruction.
  * <0 - failed to JIT.
  */
-static inline int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
+static int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
 {
 	const u8 code = insn->code;
-	const u8 dst = bpf2sw64[insn->dst_reg];
-	const u8 src = bpf2sw64[insn->src_reg];
-	const u8 tmp1 = bpf2sw64[TMP_REG_1];
-	const u8 tmp2 = bpf2sw64[TMP_REG_2];
+	u8 dst = bpf2sw64[insn->dst_reg];
+	u8 src = bpf2sw64[insn->src_reg];
+	const u8 tmp1 __maybe_unused = get_tmp_reg(ctx);
+	const u8 tmp2 __maybe_unused = get_tmp_reg(ctx);
 	const s16 off = insn->off;
 	const s32 imm = insn->imm;
-	int jmp_offset;
+	const int bpf_idx = insn - ctx->prog->insnsi;
+	s32 jmp_offset;
 	u64 func;
 	struct bpf_insn insn1;
 	u64 imm64;
+	int ret;
 
 	switch (code) {
 	case BPF_ALU | BPF_MOV | BPF_X:
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_MOV | BPF_X:
-		emit(SW64_BPF_LDI(dst, src, 0), ctx);
+		emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_ADD | BPF_X:
 		emit(SW64_BPF_ADDW_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
 		break;
 	case BPF_ALU64 | BPF_ADD | BPF_X:
 		emit(SW64_BPF_ADDL_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_SUB | BPF_X:
 		emit(SW64_BPF_SUBW_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
 		break;
 	case BPF_ALU64 | BPF_SUB | BPF_X:
 		emit(SW64_BPF_SUBL_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_MUL | BPF_X:
 		emit(SW64_BPF_MULW_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
 		break;
 	case BPF_ALU64 | BPF_MUL | BPF_X:
 		emit(SW64_BPF_MULL_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_DIV | BPF_X:
+		emit_sw64_divmod(dst, src, ctx, code);
+		break;
 	case BPF_ALU64 | BPF_DIV | BPF_X:
-		emit_sw64_div(dst, src, dst, ctx);
-		return -EINVAL;
+		emit_sw64_divmod(dst, src, ctx, code);
+		break;
 	case BPF_ALU | BPF_MOD | BPF_X:
+		emit_sw64_divmod(dst, src, ctx, code);
+		break;
 	case BPF_ALU64 | BPF_MOD | BPF_X:
-		emit_sw64_mod(dst, src, dst, ctx);
-		return -EINVAL;
+		emit_sw64_divmod(dst, src, ctx, code);
+		break;
 	case BPF_ALU | BPF_LSH | BPF_X:
+		emit(SW64_BPF_SLL_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_LSH | BPF_X:
 		emit(SW64_BPF_SLL_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_RSH | BPF_X:
-		emit(SW64_BPF_ZAPNOT_IMM(dst, 0xf, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
 	case BPF_ALU64 | BPF_RSH | BPF_X:
 		emit(SW64_BPF_SRL_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_ARSH | BPF_X:
+		emit(SW64_BPF_ADDW_REG(SW64_BPF_REG_ZR, dst, dst), ctx);
+		emit(SW64_BPF_SRA_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_ARSH | BPF_X:
 		emit(SW64_BPF_SRA_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_AND | BPF_X:
+		emit(SW64_BPF_AND_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_AND | BPF_X:
 		emit(SW64_BPF_AND_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_OR | BPF_X:
+		emit(SW64_BPF_BIS_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_OR | BPF_X:
-		emit(SW64_BPF_OR_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_BIS_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_XOR | BPF_X:
+		emit(SW64_BPF_XOR_REG(dst, src, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_XOR | BPF_X:
 		emit(SW64_BPF_XOR_REG(dst, src, dst), ctx);
 		break;
 	case BPF_ALU | BPF_NEG:
+		emit(SW64_BPF_SUBW_REG(SW64_BPF_REG_ZR, dst, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_NEG:
-		emit(SW64_BPF_SEXTB_IMM(0xff, tmp1), ctx);
-		emit(SW64_BPF_XOR_IMM(dst, tmp1, dst), ctx);
+		emit(SW64_BPF_SUBL_REG(SW64_BPF_REG_ZR, dst, dst), ctx);
 		break;
 	case BPF_ALU | BPF_END | BPF_TO_LE:
 		switch (imm) {
@@ -519,7 +818,12 @@ static inline int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
 			break;
 		case 64:
 			break;
+		default:
+			pr_err("eBPF JIT %s[%d]: BPF_TO_LE unknown size\n",
+					current->comm, current->pid);
+			return -EINVAL;
 		}
+		break;
 	case BPF_ALU | BPF_END | BPF_TO_BE:
 		switch (imm) {
 		case 16:
@@ -531,73 +835,223 @@ static inline int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
 		case 64:
 			emit_sw64_htobe64(dst, ctx);
 			break;
+		default:
+			pr_err("eBPF JIT %s[%d]: BPF_TO_BE unknown size\n",
+					current->comm, current->pid);
+			return -EINVAL;
 		}
+		break;
 
 	case BPF_ALU | BPF_MOV | BPF_K:
+		if (imm >= S16_MIN && imm <= S16_MAX)
+			emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm), ctx);
+		else
+			emit_sw64_ldu32(dst, imm, ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_MOV | BPF_K:
-		emit_sw64_lds32(dst, imm, ctx);
+		if (imm >= S16_MIN && imm <= S16_MAX)
+			emit(SW64_BPF_LDI(dst, SW64_BPF_REG_ZR, imm), ctx);
+		else
+			emit_sw64_lds32(dst, imm, ctx);
 		break;
 	case BPF_ALU | BPF_ADD | BPF_K:
+		if (imm >= S16_MIN && imm <= S16_MAX) {
+			emit(SW64_BPF_LDI(dst, dst, imm), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_ADDW_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_ADD | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
+		if (imm >= S16_MIN && imm <= S16_MAX) {
+			emit(SW64_BPF_LDI(dst, dst, imm), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_ADDL_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_SUB | BPF_K:
+		if (imm >= -S16_MAX && imm <= -S16_MIN) {
+			emit(SW64_BPF_LDI(dst, dst, -imm), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_SUBL_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_SUB | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_SUBL_REG(dst, tmp1, dst), ctx);
+		if (imm >= -S16_MAX && imm <= -S16_MIN) {
+			emit(SW64_BPF_LDI(dst, dst, -imm), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_SUBL_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_MUL | BPF_K:
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_MULL_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_MULL_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_MUL | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_MULL_REG(dst, tmp1, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_MULL_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_MULL_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_DIV | BPF_K:
+		emit_sw64_ldu32(tmp1, imm, ctx);
+		emit_sw64_divmod(dst, tmp1, ctx, code);
+		break;
 	case BPF_ALU64 | BPF_DIV | BPF_K:
 		emit_sw64_lds32(tmp1, imm, ctx);
-		emit_sw64_div(dst, src, tmp1, ctx);
-		return -EINVAL;
+		emit_sw64_divmod(dst, tmp1, ctx, code);
+		break;
 	case BPF_ALU | BPF_MOD | BPF_K:
+		emit_sw64_ldu32(tmp1, imm, ctx);
+		emit_sw64_divmod(dst, tmp1, ctx, code);
+		break;
 	case BPF_ALU64 | BPF_MOD | BPF_K:
 		emit_sw64_lds32(tmp1, imm, ctx);
-		emit_sw64_mod(dst, src, tmp1, ctx);
-		return -EINVAL;
+		emit_sw64_divmod(dst, tmp1, ctx, code);
+		break;
 	case BPF_ALU | BPF_LSH | BPF_K:
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_SLL_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_SLL_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_LSH | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_SLL_REG(dst, tmp1, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_SLL_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_SLL_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_RSH | BPF_K:
-		emit(SW64_BPF_ZAPNOT_IMM(dst, 0xf, dst), ctx);
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_SRL_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_SRL_REG(dst, tmp1, dst), ctx);
+		}
+		break;
 	case BPF_ALU64 | BPF_RSH | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_SRL_REG(dst, tmp1, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_SRL_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_SRL_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_ARSH | BPF_K:
+		emit(SW64_BPF_ADDW_REG(SW64_BPF_REG_ZR, dst, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_SRA_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_SRA_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_ARSH | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_SRA_REG(dst, tmp1, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_SRA_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_SRA_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_AND | BPF_K:
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_AND_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_AND_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_AND | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_AND_REG(dst, tmp1, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_AND_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_AND_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_OR | BPF_K:
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_BIS_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_BIS_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_OR | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_OR_REG(dst, tmp1, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_BIS_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_BIS_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 	case BPF_ALU | BPF_XOR | BPF_K:
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_XOR_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_ldu32(tmp1, imm, ctx);
+			emit(SW64_BPF_XOR_REG(dst, tmp1, dst), ctx);
+		}
+		emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+		break;
 	case BPF_ALU64 | BPF_XOR | BPF_K:
-		emit_sw64_lds32(tmp1, imm, ctx);
-		emit(SW64_BPF_XOR_REG(dst, tmp1, dst), ctx);
+		if (imm >= 0 && imm <= U8_MAX) {
+			emit(SW64_BPF_XOR_IMM(dst, imm, dst), ctx);
+		} else {
+			emit_sw64_lds32(tmp1, imm, ctx);
+			emit(SW64_BPF_XOR_REG(dst, tmp1, dst), ctx);
+		}
 		break;
 
 	case BPF_JMP | BPF_JA:
-		emit(SW64_BPF_BR(SW64_BPF_REG_RA, off), ctx);
+		jmp_offset = bpf2sw64_offset(bpf_idx, off, ctx);
+		if (jmp_offset >= -0x100000 && jmp_offset <= 0xfffff) {
+			emit(SW64_BPF_BR(SW64_BPF_REG_ZR, jmp_offset), ctx);
+		} else {
+			pr_err("eBPF JIT %s[%d]: BPF_JMP out of range, %d instructions\n",
+					current->comm, current->pid, jmp_offset);
+			return -EINVAL;
+		}
 		break;
 
+	case BPF_JMP32 | BPF_JEQ | BPF_X:
+	case BPF_JMP32 | BPF_JGT | BPF_X:
+	case BPF_JMP32 | BPF_JLT | BPF_X:
+	case BPF_JMP32 | BPF_JGE | BPF_X:
+	case BPF_JMP32 | BPF_JLE | BPF_X:
+	case BPF_JMP32 | BPF_JNE | BPF_X:
+	case BPF_JMP32 | BPF_JSGT | BPF_X:
+	case BPF_JMP32 | BPF_JSLT | BPF_X:
+	case BPF_JMP32 | BPF_JSGE | BPF_X:
+	case BPF_JMP32 | BPF_JSLE | BPF_X:
+	case BPF_JMP32 | BPF_JSET | BPF_X:
+		emit(SW64_BPF_ADDW_REG(SW64_BPF_REG_ZR, src, tmp1), ctx);
+		src = tmp1;
+		emit(SW64_BPF_ADDW_REG(SW64_BPF_REG_ZR, dst, tmp2), ctx);
+		dst = tmp2;
 	case BPF_JMP | BPF_JEQ | BPF_X:
 	case BPF_JMP | BPF_JGT | BPF_X:
 	case BPF_JMP | BPF_JLT | BPF_X:
@@ -645,9 +1099,29 @@ static inline int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
 			emit(SW64_BPF_AND_REG(dst, src, tmp1), ctx);
 			break;
 		}
-		emit(SW64_BPF_BLBS(tmp1, off), ctx);
+		jmp_offset = bpf2sw64_offset(bpf_idx, off, ctx);
+		if (jmp_offset >= -0x100000 && jmp_offset <= 0xfffff) {
+			emit(SW64_BPF_BNE(tmp1, jmp_offset), ctx);
+		} else {
+			pr_err("eBPF JIT %s[%d]: BPF_JMP out of range, %d instructions\n",
+					current->comm, current->pid, jmp_offset);
+			return -EINVAL;
+		}
 		break;
 
+	case BPF_JMP32 | BPF_JEQ | BPF_K:
+	case BPF_JMP32 | BPF_JGT | BPF_K:
+	case BPF_JMP32 | BPF_JLT | BPF_K:
+	case BPF_JMP32 | BPF_JGE | BPF_K:
+	case BPF_JMP32 | BPF_JLE | BPF_K:
+	case BPF_JMP32 | BPF_JNE | BPF_K:
+	case BPF_JMP32 | BPF_JSGT | BPF_K:
+	case BPF_JMP32 | BPF_JSLT | BPF_K:
+	case BPF_JMP32 | BPF_JSGE | BPF_K:
+	case BPF_JMP32 | BPF_JSLE | BPF_K:
+	case BPF_JMP32 | BPF_JSET | BPF_K:
+		emit(SW64_BPF_ADDW_REG(SW64_BPF_REG_ZR, dst, tmp2), ctx);
+		dst = tmp2;
 	case BPF_JMP | BPF_JEQ | BPF_K:
 	case BPF_JMP | BPF_JGT | BPF_K:
 	case BPF_JMP | BPF_JLT | BPF_K:
@@ -662,47 +1136,57 @@ static inline int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
 		emit_sw64_lds32(tmp1, imm, ctx);
 		switch (BPF_OP(code)) {
 		case BPF_JEQ:
-			emit(SW64_BPF_CMPEQ_REG(dst, tmp1, tmp1), ctx);
+			emit(SW64_BPF_CMPEQ_REG(dst, tmp1, tmp2), ctx);
 			break;
 		case BPF_JGT:
-			emit(SW64_BPF_CMPULT_REG(tmp1, dst, tmp1), ctx);
+			emit(SW64_BPF_CMPULT_REG(tmp1, dst, tmp2), ctx);
 			break;
 		case BPF_JLT:
-			emit(SW64_BPF_CMPULT_REG(dst, tmp1, tmp1), ctx);
+			emit(SW64_BPF_CMPULT_REG(dst, tmp1, tmp2), ctx);
 			break;
 		case BPF_JGE:
-			emit(SW64_BPF_CMPULE_REG(tmp1, dst, tmp1), ctx);
+			emit(SW64_BPF_CMPULE_REG(tmp1, dst, tmp2), ctx);
 			break;
 		case BPF_JLE:
-			emit(SW64_BPF_CMPULE_REG(dst, tmp1, tmp1), ctx);
+			emit(SW64_BPF_CMPULE_REG(dst, tmp1, tmp2), ctx);
 			break;
 		case BPF_JNE:
-			emit(SW64_BPF_CMPEQ_REG(dst, tmp1, tmp1), ctx);
-			emit(SW64_BPF_XOR_IMM(tmp1, 1, tmp1), ctx);
+			emit(SW64_BPF_CMPEQ_REG(dst, tmp1, tmp2), ctx);
+			emit(SW64_BPF_XOR_IMM(tmp2, 1, tmp2), ctx);
 			break;
 		case BPF_JSGT:
-			emit(SW64_BPF_CMPLT_REG(tmp1, dst, tmp1), ctx);
+			emit(SW64_BPF_CMPLT_REG(tmp1, dst, tmp2), ctx);
 			break;
 		case BPF_JSLT:
-			emit(SW64_BPF_CMPLT_REG(dst, tmp1, tmp1), ctx);
+			emit(SW64_BPF_CMPLT_REG(dst, tmp1, tmp2), ctx);
 			break;
 		case BPF_JSGE:
-			emit(SW64_BPF_CMPLE_REG(tmp1, dst, tmp1), ctx);
+			emit(SW64_BPF_CMPLE_REG(tmp1, dst, tmp2), ctx);
 			break;
 		case BPF_JSLE:
-			emit(SW64_BPF_CMPLE_REG(dst, tmp1, tmp1), ctx);
+			emit(SW64_BPF_CMPLE_REG(dst, tmp1, tmp2), ctx);
 			break;
 		case BPF_JSET:
-			emit(SW64_BPF_AND_REG(dst, tmp1, tmp1), ctx);
+			emit(SW64_BPF_AND_REG(dst, tmp1, tmp2), ctx);
 			break;
 		}
-		emit(SW64_BPF_BLBS(tmp1, off), ctx);
+		jmp_offset = bpf2sw64_offset(bpf_idx, off, ctx);
+		if (jmp_offset >= -0x100000 && jmp_offset <= 0xfffff) {
+			emit(SW64_BPF_BNE(tmp2, jmp_offset), ctx);
+		} else {
+			pr_err("eBPF JIT %s[%d]: BPF_JMP out of range, %d instructions\n",
+					current->comm, current->pid, jmp_offset);
+			return -EINVAL;
+		}
 		break;
 
 	case BPF_JMP | BPF_CALL:
 		func = (u64)__bpf_call_base + imm;
-		emit_sw64_ldu64(tmp1, func, ctx);
-		emit(SW64_BPF_CALL(SW64_BPF_REG_RA, tmp1), ctx);
+		if ((func & 0xffffffffe0000000UL) != 0xffffffff80000000UL)
+			/* calling bpf program, switch to vmalloc addr */
+			func = (func & 0xffffffff) | 0xfffff00000000000UL;
+		emit_sw64_ldu64(SW64_BPF_REG_PV, func, ctx);
+		emit(SW64_BPF_CALL(SW64_BPF_REG_RA, SW64_BPF_REG_PV), ctx);
 		break;
 
 	case BPF_JMP | BPF_TAIL_CALL:
@@ -711,38 +1195,60 @@ static inline int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
 		break;
 
 	case BPF_JMP | BPF_EXIT:
-		if (insn - ctx->prog->insnsi + 1 == ctx->prog->len)
+		// if this is the last instruction, fallthrough to epilogue
+		if (bpf_idx == ctx->prog->len - 1)
 			break;
-		jmp_offset = (offset_to_epilogue(ctx) - 1) * 4;
-		// emit(SW64_BPF_BR(SW64_BPF_REG_ZR, jmp_offset), ctx);
-		// break;
-		emit_sw64_lds32(tmp1, jmp_offset, ctx);
-		emit(SW64_BPF_BR(tmp2, 0), ctx);
-		emit(SW64_BPF_ADDL_REG(tmp1, tmp2, tmp1), ctx);
-		emit(SW64_BPF_JMP(SW64_BPF_REG_ZR, tmp1), ctx);
+		jmp_offset = offset_to_epilogue(ctx) - 1;
+		// epilogue is always at the end, must jump forward
+		if (jmp_offset >= -1 && jmp_offset <= 0xfffff) {
+			if (ctx->image && !jmp_offset)
+				// if this is the last instruction, fallthrough to epilogue
+				emit(SW64_BPF_BIS_REG(SW64_BPF_REG_ZR, SW64_BPF_REG_ZR, SW64_BPF_REG_ZR), ctx);
+			else
+				emit(SW64_BPF_BR(SW64_BPF_REG_ZR, jmp_offset), ctx);
+		} else {
+			pr_err("eBPF JIT %s[%d]: BPF_EXIT out of range, %d instructions\n",
+					current->comm, current->pid, jmp_offset);
+			return -EINVAL;
+		}
 		break;
 
 	case BPF_LD | BPF_IMM | BPF_DW:
 		insn1 = insn[1];
-		imm64 = (u64)insn1.imm << 32 | (u32)imm;
+		imm64 = ((u64)insn1.imm << 32) | (u32)imm;
 		emit_sw64_ldu64(dst, imm64, ctx);
-
+		put_tmp_reg(ctx);
+		put_tmp_reg(ctx);
 		return 1;
 
 	/* LDX: dst = *(size *)(src + off) */
 	case BPF_LDX | BPF_MEM | BPF_W:
-		emit(SW64_BPF_LDW(dst, src, off), ctx);
-		break;
 	case BPF_LDX | BPF_MEM | BPF_H:
-		emit(SW64_BPF_LDHU(dst, src, off), ctx);
-		emit(SW64_BPF_SEXTH_REG(dst, dst), ctx);
-		break;
 	case BPF_LDX | BPF_MEM | BPF_B:
-		emit(SW64_BPF_LDBU(dst, src, off), ctx);
-		emit(SW64_BPF_SEXTB_REG(dst, dst), ctx);
-		break;
 	case BPF_LDX | BPF_MEM | BPF_DW:
-		emit(SW64_BPF_LDW(dst, src, off), ctx);
+	case BPF_LDX | BPF_PROBE_MEM | BPF_DW:
+	case BPF_LDX | BPF_PROBE_MEM | BPF_W:
+	case BPF_LDX | BPF_PROBE_MEM | BPF_H:
+	case BPF_LDX | BPF_PROBE_MEM | BPF_B:
+		switch (BPF_SIZE(code)) {
+		case BPF_W:
+			emit(SW64_BPF_LDW(dst, src, off), ctx);
+			emit(SW64_BPF_ZAP_IMM(dst, 0xf0, dst), ctx);
+			break;
+		case BPF_H:
+			emit(SW64_BPF_LDHU(dst, src, off), ctx);
+			break;
+		case BPF_B:
+			emit(SW64_BPF_LDBU(dst, src, off), ctx);
+			break;
+		case BPF_DW:
+			emit(SW64_BPF_LDL(dst, src, off), ctx);
+			break;
+		}
+
+		ret = add_exception_handler(insn, ctx, dst);
+		if (ret)
+			return ret;
 		break;
 
 	/* ST: *(size *)(dst + off) = imm */
@@ -773,33 +1279,32 @@ static inline int build_insn(const struct bpf_insn *insn, struct jit_ctx *ctx)
 		emit(SW64_BPF_STW(src, dst, off), ctx);
 		break;
 	case BPF_STX | BPF_MEM | BPF_H:
-		emit(SW64_BPF_STW(src, dst, off), ctx);
+		emit(SW64_BPF_STH(src, dst, off), ctx);
 		break;
 	case BPF_STX | BPF_MEM | BPF_B:
-		emit(SW64_BPF_STW(src, dst, off), ctx);
+		emit(SW64_BPF_STB(src, dst, off), ctx);
 		break;
 	case BPF_STX | BPF_MEM | BPF_DW:
-		emit(SW64_BPF_STW(src, dst, off), ctx);
+		emit(SW64_BPF_STL(src, dst, off), ctx);
 		break;
 
 	/* STX XADD: lock *(u32 *)(dst + off) += src */
 	case BPF_STX | BPF_XADD | BPF_W:
-		emit(SW64_BPF_LDW(tmp1, dst, off), ctx);
-		emit(SW64_BPF_ADDW_REG(tmp1, src, tmp1), ctx);
-		emit(SW64_BPF_STW(tmp1, dst, off), ctx);
+		emit_sw64_xadd32(src, dst, off, ctx);
 		break;
 	/* STX XADD: lock *(u64 *)(dst + off) += src */
 	case BPF_STX | BPF_XADD | BPF_DW:
-		emit(SW64_BPF_LDL(tmp1, dst, off), ctx);
-		emit(SW64_BPF_ADDL_REG(tmp1, src, tmp1), ctx);
-		emit(SW64_BPF_STL(tmp1, dst, off), ctx);
+		emit_sw64_xadd64(src, dst, off, ctx);
 		break;
 
 	default:
-		pr_err("unknown opcode %02x\n", code);
+		pr_err("eBPF JIT %s[%d]: unknown opcode 0x%02x\n",
+				current->comm, current->pid, code);
 		return -EINVAL;
 	}
 
+	put_tmp_reg(ctx);
+	put_tmp_reg(ctx);
 	return 0;
 }
 
@@ -812,17 +1317,17 @@ static int build_body(struct jit_ctx *ctx)
 		const struct bpf_insn *insn = &prog->insnsi[i];
 		int ret;
 
+		if (ctx->image == NULL)
+			ctx->insn_offset[i] = ctx->idx;
 		ret = build_insn(insn, ctx);
-		if (ret > 0) {
+		if (ret < 0)
+			return ret;
+		while (ret > 0) {
 			i++;
 			if (ctx->image == NULL)
-				ctx->insn_offset[i] = ctx->idx;
-			continue;
+				ctx->insn_offset[i] = ctx->insn_offset[i - 1];
+			ret--;
 		}
-		if (ctx->image == NULL)
-			ctx->insn_offset[i] = ctx->idx;
-		if (ret)
-			return ret;
 	}
 
 	return 0;
@@ -837,6 +1342,9 @@ static int validate_code(struct jit_ctx *ctx)
 			return -1;
 	}
 
+	if (WARN_ON_ONCE(ctx->exentry_idx != ctx->prog->aux->num_exentries))
+		return -1;
+
 	return 0;
 }
 
@@ -854,7 +1362,7 @@ struct bpf_prog *bpf_int_jit_compile(struct bpf_prog *prog)
 	bool tmp_blinded = false;
 	bool extra_pass = false;
 	struct jit_ctx ctx;
-	int image_size;
+	int image_size, prog_size, extable_size;
 	u8 *image_ptr;
 
 	if (!prog->jit_requested)
@@ -885,13 +1393,13 @@ struct bpf_prog *bpf_int_jit_compile(struct bpf_prog *prog)
 		image_ptr = jit_data->image;
 		header = jit_data->header;
 		extra_pass = true;
-		image_size = sizeof(u32) * ctx.idx;
+		prog_size = sizeof(u32) * ctx.idx;
 		goto skip_init_ctx;
 	}
 	memset(&ctx, 0, sizeof(ctx));
 	ctx.prog = prog;
 
-	ctx.insn_offset = kcalloc(prog->len, sizeof(int), GFP_KERNEL);
+	ctx.insn_offset = kcalloc(prog->len + 1, sizeof(int), GFP_KERNEL);
 	if (ctx.insn_offset == NULL) {
 		prog = orig_prog;
 		goto out_off;
@@ -907,11 +1415,16 @@ struct bpf_prog *bpf_int_jit_compile(struct bpf_prog *prog)
 		goto out_off;
 	}
 
-	ctx.epilogue_offset = ctx.idx;
+	ctx.insn_offset[prog->len] = ctx.epilogue_offset = ctx.idx;
 	build_epilogue(&ctx);
 
+	extable_size = prog->aux->num_exentries *
+		sizeof(struct exception_table_entry);
+
 	/* Now we know the actual image size. */
-	image_size = sizeof(u32) * ctx.idx;
+	/* And we need extra 8 bytes for lock instructions alignment */
+	prog_size = sizeof(u32) * ctx.idx + 8;
+	image_size = prog_size + extable_size;
 	header = bpf_jit_binary_alloc(image_size, &image_ptr,
 				      sizeof(u32), jit_fill_hole);
 	if (header == NULL) {
@@ -921,9 +1434,13 @@ struct bpf_prog *bpf_int_jit_compile(struct bpf_prog *prog)
 
 	/* 2. Now, the actual pass. */
 
-	ctx.image = (u32 *)image_ptr;
+	/* lock instructions need 8-byte alignment */
+	ctx.image = (u32 *)(((unsigned long)image_ptr + 7) & (~7));
+	if (extable_size)
+		prog->aux->extable = (void *)image_ptr + prog_size;
 skip_init_ctx:
 	ctx.idx = 0;
+	ctx.exentry_idx = 0;
 
 	build_prologue(&ctx, was_classic);
 
@@ -944,7 +1461,7 @@ struct bpf_prog *bpf_int_jit_compile(struct bpf_prog *prog)
 
 	/* And we're done. */
 	if (bpf_jit_enable > 1)
-		bpf_jit_dump(prog->len, image_size, 2, ctx.image);
+		bpf_jit_dump(prog->len, prog_size, 2, ctx.image);
 
 	bpf_flush_icache(header, ctx.image + ctx.idx);
 
@@ -957,7 +1474,11 @@ struct bpf_prog *bpf_int_jit_compile(struct bpf_prog *prog)
 	}
 	prog->bpf_func = (void *)ctx.image;
 	prog->jited = 1;
-	prog->jited_len = image_size;
+	prog->jited_len = prog_size;
+	if (ctx.current_tmp_reg) {
+		pr_err("eBPF JIT %s[%d]: unreleased temporary regsters %d\n",
+				current->comm, current->pid, ctx.current_tmp_reg);
+	}
 
 	if (!prog->is_func || extra_pass) {
 out_off:
diff --git a/arch/sw_64/oprofile/op_impl.h b/arch/sw_64/oprofile/op_impl.h
index 10bdd455c3dde7c259e424606420c991b08ad86f..e8714cb7c2ea773b8c270bd4f9cfee7c3c8286db 100644
--- a/arch/sw_64/oprofile/op_impl.h
+++ b/arch/sw_64/oprofile/op_impl.h
@@ -53,4 +53,4 @@ struct op_axp_model {
 	unsigned char can_set_proc_mode;
 };
 
-#endif
+#endif /* _SW64_OPROFILE_OP_IMPL_H */
diff --git a/arch/sw_64/tools/relocs.c b/arch/sw_64/tools/relocs.c
index a8a9e08a0a658b6347f9f6c01dce883746b7fb1e..06bd4625bc6e0c57c5eb4fc6566bfdc884d81a69 100644
--- a/arch/sw_64/tools/relocs.c
+++ b/arch/sw_64/tools/relocs.c
@@ -487,6 +487,7 @@ static int do_reloc(struct section *sec, Elf_Rel *rel, Elf_Sym *sym,
 	case R_SW64_SREL32:
 	case R_SW64_GPRELHIGH:
 	case R_SW64_GPRELLOW:
+	case R_SW64_LITERAL_GOT:
 		/*
 		 * NONE can be ignored and PC relative relocations don't
 		 * need to be adjusted.
diff --git a/arch/sw_64/tools/relocs.h b/arch/sw_64/tools/relocs.h
index 7273ccaed11fee146d4d5ba695fb39791d266740..17c7e31113a0e5f93ac2b596d54e31bc9de7fe58 100644
--- a/arch/sw_64/tools/relocs.h
+++ b/arch/sw_64/tools/relocs.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0 */
-#ifndef RELOCS_H
-#define RELOCS_H
+#ifndef _SW64_TOOLS_RELOCS_H
+#define _SW64_TOOLS_RELOCS_H
 
 #include <stdio.h>
 #include <stdarg.h>
@@ -53,6 +53,7 @@
 #define R_SW64_TPRELHI		39
 #define R_SW64_TPRELLO		40
 #define R_SW64_TPREL16		41
+#define R_SW64_LITERAL_GOT	43	/* GP relative */
 
 void die(char *fmt, ...);
 
@@ -68,4 +69,4 @@ enum symtype {
 
 void process(FILE *fp, int as_text, int as_bin,
 		int show_reloc_info, int keep_relocs);
-#endif /* RELOCS_H */
+#endif /* _SW64_TOOLS_RELOCS_H */
diff --git a/drivers/cpufreq/sw64_cpufreq.c b/drivers/cpufreq/sw64_cpufreq.c
index 5f49b5175d34f634d13e71c18c87a6e142186359..819d8f1437e284e4da4bd381527f895916426843 100644
--- a/drivers/cpufreq/sw64_cpufreq.c
+++ b/drivers/cpufreq/sw64_cpufreq.c
@@ -40,10 +40,8 @@ static int sw64_cpu_freq_notifier(struct notifier_block *nb,
 	unsigned long cpu;
 
 	for_each_online_cpu(cpu) {
-		if (val == CPUFREQ_POSTCHANGE) {
+		if (val == CPUFREQ_POSTCHANGE)
 			sw64_update_clockevents(cpu, freqs->new * 1000);
-			current_cpu_data.loops_per_jiffy = loops_per_jiffy;
-		}
 	}
 
 	return 0;
@@ -59,7 +57,7 @@ static unsigned int sw64_cpufreq_get(unsigned int cpu)
 		return 0;
 	}
 
-	return sw64_clk_get_rate(policy->clk);
+	return __sw64_cpufreq_get(policy) * 1000;
 }
 
 /*
@@ -70,12 +68,12 @@ static int sw64_cpufreq_target(struct cpufreq_policy *policy,
 {
 	unsigned long freq;
 
-	freq = (get_cpu_freq() / 1000) * index / 48;
+	freq = 50000 * index;
 
 	sw64_store_policy(policy);
 
 	/* setting the cpu frequency */
-	sw64_set_rate(-1, freq * 1000);
+	sw64_set_rate(freq * 1000);
 
 	return 0;
 }
@@ -100,7 +98,7 @@ static int sw64_cpufreq_cpu_init(struct cpufreq_policy *policy)
 		if (sw64_clockmod_table[i].frequency == 0)
 			sw64_clockmod_table[i].frequency = (rate * i) / 48;
 
-	sw64_set_rate(-1, rate * 1000);
+	sw64_set_rate(rate * 1000);
 
 	policy->clk = cpuclk;
 
diff --git a/drivers/firmware/efi/sunway-init.c b/drivers/firmware/efi/sunway-init.c
index 9871508df58c7f89caa401e034cf0790341b7bed..b130218634fbd52ce9e3a6f85723ee58098ed1d6 100644
--- a/drivers/firmware/efi/sunway-init.c
+++ b/drivers/firmware/efi/sunway-init.c
@@ -25,8 +25,6 @@
 
 #include <asm/efi.h>
 
-extern bool __virt_addr_valid(unsigned long x);
-
 static int __init is_memory(efi_memory_desc_t *md)
 {
 	if (md->attribute & (EFI_MEMORY_WB|EFI_MEMORY_WT|EFI_MEMORY_WC))
@@ -128,23 +126,7 @@ static __init int is_usable_memory(efi_memory_desc_t *md)
 	}
 	return false;
 }
-static __initdata char memory_type_name1[][20] = {
-	"Reserved",
-	"Loader Code",
-	"Loader Data",
-	"Boot Code",
-	"Boot Data",
-	"Runtime Code",
-	"Runtime Data",
-	"Conventional Memory",
-	"Unusable Memory",
-	"ACPI Reclaim Memory",
-	"ACPI Memory NVS",
-	"Memory Mapped I/O",
-	"MMIO Port Space",
-	"PAL Code",
-	"Persistent Memory",
-};
+
 static __init void reserve_regions(void)
 {
 	efi_memory_desc_t *md;
@@ -157,22 +139,6 @@ static __init void reserve_regions(void)
 		paddr = md->phys_addr;
 		npages = md->num_pages;
 
-		if (!__virt_addr_valid(paddr))
-			continue;
-
-		if (md->type >= ARRAY_SIZE(memory_type_name1))
-			continue;
-
-		if (md->attribute & ~(EFI_MEMORY_UC | EFI_MEMORY_WC | EFI_MEMORY_WT |
-					EFI_MEMORY_WB | EFI_MEMORY_UCE | EFI_MEMORY_RO |
-					EFI_MEMORY_WP | EFI_MEMORY_RP | EFI_MEMORY_XP |
-					EFI_MEMORY_NV |
-					EFI_MEMORY_RUNTIME | EFI_MEMORY_MORE_RELIABLE))
-			continue;
-
-		if (strncmp(memory_type_name1[md->type], "Reserved", 8) == 0)
-			continue;
-
 		if (efi_enabled(EFI_DBG)) {
 			char buf[64];
 
diff --git a/drivers/iommu/sw64/sunway_iommu.c b/drivers/iommu/sw64/sunway_iommu.c
index b6c8f1272d28fc9de9575fa9d245eb8434396704..580619c6a571e8944130c622fed8e13b17cc78a1 100644
--- a/drivers/iommu/sw64/sunway_iommu.c
+++ b/drivers/iommu/sw64/sunway_iommu.c
@@ -1382,7 +1382,7 @@ static struct iommu_domain *sunway_iommu_domain_alloc(unsigned type)
 
 		sdomain->pt_root = (void *)get_zeroed_page(GFP_KERNEL);
 
-		sdomain->domain.geometry.aperture_start = SW64_DMA_START;
+		sdomain->domain.geometry.aperture_start = 0ULL;
 		sdomain->domain.geometry.aperture_end	= (~0ULL);
 		sdomain->domain.geometry.force_aperture	= true;
 		sdomain->type = IOMMU_DOMAIN_UNMANAGED;