Untitled


// Incoming pte_flags should be as if 4KB page (bit 7 is PAT bit)
void paging_map_physical(uint64_t phys_addr, uint64_t linear_base,
                         uint64_t length, uint64_t pte_flags)
{
    if (unlikely(low_bits(phys_addr, 12) != low_bits(linear_base, 12)))
    {
        printf("ERROR: "
                    "Impossible linear->physical mapping,"
                    " bits 11:0 of the physical and linear address"
                    " must be equal");
        return;
    }

    // -------------------------========---------------------------  //
    //                                                               //
    //                          4KB Only                             //
    //                                                               //
    //   <- phys_addr                        phys_addr + length ->   //
    //   <- linear_base                                            B //
    //                                                             C //
    //                                                             D //
    //                                                             E //
    // A <------------------------ r ----------------------------> F //
    // |                                                           | //
    // | L                                                         | //
    // | |                                                         | //
    // ↓ ↓                                                         ↓ //
    // +-----------------------------------------------------------+ //
    // |                        4K pg ...                          | //
    // +-----------------------------------------------------------+ //
    // ↑                                                           ↑ //
    // |                                                           | //
    // 4K                                                         4K //
    // \------------------------ alignment ------------------------/ //
    //                                                               //
    //                                                               //
    // --------------------------=======---------------------------  //
    //                                                               //
    //                           4KB/2MB                             //
    //                                                               //
    // r_up(A,21) -↘                                   ↙- r_dn(F,21) //
    //                                                 C             //
    //             ↙                                   D             //
    // A <-- r --> B <------------- s ---------------> E <-- v --> F //
    // |           |                                   |           | //
    // | L         |                                   |           | //
    // | |         |                                   |           | //
    // ↓ ↓         ↓                                   ↓           ↓ //
    // +-----------+-----------------------------------+-----------+ //
    // | 4K pg ... |             2M pg ...             | 4K pg ... | //
    // +-----------+-----------------------------------+-----------+ //
    // ↑           ↑                                   ↑           ↑ //
    // |           |                                   |           | //
    // 4K          2M                                  2M         4K //
    // \------------------------ alignment ------------------------/ //
    //                                                               //
    //                                                               //
    // ------------------------===========-------------------------  //
    //                                                               //
    //                         4KB/2MB/1GB                           //
    //                                                               //
    //                                                               //
    //              r_up(B,30)-↘           ↙-r_dn(E,30)              //
    //                         |           |                         //
    // A <-- r --> B <-- s --> C <-- t --> D <-- u --> E <-- v --> F //
    // |           |           |           |           |           | //
    // | L         |           |           |           |           | //
    // | |         |           |           |           |           | //
    // ↓ ↓         ↓           ↓           ↓           ↓           ↓ //
    // +-----------+-----------+-----------+-----------+-----------+ //
    // | 4K pgs... | 2M pgs... | 1G pgs... | 2M pgs... | 4K pgs... | //
    // +-----------+-----------+-----------+-----------+-----------+ //
    // ↑           ↑           ↑           ↑           ↑           ↑ //
    // |           |           |           |           |           | //
    // 4K          2M          1G          1G          2M         4K //
    // \------------------------ alignment ------------------------/ //
    //                                                               //
    //                                                               //
    // A is phys_addr rounded down to a 4KB boundary                 //
    // F is phys_addr + length rounded up to a 4KB boundary          //
    // B is A rounded up to a 2MB boundary                           //
    // C is B rounded up to a 1GB boundary                           //
    // E is F rounded down to a 1MB boundary                         //
    // D is E rounded down to a 1GB boundary                         //
    //                                                               //
    // r = B - A (4KB pages)                                         //
    // s = C - B (2MB pages)                                         //
    // t = D - C (1GB pages)                                         //
    // u = E - D (2MB pages)                                         //
    // v = F - E (4KB pages)                                         //
    //                                                               //
    // Usually, several regions are empty. When alignment permits,   //
    // only the largest page sizes will be used. It starts as a      //
    // single run of 4KB pages, then a run of 2MB regions is carved  //
    // out of it, eliminating some or all 4KB runs, then a run of    //
    // 1GB pages is carved out of it, eliminating some or all of the //
    // 2MB runs                                                      //

    // It's impossible to use a 1GB mapping if the low 30 bits of the
    // physical address and the linear address are not equal
    bool can_use_1G = low_bits(phys_addr, 30) == low_bits(linear_base, 30);

    // It's impossible to use a 2MB mapping if the low 21 bits of the
    // physical address and the linear address are not equal
    bool can_use_2M = low_bits(phys_addr, 21) == low_bits(linear_base, 21);

    uint64_t phys_end = phys_addr + length;

    uint64_t A, B, C, D, E, F, X, Y;

    // Start with a simple run of 4KB pages
    A = round_dn(phys_addr, 12);
    F = round_up(phys_end, 12);
    B = C = D = E = F;

    // Compute 2MB rounded boundaries for B,C/D/E
    X = round_up(A, 21);
    Y = round_dn(F, 21);

    if (X < Y && can_use_2M)
    {
        B = X;
        C = D = E = Y;
    }

    // Compute 1GB rounded boundaries for C,D
    X = round_up(X, 30);
    Y = round_dn(Y, 30);

    if (X < Y && can_use_1G)
    {
        C = X;
        D = Y;
    }

    int64_t r = B - A;  // 4KB
    int64_t s = C - B;  // 2MB
    int64_t t = D - C;  // 1GB
    int64_t u = E - D;  // 2MB
    int64_t v = F - E;  // 4KB

    int64_t phys_to_virt = linear_base - phys_addr;

    // 4KB page region
    if (r > 0)
        paging_map_physical_impl(A, A + phys_to_virt, r, pte_flags);

    // 2MB page region
    if (s > 0)
        paging_map_physical_impl(B, B + phys_to_virt, s, pte_flags);

    // 1GB page region
    if (t > 0)
        paging_map_physical_impl(C, C + phys_to_virt, t, pte_flags);

    // 2MB page region
    if (u > 0)
        paging_map_physical_impl(D, D + phys_to_virt, u, pte_flags);

    // 4KB page region
    if (v > 0)
        paging_map_physical_impl(E, E + phys_to_virt, v, pte_flags);
}

void paging_map_physical_impl(uint64_t phys_addr, uint64_t linear_base,
                              uint64_t length, uint64_t pte_flags)
{
    // Mask off bit 63:48
    linear_base &= 0xFFFFFFFFFFFF;

    // Make sure the flags don't set any address bits
    assert((pte_flags & PTE_ADDR) == 0);

    // Automatically infer the optimal page size
    uint64_t page_size;
    uint8_t log2_pagesize;

    // Try 1GB, 2MB, 4KB pages, select largest size that would work
    page_size = 1 << 30;
    for (log2_pagesize = 30; log2_pagesize > 12; log2_pagesize -= 9) {
        // If the physical address, virtual address,
        // and length are suitably aligned...
        if ((phys_addr & -page_size) == phys_addr &&
                (linear_base & -page_size) == linear_base &&
                (length & -page_size) == length) {
            // ...then use huge page

            // Move PAT bit over to PDPT/PD location
            pte_flags |= (unsigned) (!!(pte_flags & PTE_PAGESIZE)) << PTE_PAT_BIT;

            // Set PSE bit
            pte_flags |= PTE_PAGESIZE;

            break;
        }

        // Try the next smaller page size
        page_size >>= 9;
    }

    // Make sure the parameters are feasible
    assert((phys_addr & (page_size - 1)) == (linear_base & (page_size - 1)));

    // Page align and round length up to a multiple of the page size
    size_t misalignment = linear_base & (page_size - 1);
    linear_base -= misalignment;
    phys_addr -= misalignment;
    length += misalignment;
    length = (length + page_size - 1) & -page_size;

    uint64_t end = linear_base + length;

    pte_t *pte = NULL;

    for (uint64_t vaddr = linear_base; vaddr < end; vaddr += page_size) {
        // Calculate pte pointer at start
        // or when transitioning to another table
        if (!pte || ((vaddr & -(uint64_t)(1 << (log2_pagesize + 9))) == vaddr))
            pte = paging_find_pte(vaddr, log2_pagesize, true);

        *pte++ = phys_addr | pte_flags;
        phys_addr += page_size;
    }
}

uint32_t paging_root_addr()
{
    return (uintptr_t) new_pml4;
}

// Returns true if the CPU supports that leaf
// Hypervisor leaves are asinine and it always returns true for them
bool cpuid__(cpuid_t *output, uint32_t eax, uint32_t ecx)
{
    // 0x4000xxxx (hypervisor) leaves are utterly ridiculous
    if ((eax & 0xF0000000) != 0x40000000) {
        // Automatically check for support for the leaf
        if ((eax & 0x1FFFFFFF) != 0) {
            cpuid__(output, eax & 0xE0000000U, 0);
            if (output->eax < eax)
                return false;
        }
    }

    output->eax = eax;
    output->ecx = ecx;
    __asm__ __volatile__ (
        "cpuid"
        : "+a" (output->eax), "+c" (output->ecx)
        , "=d" (output->edx), "=b" (output->ebx)
        :
        : "memory"
    );

    return true;
}

bool cpu_has_global_pages()
{
    static int has;

    if (has)
        return has > 0;

    cpuid_t cpuinfo;
    has = (cpuid__(&cpuinfo, 1U, 0) &&
            (cpuinfo.edx & (1<<13)))
            ? 1 : -1;

    return has > 0;
}

static inline void cpu_page_directory_set(uintptr_t addr)
{
    __asm__ __volatile__ (
        "movq %[addr],%%cr3\n\t"
        :
        : [addr] "r" (addr)
        : "memory"
    );
}

// Incoming pte_flags should be as if 4KB page (bit 7 is PAT bit)
void paging_map_physical(uint64_t phys_addr, uint64_t linear_base,
                         uint64_t length, uint64_t pte_flags)
{
    if (unlikely(low_bits(phys_addr, 12) != low_bits(linear_base, 12)))
    {
        printf("ERROR: "
                    "Impossible linear->physical mapping,"
                    " bits 11:0 of the physical and linear address"
                    " must be equal");
        return;
    }

    // -------------------------========---------------------------  //
    //                                                               //
    //                          4KB Only                             //
    //                                                               //
    //   <- phys_addr                        phys_addr + length ->   //
    //   <- linear_base                                            B //
    //                                                             C //
    //                                                             D //
    //                                                             E //
    // A <------------------------ r ----------------------------> F //
    // |                                                           | //
    // | L                                                         | //
    // | |                                                         | //
    // ↓ ↓                                                         ↓ //
    // +-----------------------------------------------------------+ //
    // |                        4K pg ...                          | //
    // +-----------------------------------------------------------+ //
    // ↑                                                           ↑ //
    // |                                                           | //
    // 4K                                                         4K //
    // \------------------------ alignment ------------------------/ //
    //                                                               //
    //                                                               //
    // --------------------------=======---------------------------  //
    //                                                               //
    //                           4KB/2MB                             //
    //                                                               //
    // r_up(A,21) -↘                                   ↙- r_dn(F,21) //
    //                                                 C             //
    //             ↙                                   D             //
    // A <-- r --> B <------------- s ---------------> E <-- v --> F //
    // |           |                                   |           | //
    // | L         |                                   |           | //
    // | |         |                                   |           | //
    // ↓ ↓         ↓                                   ↓           ↓ //
    // +-----------+-----------------------------------+-----------+ //
    // | 4K pg ... |             2M pg ...             | 4K pg ... | //
    // +-----------+-----------------------------------+-----------+ //
    // ↑           ↑                                   ↑           ↑ //
    // |           |                                   |           | //
    // 4K          2M                                  2M         4K //
    // \------------------------ alignment ------------------------/ //
    //                                                               //
    //                                                               //
    // ------------------------===========-------------------------  //
    //                                                               //
    //                         4KB/2MB/1GB                           //
    //                                                               //
    //                                                               //
    //              r_up(B,30)-↘           ↙-r_dn(E,30)              //
    //                         |           |                         //
    // A <-- r --> B <-- s --> C <-- t --> D <-- u --> E <-- v --> F //
    // |           |           |           |           |           | //
    // | L         |           |           |           |           | //
    // | |         |           |           |           |           | //
    // ↓ ↓         ↓           ↓           ↓           ↓           ↓ //
    // +-----------+-----------+-----------+-----------+-----------+ //
    // | 4K pgs... | 2M pgs... | 1G pgs... | 2M pgs... | 4K pgs... | //
    // +-----------+-----------+-----------+-----------+-----------+ //
    // ↑           ↑           ↑           ↑           ↑           ↑ //
    // |           |           |           |           |           | //
    // 4K          2M          1G          1G          2M         4K //
    // \------------------------ alignment ------------------------/ //
    //                                                               //
    //                                                               //
    // A is phys_addr rounded down to a 4KB boundary                 //
    // F is phys_addr + length rounded up to a 4KB boundary          //
    // B is A rounded up to a 2MB boundary                           //
    // C is B rounded up to a 1GB boundary                           //
    // E is F rounded down to a 1MB boundary                         //
    // D is E rounded down to a 1GB boundary                         //
    //                                                               //
    // r = B - A (4KB pages)                                         //
    // s = C - B (2MB pages)                                         //
    // t = D - C (1GB pages)                                         //
    // u = E - D (2MB pages)                                         //
    // v = F - E (4KB pages)                                         //
    //                                                               //
    // Usually, several regions are empty. When alignment permits,   //
    // only the largest page sizes will be used. It starts as a      //
    // single run of 4KB pages, then a run of 2MB regions is carved  //
    // out of it, eliminating some or all 4KB runs, then a run of    //
    // 1GB pages is carved out of it, eliminating some or all of the //
    // 2MB runs                                                      //

    // It's impossible to use a 1GB mapping if the low 30 bits of the
    // physical address and the linear address are not equal
    bool can_use_1G = low_bits(phys_addr, 30) == low_bits(linear_base, 30);

    // It's impossible to use a 2MB mapping if the low 21 bits of the
    // physical address and the linear address are not equal
    bool can_use_2M = low_bits(phys_addr, 21) == low_bits(linear_base, 21);

    uint64_t phys_end = phys_addr + length;

    uint64_t A, B, C, D, E, F, X, Y;

    // Start with a simple run of 4KB pages
    A = round_dn(phys_addr, 12);
    F = round_up(phys_end, 12);
    B = C = D = E = F;

    // Compute 2MB rounded boundaries for B,C/D/E
    X = round_up(A, 21);
    Y = round_dn(F, 21);

    if (X < Y && can_use_2M)
    {
        B = X;
        C = D = E = Y;
    }

    // Compute 1GB rounded boundaries for C,D
    X = round_up(X, 30);
    Y = round_dn(Y, 30);

    if (X < Y && can_use_1G)
    {
        C = X;
        D = Y;
    }

    int64_t r = B - A;  // 4KB
    int64_t s = C - B;  // 2MB
    int64_t t = D - C;  // 1GB
    int64_t u = E - D;  // 2MB
    int64_t v = F - E;  // 4KB

    int64_t phys_to_virt = linear_base - phys_addr;

    // 4KB page region
    if (r > 0)
        paging_map_physical_impl(A, A + phys_to_virt, r, pte_flags);

    // 2MB page region
    if (s > 0)
        paging_map_physical_impl(B, B + phys_to_virt, s, pte_flags);

    // 1GB page region
    if (t > 0)
        paging_map_physical_impl(C, C + phys_to_virt, t, pte_flags);

    // 2MB page region
    if (u > 0)
        paging_map_physical_impl(D, D + phys_to_virt, u, pte_flags);

    // 4KB page region
    if (v > 0)
        paging_map_physical_impl(E, E + phys_to_virt, v, pte_flags);
}

void paging_map_physical_impl(uint64_t phys_addr, uint64_t linear_base,
                              uint64_t length, uint64_t pte_flags)
{
    // Mask off bit 63:48
    linear_base &= 0xFFFFFFFFFFFF;

    // Make sure the flags don't set any address bits
    assert((pte_flags & PTE_ADDR) == 0);

    // Automatically infer the optimal page size
    uint64_t page_size;
    uint8_t log2_pagesize;

    // Try 1GB, 2MB, 4KB pages, select largest size that would work
    page_size = 1 << 30;
    for (log2_pagesize = 30; log2_pagesize > 12; log2_pagesize -= 9) {
        // If the physical address, virtual address,
        // and length are suitably aligned...
        if ((phys_addr & -page_size) == phys_addr &&
                (linear_base & -page_size) == linear_base &&
                (length & -page_size) == length) {
            // ...then use huge page

            // Move PAT bit over to PDPT/PD location
            pte_flags |= (unsigned) (!!(pte_flags & PTE_PAGESIZE)) << PTE_PAT_BIT;

            // Set PSE bit
            pte_flags |= PTE_PAGESIZE;

            break;
        }

        // Try the next smaller page size
        page_size >>= 9;
    }

    // Make sure the parameters are feasible
    assert((phys_addr & (page_size - 1)) == (linear_base & (page_size - 1)));

    // Page align and round length up to a multiple of the page size
    size_t misalignment = linear_base & (page_size - 1);
    linear_base -= misalignment;
    phys_addr -= misalignment;
    length += misalignment;
    length = (length + page_size - 1) & -page_size;

    uint64_t end = linear_base + length;

    pte_t *pte = NULL;

    for (uint64_t vaddr = linear_base; vaddr < end; vaddr += page_size) {
        // Calculate pte pointer at start
        // or when transitioning to another table
        if (!pte || ((vaddr & -(uint64_t)(1 << (log2_pagesize + 9))) == vaddr))
            pte = paging_find_pte(vaddr, log2_pagesize, true);

        *pte++ = phys_addr | pte_flags;
        phys_addr += page_size;
    }
}

uint32_t paging_root_addr()
{
    return (uintptr_t) new_pml4;
}

// Returns true if the CPU supports that leaf
// Hypervisor leaves are asinine and it always returns true for them
bool cpuid__(cpuid_t *output, uint32_t eax, uint32_t ecx)
{
    // 0x4000xxxx (hypervisor) leaves are utterly ridiculous
    if ((eax & 0xF0000000) != 0x40000000) {
        // Automatically check for support for the leaf
        if ((eax & 0x1FFFFFFF) != 0) {
            cpuid__(output, eax & 0xE0000000U, 0);
            if (output->eax < eax)
                return false;
        }
    }

    output->eax = eax;
    output->ecx = ecx;
    __asm__ __volatile__ (
        "cpuid"
        : "+a" (output->eax), "+c" (output->ecx)
        , "=d" (output->edx), "=b" (output->ebx)
        :
        : "memory"
    );

    return true;
}

bool cpu_has_global_pages()
{
    static int has;

    if (has)
        return has > 0;

    cpuid_t cpuinfo;
    has = (cpuid__(&cpuinfo, 1U, 0) &&
            (cpuinfo.edx & (1<<13)))
            ? 1 : -1;

    return has > 0;
}

static inline void cpu_page_directory_set(uintptr_t addr)
{
    __asm__ __volatile__ (
        "movq %[addr],%%cr3\n\t"
        :
        : [addr] "r" (addr)
        : "memory"
    );
}

void paging_init()
{
    paging_map_physical(framebuffer, 0xffffff8000000000ull, 4*boot_info.vres*boot_info.hres,
                        PTE_PRESENT | PTE_WRITABLE |
                        (-cpu_has_global_pages() & PTE_GLOBAL) |
                        PTE_PCD | PTE_PWT);
    cpu_page_directory_set(new_pml4);
    framebuffer = 0xffffff8000000000ull;
}