sh: romImage support V2
[linux-2.6.git] / arch / powerpc / lib / code-patching.c
1 /*
2  *  Copyright 2008 Michael Ellerman, IBM Corporation.
3  *
4  *  This program is free software; you can redistribute it and/or
5  *  modify it under the terms of the GNU General Public License
6  *  as published by the Free Software Foundation; either version
7  *  2 of the License, or (at your option) any later version.
8  */
9
10 #include <linux/kernel.h>
11 #include <linux/vmalloc.h>
12 #include <linux/init.h>
13 #include <linux/mm.h>
14 #include <asm/page.h>
15 #include <asm/code-patching.h>
16
17
18 void patch_instruction(unsigned int *addr, unsigned int instr)
19 {
20         *addr = instr;
21         asm ("dcbst 0, %0; sync; icbi 0,%0; sync; isync" : : "r" (addr));
22 }
23
24 void patch_branch(unsigned int *addr, unsigned long target, int flags)
25 {
26         patch_instruction(addr, create_branch(addr, target, flags));
27 }
28
29 unsigned int create_branch(const unsigned int *addr,
30                            unsigned long target, int flags)
31 {
32         unsigned int instruction;
33         long offset;
34
35         offset = target;
36         if (! (flags & BRANCH_ABSOLUTE))
37                 offset = offset - (unsigned long)addr;
38
39         /* Check we can represent the target in the instruction format */
40         if (offset < -0x2000000 || offset > 0x1fffffc || offset & 0x3)
41                 return 0;
42
43         /* Mask out the flags and target, so they don't step on each other. */
44         instruction = 0x48000000 | (flags & 0x3) | (offset & 0x03FFFFFC);
45
46         return instruction;
47 }
48
49 unsigned int create_cond_branch(const unsigned int *addr,
50                                 unsigned long target, int flags)
51 {
52         unsigned int instruction;
53         long offset;
54
55         offset = target;
56         if (! (flags & BRANCH_ABSOLUTE))
57                 offset = offset - (unsigned long)addr;
58
59         /* Check we can represent the target in the instruction format */
60         if (offset < -0x8000 || offset > 0x7FFF || offset & 0x3)
61                 return 0;
62
63         /* Mask out the flags and target, so they don't step on each other. */
64         instruction = 0x40000000 | (flags & 0x3FF0003) | (offset & 0xFFFC);
65
66         return instruction;
67 }
68
69 static unsigned int branch_opcode(unsigned int instr)
70 {
71         return (instr >> 26) & 0x3F;
72 }
73
74 static int instr_is_branch_iform(unsigned int instr)
75 {
76         return branch_opcode(instr) == 18;
77 }
78
79 static int instr_is_branch_bform(unsigned int instr)
80 {
81         return branch_opcode(instr) == 16;
82 }
83
84 int instr_is_relative_branch(unsigned int instr)
85 {
86         if (instr & BRANCH_ABSOLUTE)
87                 return 0;
88
89         return instr_is_branch_iform(instr) || instr_is_branch_bform(instr);
90 }
91
92 static unsigned long branch_iform_target(const unsigned int *instr)
93 {
94         signed long imm;
95
96         imm = *instr & 0x3FFFFFC;
97
98         /* If the top bit of the immediate value is set this is negative */
99         if (imm & 0x2000000)
100                 imm -= 0x4000000;
101
102         if ((*instr & BRANCH_ABSOLUTE) == 0)
103                 imm += (unsigned long)instr;
104
105         return (unsigned long)imm;
106 }
107
108 static unsigned long branch_bform_target(const unsigned int *instr)
109 {
110         signed long imm;
111
112         imm = *instr & 0xFFFC;
113
114         /* If the top bit of the immediate value is set this is negative */
115         if (imm & 0x8000)
116                 imm -= 0x10000;
117
118         if ((*instr & BRANCH_ABSOLUTE) == 0)
119                 imm += (unsigned long)instr;
120
121         return (unsigned long)imm;
122 }
123
124 unsigned long branch_target(const unsigned int *instr)
125 {
126         if (instr_is_branch_iform(*instr))
127                 return branch_iform_target(instr);
128         else if (instr_is_branch_bform(*instr))
129                 return branch_bform_target(instr);
130
131         return 0;
132 }
133
134 int instr_is_branch_to_addr(const unsigned int *instr, unsigned long addr)
135 {
136         if (instr_is_branch_iform(*instr) || instr_is_branch_bform(*instr))
137                 return branch_target(instr) == addr;
138
139         return 0;
140 }
141
142 unsigned int translate_branch(const unsigned int *dest, const unsigned int *src)
143 {
144         unsigned long target;
145
146         target = branch_target(src);
147
148         if (instr_is_branch_iform(*src))
149                 return create_branch(dest, target, *src);
150         else if (instr_is_branch_bform(*src))
151                 return create_cond_branch(dest, target, *src);
152
153         return 0;
154 }
155
156
157 #ifdef CONFIG_CODE_PATCHING_SELFTEST
158
159 static void __init test_trampoline(void)
160 {
161         asm ("nop;\n");
162 }
163
164 #define check(x)        \
165         if (!(x)) printk("code-patching: test failed at line %d\n", __LINE__);
166
167 static void __init test_branch_iform(void)
168 {
169         unsigned int instr;
170         unsigned long addr;
171
172         addr = (unsigned long)&instr;
173
174         /* The simplest case, branch to self, no flags */
175         check(instr_is_branch_iform(0x48000000));
176         /* All bits of target set, and flags */
177         check(instr_is_branch_iform(0x4bffffff));
178         /* High bit of opcode set, which is wrong */
179         check(!instr_is_branch_iform(0xcbffffff));
180         /* Middle bits of opcode set, which is wrong */
181         check(!instr_is_branch_iform(0x7bffffff));
182
183         /* Simplest case, branch to self with link */
184         check(instr_is_branch_iform(0x48000001));
185         /* All bits of targets set */
186         check(instr_is_branch_iform(0x4bfffffd));
187         /* Some bits of targets set */
188         check(instr_is_branch_iform(0x4bff00fd));
189         /* Must be a valid branch to start with */
190         check(!instr_is_branch_iform(0x7bfffffd));
191
192         /* Absolute branch to 0x100 */
193         instr = 0x48000103;
194         check(instr_is_branch_to_addr(&instr, 0x100));
195         /* Absolute branch to 0x420fc */
196         instr = 0x480420ff;
197         check(instr_is_branch_to_addr(&instr, 0x420fc));
198         /* Maximum positive relative branch, + 20MB - 4B */
199         instr = 0x49fffffc;
200         check(instr_is_branch_to_addr(&instr, addr + 0x1FFFFFC));
201         /* Smallest negative relative branch, - 4B */
202         instr = 0x4bfffffc;
203         check(instr_is_branch_to_addr(&instr, addr - 4));
204         /* Largest negative relative branch, - 32 MB */
205         instr = 0x4a000000;
206         check(instr_is_branch_to_addr(&instr, addr - 0x2000000));
207
208         /* Branch to self, with link */
209         instr = create_branch(&instr, addr, BRANCH_SET_LINK);
210         check(instr_is_branch_to_addr(&instr, addr));
211
212         /* Branch to self - 0x100, with link */
213         instr = create_branch(&instr, addr - 0x100, BRANCH_SET_LINK);
214         check(instr_is_branch_to_addr(&instr, addr - 0x100));
215
216         /* Branch to self + 0x100, no link */
217         instr = create_branch(&instr, addr + 0x100, 0);
218         check(instr_is_branch_to_addr(&instr, addr + 0x100));
219
220         /* Maximum relative negative offset, - 32 MB */
221         instr = create_branch(&instr, addr - 0x2000000, BRANCH_SET_LINK);
222         check(instr_is_branch_to_addr(&instr, addr - 0x2000000));
223
224         /* Out of range relative negative offset, - 32 MB + 4*/
225         instr = create_branch(&instr, addr - 0x2000004, BRANCH_SET_LINK);
226         check(instr == 0);
227
228         /* Out of range relative positive offset, + 32 MB */
229         instr = create_branch(&instr, addr + 0x2000000, BRANCH_SET_LINK);
230         check(instr == 0);
231
232         /* Unaligned target */
233         instr = create_branch(&instr, addr + 3, BRANCH_SET_LINK);
234         check(instr == 0);
235
236         /* Check flags are masked correctly */
237         instr = create_branch(&instr, addr, 0xFFFFFFFC);
238         check(instr_is_branch_to_addr(&instr, addr));
239         check(instr == 0x48000000);
240 }
241
242 static void __init test_create_function_call(void)
243 {
244         unsigned int *iptr;
245         unsigned long dest;
246
247         /* Check we can create a function call */
248         iptr = (unsigned int *)ppc_function_entry(test_trampoline);
249         dest = ppc_function_entry(test_create_function_call);
250         patch_instruction(iptr, create_branch(iptr, dest, BRANCH_SET_LINK));
251         check(instr_is_branch_to_addr(iptr, dest));
252 }
253
254 static void __init test_branch_bform(void)
255 {
256         unsigned long addr;
257         unsigned int *iptr, instr, flags;
258
259         iptr = &instr;
260         addr = (unsigned long)iptr;
261
262         /* The simplest case, branch to self, no flags */
263         check(instr_is_branch_bform(0x40000000));
264         /* All bits of target set, and flags */
265         check(instr_is_branch_bform(0x43ffffff));
266         /* High bit of opcode set, which is wrong */
267         check(!instr_is_branch_bform(0xc3ffffff));
268         /* Middle bits of opcode set, which is wrong */
269         check(!instr_is_branch_bform(0x7bffffff));
270
271         /* Absolute conditional branch to 0x100 */
272         instr = 0x43ff0103;
273         check(instr_is_branch_to_addr(&instr, 0x100));
274         /* Absolute conditional branch to 0x20fc */
275         instr = 0x43ff20ff;
276         check(instr_is_branch_to_addr(&instr, 0x20fc));
277         /* Maximum positive relative conditional branch, + 32 KB - 4B */
278         instr = 0x43ff7ffc;
279         check(instr_is_branch_to_addr(&instr, addr + 0x7FFC));
280         /* Smallest negative relative conditional branch, - 4B */
281         instr = 0x43fffffc;
282         check(instr_is_branch_to_addr(&instr, addr - 4));
283         /* Largest negative relative conditional branch, - 32 KB */
284         instr = 0x43ff8000;
285         check(instr_is_branch_to_addr(&instr, addr - 0x8000));
286
287         /* All condition code bits set & link */
288         flags = 0x3ff000 | BRANCH_SET_LINK;
289
290         /* Branch to self */
291         instr = create_cond_branch(iptr, addr, flags);
292         check(instr_is_branch_to_addr(&instr, addr));
293
294         /* Branch to self - 0x100 */
295         instr = create_cond_branch(iptr, addr - 0x100, flags);
296         check(instr_is_branch_to_addr(&instr, addr - 0x100));
297
298         /* Branch to self + 0x100 */
299         instr = create_cond_branch(iptr, addr + 0x100, flags);
300         check(instr_is_branch_to_addr(&instr, addr + 0x100));
301
302         /* Maximum relative negative offset, - 32 KB */
303         instr = create_cond_branch(iptr, addr - 0x8000, flags);
304         check(instr_is_branch_to_addr(&instr, addr - 0x8000));
305
306         /* Out of range relative negative offset, - 32 KB + 4*/
307         instr = create_cond_branch(iptr, addr - 0x8004, flags);
308         check(instr == 0);
309
310         /* Out of range relative positive offset, + 32 KB */
311         instr = create_cond_branch(iptr, addr + 0x8000, flags);
312         check(instr == 0);
313
314         /* Unaligned target */
315         instr = create_cond_branch(iptr, addr + 3, flags);
316         check(instr == 0);
317
318         /* Check flags are masked correctly */
319         instr = create_cond_branch(iptr, addr, 0xFFFFFFFC);
320         check(instr_is_branch_to_addr(&instr, addr));
321         check(instr == 0x43FF0000);
322 }
323
324 static void __init test_translate_branch(void)
325 {
326         unsigned long addr;
327         unsigned int *p, *q;
328         void *buf;
329
330         buf = vmalloc(PAGE_ALIGN(0x2000000 + 1));
331         check(buf);
332         if (!buf)
333                 return;
334
335         /* Simple case, branch to self moved a little */
336         p = buf;
337         addr = (unsigned long)p;
338         patch_branch(p, addr, 0);
339         check(instr_is_branch_to_addr(p, addr));
340         q = p + 1;
341         patch_instruction(q, translate_branch(q, p));
342         check(instr_is_branch_to_addr(q, addr));
343
344         /* Maximum negative case, move b . to addr + 32 MB */
345         p = buf;
346         addr = (unsigned long)p;
347         patch_branch(p, addr, 0);
348         q = buf + 0x2000000;
349         patch_instruction(q, translate_branch(q, p));
350         check(instr_is_branch_to_addr(p, addr));
351         check(instr_is_branch_to_addr(q, addr));
352         check(*q == 0x4a000000);
353
354         /* Maximum positive case, move x to x - 32 MB + 4 */
355         p = buf + 0x2000000;
356         addr = (unsigned long)p;
357         patch_branch(p, addr, 0);
358         q = buf + 4;
359         patch_instruction(q, translate_branch(q, p));
360         check(instr_is_branch_to_addr(p, addr));
361         check(instr_is_branch_to_addr(q, addr));
362         check(*q == 0x49fffffc);
363
364         /* Jump to x + 16 MB moved to x + 20 MB */
365         p = buf;
366         addr = 0x1000000 + (unsigned long)buf;
367         patch_branch(p, addr, BRANCH_SET_LINK);
368         q = buf + 0x1400000;
369         patch_instruction(q, translate_branch(q, p));
370         check(instr_is_branch_to_addr(p, addr));
371         check(instr_is_branch_to_addr(q, addr));
372
373         /* Jump to x + 16 MB moved to x - 16 MB + 4 */
374         p = buf + 0x1000000;
375         addr = 0x2000000 + (unsigned long)buf;
376         patch_branch(p, addr, 0);
377         q = buf + 4;
378         patch_instruction(q, translate_branch(q, p));
379         check(instr_is_branch_to_addr(p, addr));
380         check(instr_is_branch_to_addr(q, addr));
381
382
383         /* Conditional branch tests */
384
385         /* Simple case, branch to self moved a little */
386         p = buf;
387         addr = (unsigned long)p;
388         patch_instruction(p, create_cond_branch(p, addr, 0));
389         check(instr_is_branch_to_addr(p, addr));
390         q = p + 1;
391         patch_instruction(q, translate_branch(q, p));
392         check(instr_is_branch_to_addr(q, addr));
393
394         /* Maximum negative case, move b . to addr + 32 KB */
395         p = buf;
396         addr = (unsigned long)p;
397         patch_instruction(p, create_cond_branch(p, addr, 0xFFFFFFFC));
398         q = buf + 0x8000;
399         patch_instruction(q, translate_branch(q, p));
400         check(instr_is_branch_to_addr(p, addr));
401         check(instr_is_branch_to_addr(q, addr));
402         check(*q == 0x43ff8000);
403
404         /* Maximum positive case, move x to x - 32 KB + 4 */
405         p = buf + 0x8000;
406         addr = (unsigned long)p;
407         patch_instruction(p, create_cond_branch(p, addr, 0xFFFFFFFC));
408         q = buf + 4;
409         patch_instruction(q, translate_branch(q, p));
410         check(instr_is_branch_to_addr(p, addr));
411         check(instr_is_branch_to_addr(q, addr));
412         check(*q == 0x43ff7ffc);
413
414         /* Jump to x + 12 KB moved to x + 20 KB */
415         p = buf;
416         addr = 0x3000 + (unsigned long)buf;
417         patch_instruction(p, create_cond_branch(p, addr, BRANCH_SET_LINK));
418         q = buf + 0x5000;
419         patch_instruction(q, translate_branch(q, p));
420         check(instr_is_branch_to_addr(p, addr));
421         check(instr_is_branch_to_addr(q, addr));
422
423         /* Jump to x + 8 KB moved to x - 8 KB + 4 */
424         p = buf + 0x2000;
425         addr = 0x4000 + (unsigned long)buf;
426         patch_instruction(p, create_cond_branch(p, addr, 0));
427         q = buf + 4;
428         patch_instruction(q, translate_branch(q, p));
429         check(instr_is_branch_to_addr(p, addr));
430         check(instr_is_branch_to_addr(q, addr));
431
432         /* Free the buffer we were using */
433         vfree(buf);
434 }
435
436 static int __init test_code_patching(void)
437 {
438         printk(KERN_DEBUG "Running code patching self-tests ...\n");
439
440         test_branch_iform();
441         test_branch_bform();
442         test_create_function_call();
443         test_translate_branch();
444
445         return 0;
446 }
447 late_initcall(test_code_patching);
448
449 #endif /* CONFIG_CODE_PATCHING_SELFTEST */