static int set_up_temporary_text_mapping(pgd_t *pgd)
{
pmd_t *pmd;
pud_t *pud;
/*
* The new mapping only has to cover the page containing the image
* kernel's entry point (jump_address_phys), because the switch over to
* it is carried out by relocated code running from a page allocated
* specifically for this purpose and covered by the identity mapping, so
* the temporary kernel text mapping is only needed for the final jump.
* Moreover, in that mapping the virtual address of the image kernel's
* entry point must be the same as its virtual address in the image
* kernel (restore_jump_address), so the image kernel's
* restore_registers() code doesn't find itself in a different area of
* the virtual address space after switching over to the original page
* tables used by the image kernel.
*/
pud = (pud_t *)get_safe_page(GFP_ATOMIC);
if (!pud)
return -ENOMEM;
pmd = (pmd_t *)get_safe_page(GFP_ATOMIC);
if (!pmd)
return -ENOMEM;
set_pmd(pmd + pmd_index(restore_jump_address),
__pmd((jump_address_phys & PMD_MASK) | __PAGE_KERNEL_LARGE_EXEC));
set_pud(pud + pud_index(restore_jump_address),
__pud(__pa(pmd) | _KERNPG_TABLE));
set_pgd(pgd + pgd_index(restore_jump_address),
__pgd(__pa(pud) | _KERNPG_TABLE));
return 0;
}
static int set_up_temporary_mappings(void)
{
unsigned long start, end, next;
int error;
temp_level4_pgt = (pgd_t *)get_safe_page(GFP_ATOMIC);
if (!temp_level4_pgt)
return -ENOMEM;
/* It is safe to reuse the original kernel mapping */
set_pgd(temp_level4_pgt + pgd_index(__START_KERNEL_map),
init_level4_pgt[pgd_index(__START_KERNEL_map)]);
/* Set up the direct mapping from scratch */
start = (unsigned long)pfn_to_kaddr(0);
end = (unsigned long)pfn_to_kaddr(max_pfn);
for (; start < end; start = next) {
pud_t *pud = (pud_t *)get_safe_page(GFP_ATOMIC);
if (!pud)
return -ENOMEM;
next = start + PGDIR_SIZE;
if (next > end)
next = end;
if ((error = res_phys_pud_init(pud, __pa(start), __pa(next))))
return error;
set_pgd(temp_level4_pgt + pgd_index(start),
mk_kernel_pgd(__pa(pud)));
}
return 0;
}
static int set_up_temporary_text_mapping(pgd_t *pgd)
{
pmd_t *pmd;
pud_t *pud;
p4d_t *p4d = NULL;
pgprot_t pgtable_prot = __pgprot(_KERNPG_TABLE);
pgprot_t pmd_text_prot = __pgprot(__PAGE_KERNEL_LARGE_EXEC);
/* Filter out unsupported __PAGE_KERNEL* bits: */
pgprot_val(pmd_text_prot) &= __default_kernel_pte_mask;
pgprot_val(pgtable_prot) &= __default_kernel_pte_mask;
/*
* The new mapping only has to cover the page containing the image
* kernel's entry point (jump_address_phys), because the switch over to
* it is carried out by relocated code running from a page allocated
* specifically for this purpose and covered by the identity mapping, so
* the temporary kernel text mapping is only needed for the final jump.
* Moreover, in that mapping the virtual address of the image kernel's
* entry point must be the same as its virtual address in the image
* kernel (restore_jump_address), so the image kernel's
* restore_registers() code doesn't find itself in a different area of
* the virtual address space after switching over to the original page
* tables used by the image kernel.
*/
if (pgtable_l5_enabled()) {
p4d = (p4d_t *)get_safe_page(GFP_ATOMIC);
if (!p4d)
return -ENOMEM;
}
pud = (pud_t *)get_safe_page(GFP_ATOMIC);
if (!pud)
return -ENOMEM;
pmd = (pmd_t *)get_safe_page(GFP_ATOMIC);
if (!pmd)
return -ENOMEM;
set_pmd(pmd + pmd_index(restore_jump_address),
__pmd((jump_address_phys & PMD_MASK) | pgprot_val(pmd_text_prot)));
set_pud(pud + pud_index(restore_jump_address),
__pud(__pa(pmd) | pgprot_val(pgtable_prot)));
if (p4d) {
p4d_t new_p4d = __p4d(__pa(pud) | pgprot_val(pgtable_prot));
pgd_t new_pgd = __pgd(__pa(p4d) | pgprot_val(pgtable_prot));
set_p4d(p4d + p4d_index(restore_jump_address), new_p4d);
set_pgd(pgd + pgd_index(restore_jump_address), new_pgd);
} else {
/* No p4d for 4-level paging: point the pgd to the pud page table */
pgd_t new_pgd = __pgd(__pa(pud) | pgprot_val(pgtable_prot));
set_pgd(pgd + pgd_index(restore_jump_address), new_pgd);
}
return 0;
}
static int set_up_temporary_mappings(void)
{
struct x86_mapping_info info = {
.alloc_pgt_page = alloc_pgt_page,
.pmd_flag = __PAGE_KERNEL_LARGE_EXEC,
.kernel_mapping = true,
};
unsigned long mstart, mend;
int result;
int i;
temp_level4_pgt = (pgd_t *)get_safe_page(GFP_ATOMIC);
if (!temp_level4_pgt)
return -ENOMEM;
/* It is safe to reuse the original kernel mapping */
set_pgd(temp_level4_pgt + pgd_index(__START_KERNEL_map),
init_level4_pgt[pgd_index(__START_KERNEL_map)]);
/* Set up the direct mapping from scratch */
for (i = 0; i < nr_pfn_mapped; i++) {
mstart = pfn_mapped[i].start << PAGE_SHIFT;
mend = pfn_mapped[i].end << PAGE_SHIFT;
result = kernel_ident_mapping_init(&info, temp_level4_pgt,
mstart, mend);
if (result)
return result;
}
return 0;
}
int swsusp_arch_resume(void)
{
int error;
/* We have got enough memory and from now on we cannot recover */
if ((error = set_up_temporary_mappings()))
return error;
relocated_restore_code = (void *)get_safe_page(GFP_ATOMIC);
if (!relocated_restore_code)
return -ENOMEM;
memcpy(relocated_restore_code, &core_restore_code,
&restore_registers - &core_restore_code);
restore_image();
return 0;
}
static int copy_pud(pgd_t *dst_pgdp, pgd_t *src_pgdp, unsigned long start,
unsigned long end)
{
pud_t *dst_pudp;
pud_t *src_pudp;
unsigned long next;
unsigned long addr = start;
if (pgd_none(READ_ONCE(*dst_pgdp))) {
dst_pudp = (pud_t *)get_safe_page(GFP_ATOMIC);
if (!dst_pudp)
return -ENOMEM;
pgd_populate(&init_mm, dst_pgdp, dst_pudp);
}
dst_pudp = pud_offset(dst_pgdp, start);
src_pudp = pud_offset(src_pgdp, start);
do {
pud_t pud = READ_ONCE(*src_pudp);
next = pud_addr_end(addr, end);
if (pud_none(pud))
continue;
if (pud_table(pud)) {
if (copy_pmd(dst_pudp, src_pudp, addr, next))
return -ENOMEM;
} else {
set_pud(dst_pudp,
__pud(pud_val(pud) & ~PMD_SECT_RDONLY));
}
} while (dst_pudp++, src_pudp++, addr = next, addr != end);
return 0;
}
asmlinkage int swsusp_arch_resume(void)
{
int error;
resume_pg_dir = (pgd_t *)get_safe_page(GFP_ATOMIC);
if (!resume_pg_dir)
return -ENOMEM;
resume_init_first_level_page_table(resume_pg_dir);
error = set_up_temporary_text_mapping(resume_pg_dir);
if (error)
return error;
error = resume_physical_mapping_init(resume_pg_dir);
if (error)
return error;
temp_pgt = __pa(resume_pg_dir);
error = relocate_restore_code();
if (error)
return error;
/* We have got enough memory and from now on we cannot recover */
restore_image();
return 0;
}
static int relocate_restore_code(void)
{
pgd_t *pgd;
pud_t *pud;
relocated_restore_code = get_safe_page(GFP_ATOMIC);
if (!relocated_restore_code)
return -ENOMEM;
memcpy((void *)relocated_restore_code, &core_restore_code, PAGE_SIZE);
/* Make the page containing the relocated code executable */
pgd = (pgd_t *)__va(read_cr3()) + pgd_index(relocated_restore_code);
pud = pud_offset(pgd, relocated_restore_code);
if (pud_large(*pud)) {
set_pud(pud, __pud(pud_val(*pud) & ~_PAGE_NX));
} else {
pmd_t *pmd = pmd_offset(pud, relocated_restore_code);
if (pmd_large(*pmd)) {
set_pmd(pmd, __pmd(pmd_val(*pmd) & ~_PAGE_NX));
} else {
pte_t *pte = pte_offset_kernel(pmd, relocated_restore_code);
set_pte(pte, __pte(pte_val(*pte) & ~_PAGE_NX));
}
}
__flush_tlb_all();
return 0;
}
static int copy_pmd(pud_t *dst_pud, pud_t *src_pud, unsigned long start,
unsigned long end)
{
pmd_t *src_pmd;
pmd_t *dst_pmd;
unsigned long next;
unsigned long addr = start;
if (pud_none(*dst_pud)) {
dst_pmd = (pmd_t *)get_safe_page(GFP_ATOMIC);
if (!dst_pmd)
return -ENOMEM;
pud_populate(&init_mm, dst_pud, dst_pmd);
}
dst_pmd = pmd_offset(dst_pud, start);
src_pmd = pmd_offset(src_pud, start);
do {
next = pmd_addr_end(addr, end);
if (pmd_none(*src_pmd))
continue;
if (pmd_table(*src_pmd)) {
if (copy_pte(dst_pmd, src_pmd, addr, next))
return -ENOMEM;
} else {
set_pmd(dst_pmd,
__pmd(pmd_val(*src_pmd) & ~PMD_SECT_RDONLY));
}
} while (dst_pmd++, src_pmd++, addr = next, addr != end);
return 0;
}
static int set_up_temporary_mappings(void)
{
struct x86_mapping_info info = {
.alloc_pgt_page = alloc_pgt_page,
.pmd_flag = __PAGE_KERNEL_LARGE_EXEC,
.offset = __PAGE_OFFSET,
};
unsigned long mstart, mend;
pgd_t *pgd;
int result;
int i;
pgd = (pgd_t *)get_safe_page(GFP_ATOMIC);
if (!pgd)
return -ENOMEM;
/* Prepare a temporary mapping for the kernel text */
result = set_up_temporary_text_mapping(pgd);
if (result)
return result;
/* Set up the direct mapping from scratch */
for (i = 0; i < nr_pfn_mapped; i++) {
mstart = pfn_mapped[i].start << PAGE_SHIFT;
mend = pfn_mapped[i].end << PAGE_SHIFT;
result = kernel_ident_mapping_init(&info, pgd, mstart, mend);
if (result)
return result;
}
temp_level4_pgt = __pa(pgd);
return 0;
}
static int res_phys_pud_init(pud_t *pud, unsigned long address, unsigned long end)
{
long i, j;
i = pud_index(address);
pud = pud + i;
for (; i < PTRS_PER_PUD; pud++, i++) {
unsigned long paddr;
pmd_t *pmd;
paddr = address + i*PUD_SIZE;
if (paddr >= end)
break;
pmd = (pmd_t *)get_safe_page(GFP_ATOMIC);
if (!pmd)
return -ENOMEM;
set_pud(pud, __pud(__pa(pmd) | _KERNPG_TABLE));
for (j = 0; j < PTRS_PER_PMD; pmd++, j++, paddr += PMD_SIZE) {
unsigned long pe;
if (paddr >= end)
break;
pe = __PAGE_KERNEL_LARGE_EXEC | paddr;
pe &= __supported_pte_mask;
set_pmd(pmd, __pmd(pe));
}
}
return 0;
}
static pte_t *resume_one_page_table_init(pmd_t *pmd)
{
if (pmd_none(*pmd)) {
pte_t *page_table = (pte_t *)get_safe_page(GFP_ATOMIC);
if (!page_table)
return NULL;
set_pmd(pmd, __pmd(__pa(page_table) | _PAGE_KERNEL_TABLE));
BUG_ON(page_table != pte_offset_kernel(pmd, 0));
return page_table;
}
return pte_offset_kernel(pmd, 0);
}
int swsusp_arch_resume(void)
{
int error;
resume_pg_dir = (pgd_t *)get_safe_page(GFP_ATOMIC);
if (!resume_pg_dir)
return -ENOMEM;
resume_init_first_level_page_table(resume_pg_dir);
error = resume_physical_mapping_init(resume_pg_dir);
if (error)
return error;
restore_image(resume_pg_dir, restore_pblist);
return 0;
}
int swsusp_arch_resume(void)
{
int error;
/* We have got enough memory and from now on we cannot recover */
if ((error = set_up_temporary_mappings()))
return error;
relocated_restore_code = (void *)get_safe_page(GFP_ATOMIC);
if (!relocated_restore_code)
return -ENOMEM;
memcpy(relocated_restore_code, &core_restore_code,
&restore_registers - &core_restore_code);
restore_image();
return 0;
}
int swsusp_arch_resume(void)
{
int error;
resume_pg_dir = (pgd_t *)get_safe_page(GFP_ATOMIC);
if (!resume_pg_dir)
return -ENOMEM;
resume_init_first_level_page_table(resume_pg_dir);
error = resume_physical_mapping_init(resume_pg_dir);
if (error)
return error;
/* We have got enough memory and from now on we cannot recover */
restore_image();
return 0;
}
static int copy_pte(pmd_t *dst_pmdp, pmd_t *src_pmdp, unsigned long start,
unsigned long end)
{
pte_t *src_ptep;
pte_t *dst_ptep;
unsigned long addr = start;
dst_ptep = (pte_t *)get_safe_page(GFP_ATOMIC);
if (!dst_ptep)
return -ENOMEM;
pmd_populate_kernel(&init_mm, dst_pmdp, dst_ptep);
dst_ptep = pte_offset_kernel(dst_pmdp, start);
src_ptep = pte_offset_kernel(src_pmdp, start);
do {
_copy_pte(dst_ptep, src_ptep, addr);
} while (dst_ptep++, src_ptep++, addr += PAGE_SIZE, addr != end);
return 0;
}
/*
* Create a middle page table on a resume-safe page and put a pointer to it in
* the given global directory entry. This only returns the gd entry
* in non-PAE compilation mode, since the middle layer is folded.
*/
static pmd_t *resume_one_md_table_init(pgd_t *pgd)
{
pud_t *pud;
pmd_t *pmd_table;
#ifdef CONFIG_X86_PAE
pmd_table = (pmd_t *)get_safe_page(GFP_ATOMIC);
if (!pmd_table)
return NULL;
set_pgd(pgd, __pgd(__pa(pmd_table) | _PAGE_PRESENT));
pud = pud_offset(pgd, 0);
BUG_ON(pmd_table != pmd_offset(pud, 0));
#else
pud = pud_offset(pgd, 0);
pmd_table = pmd_offset(pud, 0);
#endif
return pmd_table;
}
/*
* Setup then Resume from the hibernate image using swsusp_arch_suspend_exit().
*
* Memory allocated by get_safe_page() will be dealt with by the hibernate code,
* we don't need to free it here.
*/
int swsusp_arch_resume(void)
{
int rc = 0;
void *zero_page;
size_t exit_size;
pgd_t *tmp_pg_dir;
phys_addr_t phys_hibernate_exit;
void __noreturn (*hibernate_exit)(phys_addr_t, phys_addr_t, void *,
void *, phys_addr_t, phys_addr_t);
/*
* Restoring the memory image will overwrite the ttbr1 page tables.
* Create a second copy of just the linear map, and use this when
* restoring.
*/
tmp_pg_dir = (pgd_t *)get_safe_page(GFP_ATOMIC);
if (!tmp_pg_dir) {
pr_err("Failed to allocate memory for temporary page tables.\n");
rc = -ENOMEM;
goto out;
}
rc = copy_page_tables(tmp_pg_dir, PAGE_OFFSET, 0);
if (rc)
goto out;
/*
* We need a zero page that is zero before & after resume in order to
* to break before make on the ttbr1 page tables.
*/
zero_page = (void *)get_safe_page(GFP_ATOMIC);
if (!zero_page) {
pr_err("Failed to allocate zero page.\n");
rc = -ENOMEM;
goto out;
}
/*
* Locate the exit code in the bottom-but-one page, so that *NULL
* still has disastrous affects.
*/
hibernate_exit = (void *)PAGE_SIZE;
exit_size = __hibernate_exit_text_end - __hibernate_exit_text_start;
/*
* Copy swsusp_arch_suspend_exit() to a safe page. This will generate
* a new set of ttbr0 page tables and load them.
*/
rc = create_safe_exec_page(__hibernate_exit_text_start, exit_size,
(unsigned long)hibernate_exit,
&phys_hibernate_exit,
(void *)get_safe_page, GFP_ATOMIC);
if (rc) {
pr_err("Failed to create safe executable page for hibernate_exit code.\n");
goto out;
}
/*
* The hibernate exit text contains a set of el2 vectors, that will
* be executed at el2 with the mmu off in order to reload hyp-stub.
*/
__flush_dcache_area(hibernate_exit, exit_size);
/*
* KASLR will cause the el2 vectors to be in a different location in
* the resumed kernel. Load hibernate's temporary copy into el2.
*
* We can skip this step if we booted at EL1, or are running with VHE.
*/
if (el2_reset_needed()) {
phys_addr_t el2_vectors = phys_hibernate_exit; /* base */
el2_vectors += hibernate_el2_vectors -
__hibernate_exit_text_start; /* offset */
__hyp_set_vectors(el2_vectors);
}
hibernate_exit(virt_to_phys(tmp_pg_dir), resume_hdr.ttbr1_el1,
resume_hdr.reenter_kernel, restore_pblist,
resume_hdr.__hyp_stub_vectors, virt_to_phys(zero_page));
out:
return rc;
}
static void *alloc_pgt_page(void *context)
{
return (void *)get_safe_page(GFP_ATOMIC);
}
