/*
 * arch/arm64/kernel/patch_text.c
 *
 * Copyright (C) 2013 Linaro Limited.
 * Based on arch/arm/kernel/patch.c
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 */
#include <linux/kernel.h>
#include <linux/kprobes.h>
#include <linux/stop_machine.h>
#include <linux/swab.h>
#include <asm/cacheflush.h>
#include <asm/smp_plat.h>

#include "patch_text.h"

#define opcode_to_mem_le(x) (x)
#define opcode_to_mem_be(x) swab32(x)

/*
 * get cpu endian mode from SCTLR_EL1.EE
 * 0x1=Big Endian, 0x0=Litle Endian.
 */
static inline unsigned int is_el1_big_endian(void)
{
	u32 sctlr;

	asm volatile ("mrs %0, sctlr_el1" : "=r" (sctlr));
	return (sctlr >> 25) & 0x1;
}

struct patch {
	void *addr;
	unsigned int insn;
};

/* Patch text - Supported for kernel in AArch64 mode only */
void __kprobes __patch_text(void *addr, unsigned int insn)
{
	int size = sizeof(u32);

	/* address 32-bit alignment check */
	if ((unsigned long)addr % size) {
		pr_warn("text patching failed @unaligned addr %p\n", addr);
		return;
	}

	if (is_el1_big_endian())
		*(u32 *) addr = opcode_to_mem_be(insn);
	else
		*(u32 *) addr = opcode_to_mem_le(insn);

	/* sync Icache, ISB is issued as part of this */
	flush_icache_range((uintptr_t) (addr), (uintptr_t) (addr) + size);
}

static int __kprobes patch_text_stop_machine(void *data)
{
	struct patch *patch = data;

	__patch_text(patch->addr, patch->insn);
	return 0;
}

void __kprobes patch_text(void *addr, unsigned int insn)
{
	struct patch patch = {
		.addr = addr,
		.insn = insn,
	};
	stop_machine(patch_text_stop_machine, &patch, cpu_online_mask);
}