1 #include <linux/bootmem.h> 2 #include <linux/gfp.h> 3 #include <linux/export.h> 4 #include <linux/rwlock.h> 5 #include <linux/slab.h> 6 #include <linux/types.h> 7 #include <linux/dma-mapping.h> 8 #include <linux/vmalloc.h> 9 #include <linux/swiotlb.h> 10 11 #include <xen/xen.h> 12 #include <xen/interface/memory.h> 13 #include <xen/swiotlb-xen.h> 14 15 #include <asm/cacheflush.h> 16 #include <asm/xen/page.h> 17 #include <asm/xen/hypercall.h> 18 #include <asm/xen/interface.h> 19 20 struct xen_p2m_entry { 21 unsigned long pfn; 22 unsigned long mfn; 23 unsigned long nr_pages; 24 struct rb_node rbnode_mach; 25 struct rb_node rbnode_phys; 26 }; 27 28 rwlock_t p2m_lock; 29 struct rb_root phys_to_mach = RB_ROOT; 30 static struct rb_root mach_to_phys = RB_ROOT; 31 32 static int xen_add_phys_to_mach_entry(struct xen_p2m_entry *new) 33 { 34 struct rb_node **link = &phys_to_mach.rb_node; 35 struct rb_node *parent = NULL; 36 struct xen_p2m_entry *entry; 37 int rc = 0; 38 39 while (*link) { 40 parent = *link; 41 entry = rb_entry(parent, struct xen_p2m_entry, rbnode_phys); 42 43 if (new->mfn == entry->mfn) 44 goto err_out; 45 if (new->pfn == entry->pfn) 46 goto err_out; 47 48 if (new->pfn < entry->pfn) 49 link = &(*link)->rb_left; 50 else 51 link = &(*link)->rb_right; 52 } 53 rb_link_node(&new->rbnode_phys, parent, link); 54 rb_insert_color(&new->rbnode_phys, &phys_to_mach); 55 goto out; 56 57 err_out: 58 rc = -EINVAL; 59 pr_warn("%s: cannot add pfn=%pa -> mfn=%pa: pfn=%pa -> mfn=%pa already exists\n", 60 __func__, &new->pfn, &new->mfn, &entry->pfn, &entry->mfn); 61 out: 62 return rc; 63 } 64 65 unsigned long __pfn_to_mfn(unsigned long pfn) 66 { 67 struct rb_node *n = phys_to_mach.rb_node; 68 struct xen_p2m_entry *entry; 69 unsigned long irqflags; 70 71 read_lock_irqsave(&p2m_lock, irqflags); 72 while (n) { 73 entry = rb_entry(n, struct xen_p2m_entry, rbnode_phys); 74 if (entry->pfn <= pfn && 75 entry->pfn + entry->nr_pages > pfn) { 76 read_unlock_irqrestore(&p2m_lock, irqflags); 77 return entry->mfn + (pfn - entry->pfn); 78 } 79 if (pfn < entry->pfn) 80 n = n->rb_left; 81 else 82 n = n->rb_right; 83 } 84 read_unlock_irqrestore(&p2m_lock, irqflags); 85 86 return INVALID_P2M_ENTRY; 87 } 88 EXPORT_SYMBOL_GPL(__pfn_to_mfn); 89 90 static int xen_add_mach_to_phys_entry(struct xen_p2m_entry *new) 91 { 92 struct rb_node **link = &mach_to_phys.rb_node; 93 struct rb_node *parent = NULL; 94 struct xen_p2m_entry *entry; 95 int rc = 0; 96 97 while (*link) { 98 parent = *link; 99 entry = rb_entry(parent, struct xen_p2m_entry, rbnode_mach); 100 101 if (new->mfn == entry->mfn) 102 goto err_out; 103 if (new->pfn == entry->pfn) 104 goto err_out; 105 106 if (new->mfn < entry->mfn) 107 link = &(*link)->rb_left; 108 else 109 link = &(*link)->rb_right; 110 } 111 rb_link_node(&new->rbnode_mach, parent, link); 112 rb_insert_color(&new->rbnode_mach, &mach_to_phys); 113 goto out; 114 115 err_out: 116 rc = -EINVAL; 117 pr_warn("%s: cannot add pfn=%pa -> mfn=%pa: pfn=%pa -> mfn=%pa already exists\n", 118 __func__, &new->pfn, &new->mfn, &entry->pfn, &entry->mfn); 119 out: 120 return rc; 121 } 122 123 unsigned long __mfn_to_pfn(unsigned long mfn) 124 { 125 struct rb_node *n = mach_to_phys.rb_node; 126 struct xen_p2m_entry *entry; 127 unsigned long irqflags; 128 129 read_lock_irqsave(&p2m_lock, irqflags); 130 while (n) { 131 entry = rb_entry(n, struct xen_p2m_entry, rbnode_mach); 132 if (entry->mfn <= mfn && 133 entry->mfn + entry->nr_pages > mfn) { 134 read_unlock_irqrestore(&p2m_lock, irqflags); 135 return entry->pfn + (mfn - entry->mfn); 136 } 137 if (mfn < entry->mfn) 138 n = n->rb_left; 139 else 140 n = n->rb_right; 141 } 142 read_unlock_irqrestore(&p2m_lock, irqflags); 143 144 return INVALID_P2M_ENTRY; 145 } 146 EXPORT_SYMBOL_GPL(__mfn_to_pfn); 147 148 bool __set_phys_to_machine_multi(unsigned long pfn, 149 unsigned long mfn, unsigned long nr_pages) 150 { 151 int rc; 152 unsigned long irqflags; 153 struct xen_p2m_entry *p2m_entry; 154 struct rb_node *n = phys_to_mach.rb_node; 155 156 if (mfn == INVALID_P2M_ENTRY) { 157 write_lock_irqsave(&p2m_lock, irqflags); 158 while (n) { 159 p2m_entry = rb_entry(n, struct xen_p2m_entry, rbnode_phys); 160 if (p2m_entry->pfn <= pfn && 161 p2m_entry->pfn + p2m_entry->nr_pages > pfn) { 162 rb_erase(&p2m_entry->rbnode_mach, &mach_to_phys); 163 rb_erase(&p2m_entry->rbnode_phys, &phys_to_mach); 164 write_unlock_irqrestore(&p2m_lock, irqflags); 165 kfree(p2m_entry); 166 return true; 167 } 168 if (pfn < p2m_entry->pfn) 169 n = n->rb_left; 170 else 171 n = n->rb_right; 172 } 173 write_unlock_irqrestore(&p2m_lock, irqflags); 174 return true; 175 } 176 177 p2m_entry = kzalloc(sizeof(struct xen_p2m_entry), GFP_NOWAIT); 178 if (!p2m_entry) { 179 pr_warn("cannot allocate xen_p2m_entry\n"); 180 return false; 181 } 182 p2m_entry->pfn = pfn; 183 p2m_entry->nr_pages = nr_pages; 184 p2m_entry->mfn = mfn; 185 186 write_lock_irqsave(&p2m_lock, irqflags); 187 if ((rc = xen_add_phys_to_mach_entry(p2m_entry) < 0) || 188 (rc = xen_add_mach_to_phys_entry(p2m_entry) < 0)) { 189 write_unlock_irqrestore(&p2m_lock, irqflags); 190 return false; 191 } 192 write_unlock_irqrestore(&p2m_lock, irqflags); 193 return true; 194 } 195 EXPORT_SYMBOL_GPL(__set_phys_to_machine_multi); 196 197 bool __set_phys_to_machine(unsigned long pfn, unsigned long mfn) 198 { 199 return __set_phys_to_machine_multi(pfn, mfn, 1); 200 } 201 EXPORT_SYMBOL_GPL(__set_phys_to_machine); 202 203 int p2m_init(void) 204 { 205 rwlock_init(&p2m_lock); 206 return 0; 207 } 208 arch_initcall(p2m_init); 209