+#define MAX_PTE_ENTRIES 512
+
+static int pte_type(u64 *pte)
+{
+ return *pte & PTE_TYPE_MASK;
+}
+
+/* Returns the LSB number for a PTE on level <level> */
+static int level2shift(int level)
+{
+ /* Page is 12 bits wide, every level translates 9 bits */
+ return (12 + 9 * (3 - level));
+}
+
+static u64 *find_pte(u64 addr, int level)
+{
+ int start_level = 0;
+ u64 *pte;
+ u64 idx;
+ u64 va_bits;
+ int i;
+
+ debug("addr=%llx level=%d\n", addr, level);
+
+ get_tcr(0, NULL, &va_bits);
+ if (va_bits < 39)
+ start_level = 1;
+
+ if (level < start_level)
+ return NULL;
+
+ /* Walk through all page table levels to find our PTE */
+ pte = (u64*)gd->arch.tlb_addr;
+ for (i = start_level; i < 4; i++) {
+ idx = (addr >> level2shift(i)) & 0x1FF;
+ pte += idx;
+ debug("idx=%llx PTE %p at level %d: %llx\n", idx, pte, i, *pte);
+
+ /* Found it */
+ if (i == level)
+ return pte;
+ /* PTE is no table (either invalid or block), can't traverse */
+ if (pte_type(pte) != PTE_TYPE_TABLE)
+ return NULL;
+ /* Off to the next level */
+ pte = (u64*)(*pte & 0x0000fffffffff000ULL);
+ }
+
+ /* Should never reach here */
+ return NULL;
+}
+
+/* Returns and creates a new full table (512 entries) */
+static u64 *create_table(void)
+{
+ u64 *new_table = (u64*)gd->arch.tlb_fillptr;
+ u64 pt_len = MAX_PTE_ENTRIES * sizeof(u64);
+
+ /* Allocate MAX_PTE_ENTRIES pte entries */
+ gd->arch.tlb_fillptr += pt_len;
+
+ if (gd->arch.tlb_fillptr - gd->arch.tlb_addr > gd->arch.tlb_size)
+ panic("Insufficient RAM for page table: 0x%lx > 0x%lx. "
+ "Please increase the size in get_page_table_size()",
+ gd->arch.tlb_fillptr - gd->arch.tlb_addr,
+ gd->arch.tlb_size);
+
+ /* Mark all entries as invalid */
+ memset(new_table, 0, pt_len);
+
+ return new_table;
+}
+
+static void set_pte_table(u64 *pte, u64 *table)
+{
+ /* Point *pte to the new table */
+ debug("Setting %p to addr=%p\n", pte, table);
+ *pte = PTE_TYPE_TABLE | (ulong)table;
+}
+
+/* Splits a block PTE into table with subpages spanning the old block */
+static void split_block(u64 *pte, int level)
+{
+ u64 old_pte = *pte;
+ u64 *new_table;
+ u64 i = 0;
+ /* level describes the parent level, we need the child ones */
+ int levelshift = level2shift(level + 1);
+
+ if (pte_type(pte) != PTE_TYPE_BLOCK)
+ panic("PTE %p (%llx) is not a block. Some driver code wants to "
+ "modify dcache settings for an range not covered in "
+ "mem_map.", pte, old_pte);
+
+ new_table = create_table();
+ debug("Splitting pte %p (%llx) into %p\n", pte, old_pte, new_table);
+
+ for (i = 0; i < MAX_PTE_ENTRIES; i++) {
+ new_table[i] = old_pte | (i << levelshift);
+
+ /* Level 3 block PTEs have the table type */
+ if ((level + 1) == 3)
+ new_table[i] |= PTE_TYPE_TABLE;
+
+ debug("Setting new_table[%lld] = %llx\n", i, new_table[i]);
+ }
+
+ /* Set the new table into effect */
+ set_pte_table(pte, new_table);
+}
+
+/* Add one mm_region map entry to the page tables */
+static void add_map(struct mm_region *map)
+{
+ u64 *pte;
+ u64 virt = map->virt;
+ u64 phys = map->phys;
+ u64 size = map->size;
+ u64 attrs = map->attrs | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
+ u64 blocksize;
+ int level;
+ u64 *new_table;
+
+ while (size) {
+ pte = find_pte(virt, 0);
+ if (pte && (pte_type(pte) == PTE_TYPE_FAULT)) {
+ debug("Creating table for virt 0x%llx\n", virt);
+ new_table = create_table();
+ set_pte_table(pte, new_table);
+ }
+
+ for (level = 1; level < 4; level++) {
+ pte = find_pte(virt, level);
+ if (!pte)
+ panic("pte not found\n");
+
+ blocksize = 1ULL << level2shift(level);
+ debug("Checking if pte fits for virt=%llx size=%llx blocksize=%llx\n",
+ virt, size, blocksize);
+ if (size >= blocksize && !(virt & (blocksize - 1))) {
+ /* Page fits, create block PTE */
+ debug("Setting PTE %p to block virt=%llx\n",
+ pte, virt);
+ *pte = phys | attrs;
+ virt += blocksize;
+ phys += blocksize;
+ size -= blocksize;
+ break;
+ } else if (pte_type(pte) == PTE_TYPE_FAULT) {
+ /* Page doesn't fit, create subpages */
+ debug("Creating subtable for virt 0x%llx blksize=%llx\n",
+ virt, blocksize);
+ new_table = create_table();
+ set_pte_table(pte, new_table);
+ } else if (pte_type(pte) == PTE_TYPE_BLOCK) {
+ debug("Split block into subtable for virt 0x%llx blksize=0x%llx\n",
+ virt, blocksize);
+ split_block(pte, level);
+ }