Commit | Line | Data |
---|---|---|
4a19138c SS |
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); | |
e1d8f62a | 165 | kfree(p2m_entry); |
4a19138c SS |
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); |