/arch/unicore32/mm/alignment.c

https://bitbucket.org/ndreys/linux-sunxi · C · 523 lines · 404 code · 69 blank · 50 comment · 56 complexity · 83c7fef1b2246d01d5da7ade6eaeb34e MD5 · raw file 

    

  1. /*
    2. 

  2.  * linux/arch/unicore32/mm/alignment.c
    3. 

  3.  *
    4. 

  4.  * Code specific to PKUnity SoC and UniCore ISA
    5. 

  5.  *
    6. 

  6.  * Copyright (C) 2001-2010 GUAN Xue-tao
    7. 

  7.  *
    8. 

  8.  * This program is free software; you can redistribute it and/or modify
    9. 

  9.  * it under the terms of the GNU General Public License version 2 as
    10. 

  10.  * published by the Free Software Foundation.
    11. 

  11.  */
    12. 

  12. /*
    13. 

  13.  * TODO:
    14. 

  14.  *  FPU ldm/stm not handling
    15. 

  15.  */
    16. 

  16. #include <linux/compiler.h>
    17. 

  17. #include <linux/kernel.h>
    18. 

  18. #include <linux/errno.h>
    19. 

  19. #include <linux/string.h>
    20. 

  20. #include <linux/init.h>
    21. 

  21. #include <linux/sched.h>
    22. 

  22. #include <linux/uaccess.h>
    23. 

  23. 

  24. #include <asm/tlbflush.h>
    25. 

  25. #include <asm/unaligned.h>
    26. 

  26. 

  27. #define CODING_BITS(i)	(i & 0xe0000120)
    28. 

  28. 

  29. #define LDST_P_BIT(i)	(i & (1 << 28))	/* Preindex             */
    30. 

  30. #define LDST_U_BIT(i)	(i & (1 << 27))	/* Add offset           */
    31. 

  31. #define LDST_W_BIT(i)	(i & (1 << 25))	/* Writeback            */
    32. 

  32. #define LDST_L_BIT(i)	(i & (1 << 24))	/* Load                 */
    33. 

  33. 

  34. #define LDST_P_EQ_U(i)	((((i) ^ ((i) >> 1)) & (1 << 27)) == 0)
    35. 

  35. 

  36. #define LDSTH_I_BIT(i)	(i & (1 << 26))	/* half-word immed      */
    37. 

  37. #define LDM_S_BIT(i)	(i & (1 << 26))	/* write ASR from BSR */
    38. 

  38. #define LDM_H_BIT(i)	(i & (1 << 6))	/* select r0-r15 or r16-r31 */
    39. 

  39. 

  40. #define RN_BITS(i)	((i >> 19) & 31)	/* Rn                   */
    41. 

  41. #define RD_BITS(i)	((i >> 14) & 31)	/* Rd                   */
    42. 

  42. #define RM_BITS(i)	(i & 31)	/* Rm                   */
    43. 

  43. 

  44. #define REGMASK_BITS(i)	(((i & 0x7fe00) >> 3) | (i & 0x3f))
    45. 

  45. #define OFFSET_BITS(i)	(i & 0x03fff)
    46. 

  46. 

  47. #define SHIFT_BITS(i)	((i >> 9) & 0x1f)
    48. 

  48. #define SHIFT_TYPE(i)	(i & 0xc0)
    49. 

  49. #define SHIFT_LSL	0x00
    50. 

  50. #define SHIFT_LSR	0x40
    51. 

  51. #define SHIFT_ASR	0x80
    52. 

  52. #define SHIFT_RORRRX	0xc0
    53. 

  53. 

  54. union offset_union {
    55. 

  55. 	unsigned long un;
    56. 

  56. 	signed long sn;
    57. 

  57. };
    58. 

  58. 

  59. #define TYPE_ERROR	0
    60. 

  60. #define TYPE_FAULT	1
    61. 

  61. #define TYPE_LDST	2
    62. 

  62. #define TYPE_DONE	3
    63. 

  63. #define TYPE_SWAP  4
    64. 

  64. #define TYPE_COLS  5		/* Coprocessor load/store */
    65. 

  65. 

  66. #define get8_unaligned_check(val, addr, err)		\
    67. 

  67. 	__asm__(					\
    68. 

  68. 	"1:	ldb.u	%1, [%2], #1\n"			\
    69. 

  69. 	"2:\n"						\
    70. 

  70. 	"	.pushsection .fixup,\"ax\"\n"		\
    71. 

  71. 	"	.align	2\n"				\
    72. 

  72. 	"3:	mov	%0, #1\n"			\
    73. 

  73. 	"	b	2b\n"				\
    74. 

  74. 	"	.popsection\n"				\
    75. 

  75. 	"	.pushsection __ex_table,\"a\"\n"		\
    76. 

  76. 	"	.align	3\n"				\
    77. 

  77. 	"	.long	1b, 3b\n"			\
    78. 

  78. 	"	.popsection\n"				\
    79. 

  79. 	: "=r" (err), "=&r" (val), "=r" (addr)		\
    80. 

  80. 	: "0" (err), "2" (addr))
    81. 

  81. 

  82. #define get8t_unaligned_check(val, addr, err)		\
    83. 

  83. 	__asm__(					\
    84. 

  84. 	"1:	ldb.u	%1, [%2], #1\n"			\
    85. 

  85. 	"2:\n"						\
    86. 

  86. 	"	.pushsection .fixup,\"ax\"\n"		\
    87. 

  87. 	"	.align	2\n"				\
    88. 

  88. 	"3:	mov	%0, #1\n"			\
    89. 

  89. 	"	b	2b\n"				\
    90. 

  90. 	"	.popsection\n"				\
    91. 

  91. 	"	.pushsection __ex_table,\"a\"\n"		\
    92. 

  92. 	"	.align	3\n"				\
    93. 

  93. 	"	.long	1b, 3b\n"			\
    94. 

  94. 	"	.popsection\n"				\
    95. 

  95. 	: "=r" (err), "=&r" (val), "=r" (addr)		\
    96. 

  96. 	: "0" (err), "2" (addr))
    97. 

  97. 

  98. #define get16_unaligned_check(val, addr)			\
    99. 

  99. 	do {							\
    100. 

  100. 		unsigned int err = 0, v, a = addr;		\
    101. 

  101. 		get8_unaligned_check(val, a, err);		\
    102. 

  102. 		get8_unaligned_check(v, a, err);		\
    103. 

  103. 		val |= v << 8;					\
    104. 

  104. 		if (err)					\
    105. 

  105. 			goto fault;				\
    106. 

  106. 	} while (0)
    107. 

  107. 

  108. #define put16_unaligned_check(val, addr)			\
    109. 

  109. 	do {							\
    110. 

  110. 		unsigned int err = 0, v = val, a = addr;	\
    111. 

  111. 		__asm__(					\
    112. 

  112. 		"1:	stb.u	%1, [%2], #1\n"			\
    113. 

  113. 		"	mov	%1, %1 >> #8\n"			\
    114. 

  114. 		"2:	stb.u	%1, [%2]\n"			\
    115. 

  115. 		"3:\n"						\
    116. 

  116. 		"	.pushsection .fixup,\"ax\"\n"		\
    117. 

  117. 		"	.align	2\n"				\
    118. 

  118. 		"4:	mov	%0, #1\n"			\
    119. 

  119. 		"	b	3b\n"				\
    120. 

  120. 		"	.popsection\n"				\
    121. 

  121. 		"	.pushsection __ex_table,\"a\"\n"		\
    122. 

  122. 		"	.align	3\n"				\
    123. 

  123. 		"	.long	1b, 4b\n"			\
    124. 

  124. 		"	.long	2b, 4b\n"			\
    125. 

  125. 		"	.popsection\n"				\
    126. 

  126. 		: "=r" (err), "=&r" (v), "=&r" (a)		\
    127. 

  127. 		: "0" (err), "1" (v), "2" (a));			\
    128. 

  128. 		if (err)					\
    129. 

  129. 			goto fault;				\
    130. 

  130. 	} while (0)
    131. 

  131. 

  132. #define __put32_unaligned_check(ins, val, addr)			\
    133. 

  133. 	do {							\
    134. 

  134. 		unsigned int err = 0, v = val, a = addr;	\
    135. 

  135. 		__asm__(					\
    136. 

  136. 		"1:	"ins"	%1, [%2], #1\n"			\
    137. 

  137. 		"	mov	%1, %1 >> #8\n"			\
    138. 

  138. 		"2:	"ins"	%1, [%2], #1\n"			\
    139. 

  139. 		"	mov	%1, %1 >> #8\n"			\
    140. 

  140. 		"3:	"ins"	%1, [%2], #1\n"			\
    141. 

  141. 		"	mov	%1, %1 >> #8\n"			\
    142. 

  142. 		"4:	"ins"	%1, [%2]\n"			\
    143. 

  143. 		"5:\n"						\
    144. 

  144. 		"	.pushsection .fixup,\"ax\"\n"		\
    145. 

  145. 		"	.align	2\n"				\
    146. 

  146. 		"6:	mov	%0, #1\n"			\
    147. 

  147. 		"	b	5b\n"				\
    148. 

  148. 		"	.popsection\n"				\
    149. 

  149. 		"	.pushsection __ex_table,\"a\"\n"		\
    150. 

  150. 		"	.align	3\n"				\
    151. 

  151. 		"	.long	1b, 6b\n"			\
    152. 

  152. 		"	.long	2b, 6b\n"			\
    153. 

  153. 		"	.long	3b, 6b\n"			\
    154. 

  154. 		"	.long	4b, 6b\n"			\
    155. 

  155. 		"	.popsection\n"				\
    156. 

  156. 		: "=r" (err), "=&r" (v), "=&r" (a)		\
    157. 

  157. 		: "0" (err), "1" (v), "2" (a));			\
    158. 

  158. 		if (err)					\
    159. 

  159. 			goto fault;				\
    160. 

  160. 	} while (0)
    161. 

  161. 

  162. #define get32_unaligned_check(val, addr)			\
    163. 

  163. 	do {							\
    164. 

  164. 		unsigned int err = 0, v, a = addr;		\
    165. 

  165. 		get8_unaligned_check(val, a, err);		\
    166. 

  166. 		get8_unaligned_check(v, a, err);		\
    167. 

  167. 		val |= v << 8;					\
    168. 

  168. 		get8_unaligned_check(v, a, err);		\
    169. 

  169. 		val |= v << 16;					\
    170. 

  170. 		get8_unaligned_check(v, a, err);		\
    171. 

  171. 		val |= v << 24;					\
    172. 

  172. 		if (err)					\
    173. 

  173. 			goto fault;				\
    174. 

  174. 	} while (0)
    175. 

  175. 

  176. #define put32_unaligned_check(val, addr)			\
    177. 

  177. 	__put32_unaligned_check("stb.u", val, addr)
    178. 

  178. 

  179. #define get32t_unaligned_check(val, addr)			\
    180. 

  180. 	do {							\
    181. 

  181. 		unsigned int err = 0, v, a = addr;		\
    182. 

  182. 		get8t_unaligned_check(val, a, err);		\
    183. 

  183. 		get8t_unaligned_check(v, a, err);		\
    184. 

  184. 		val |= v << 8;					\
    185. 

  185. 		get8t_unaligned_check(v, a, err);		\
    186. 

  186. 		val |= v << 16;					\
    187. 

  187. 		get8t_unaligned_check(v, a, err);		\
    188. 

  188. 		val |= v << 24;					\
    189. 

  189. 		if (err)					\
    190. 

  190. 			goto fault;				\
    191. 

  191. 	} while (0)
    192. 

  192. 

  193. #define put32t_unaligned_check(val, addr)			\
    194. 

  194. 	__put32_unaligned_check("stb.u", val, addr)
    195. 

  195. 

  196. static void
    197. 

  197. do_alignment_finish_ldst(unsigned long addr, unsigned long instr,
    198. 

  198. 			 struct pt_regs *regs, union offset_union offset)
    199. 

  199. {
    200. 

  200. 	if (!LDST_U_BIT(instr))
    201. 

  201. 		offset.un = -offset.un;
    202. 

  202. 

  203. 	if (!LDST_P_BIT(instr))
    204. 

  204. 		addr += offset.un;
    205. 

  205. 

  206. 	if (!LDST_P_BIT(instr) || LDST_W_BIT(instr))
    207. 

  207. 		regs->uregs[RN_BITS(instr)] = addr;
    208. 

  208. }
    209. 

  209. 

  210. static int
    211. 

  211. do_alignment_ldrhstrh(unsigned long addr, unsigned long instr,
    212. 

  212. 		      struct pt_regs *regs)
    213. 

  213. {
    214. 

  214. 	unsigned int rd = RD_BITS(instr);
    215. 

  215. 

  216. 	/* old value 0x40002120, can't judge swap instr correctly */
    217. 

  217. 	if ((instr & 0x4b003fe0) == 0x40000120)
    218. 

  218. 		goto swp;
    219. 

  219. 

  220. 	if (LDST_L_BIT(instr)) {
    221. 

  221. 		unsigned long val;
    222. 

  222. 		get16_unaligned_check(val, addr);
    223. 

  223. 

  224. 		/* signed half-word? */
    225. 

  225. 		if (instr & 0x80)
    226. 

  226. 			val = (signed long)((signed short)val);
    227. 

  227. 

  228. 		regs->uregs[rd] = val;
    229. 

  229. 	} else
    230. 

  230. 		put16_unaligned_check(regs->uregs[rd], addr);
    231. 

  231. 

  232. 	return TYPE_LDST;
    233. 

  233. 

  234. swp:
    235. 

  235. 	/* only handle swap word
    236. 

  236. 	 * for swap byte should not active this alignment exception */
    237. 

  237. 	get32_unaligned_check(regs->uregs[RD_BITS(instr)], addr);
    238. 

  238. 	put32_unaligned_check(regs->uregs[RM_BITS(instr)], addr);
    239. 

  239. 	return TYPE_SWAP;
    240. 

  240. 

  241. fault:
    242. 

  242. 	return TYPE_FAULT;
    243. 

  243. }
    244. 

  244. 

  245. static int
    246. 

  246. do_alignment_ldrstr(unsigned long addr, unsigned long instr,
    247. 

  247. 		    struct pt_regs *regs)
    248. 

  248. {
    249. 

  249. 	unsigned int rd = RD_BITS(instr);
    250. 

  250. 

  251. 	if (!LDST_P_BIT(instr) && LDST_W_BIT(instr))
    252. 

  252. 		goto trans;
    253. 

  253. 

  254. 	if (LDST_L_BIT(instr))
    255. 

  255. 		get32_unaligned_check(regs->uregs[rd], addr);
    256. 

  256. 	else
    257. 

  257. 		put32_unaligned_check(regs->uregs[rd], addr);
    258. 

  258. 	return TYPE_LDST;
    259. 

  259. 

  260. trans:
    261. 

  261. 	if (LDST_L_BIT(instr))
    262. 

  262. 		get32t_unaligned_check(regs->uregs[rd], addr);
    263. 

  263. 	else
    264. 

  264. 		put32t_unaligned_check(regs->uregs[rd], addr);
    265. 

  265. 	return TYPE_LDST;
    266. 

  266. 

  267. fault:
    268. 

  268. 	return TYPE_FAULT;
    269. 

  269. }
    270. 

  270. 

  271. /*
    272. 

  272.  * LDM/STM alignment handler.
    273. 

  273.  *
    274. 

  274.  * There are 4 variants of this instruction:
    275. 

  275.  *
    276. 

  276.  * B = rn pointer before instruction, A = rn pointer after instruction
    277. 

  277.  *              ------ increasing address ----->
    278. 

  278.  *	        |    | r0 | r1 | ... | rx |    |
    279. 

  279.  * PU = 01             B                    A
    280. 

  280.  * PU = 11        B                    A
    281. 

  281.  * PU = 00        A                    B
    282. 

  282.  * PU = 10             A                    B
    283. 

  283.  */
    284. 

  284. static int
    285. 

  285. do_alignment_ldmstm(unsigned long addr, unsigned long instr,
    286. 

  286. 		    struct pt_regs *regs)
    287. 

  287. {
    288. 

  288. 	unsigned int rd, rn, pc_correction, reg_correction, nr_regs, regbits;
    289. 

  289. 	unsigned long eaddr, newaddr;
    290. 

  290. 

  291. 	if (LDM_S_BIT(instr))
    292. 

  292. 		goto bad;
    293. 

  293. 

  294. 	pc_correction = 4;	/* processor implementation defined */
    295. 

  295. 

  296. 	/* count the number of registers in the mask to be transferred */
    297. 

  297. 	nr_regs = hweight16(REGMASK_BITS(instr)) * 4;
    298. 

  298. 

  299. 	rn = RN_BITS(instr);
    300. 

  300. 	newaddr = eaddr = regs->uregs[rn];
    301. 

  301. 

  302. 	if (!LDST_U_BIT(instr))
    303. 

  303. 		nr_regs = -nr_regs;
    304. 

  304. 	newaddr += nr_regs;
    305. 

  305. 	if (!LDST_U_BIT(instr))
    306. 

  306. 		eaddr = newaddr;
    307. 

  307. 

  308. 	if (LDST_P_EQ_U(instr))	/* U = P */
    309. 

  309. 		eaddr += 4;
    310. 

  310. 

  311. 	/*
    312. 

  312. 	 * This is a "hint" - we already have eaddr worked out by the
    313. 

  313. 	 * processor for us.
    314. 

  314. 	 */
    315. 

  315. 	if (addr != eaddr) {
    316. 

  316. 		printk(KERN_ERR "LDMSTM: PC = %08lx, instr = %08lx, "
    317. 

  317. 		       "addr = %08lx, eaddr = %08lx\n",
    318. 

  318. 		       instruction_pointer(regs), instr, addr, eaddr);
    319. 

  319. 		show_regs(regs);
    320. 

  320. 	}
    321. 

  321. 

  322. 	if (LDM_H_BIT(instr))
    323. 

  323. 		reg_correction = 0x10;
    324. 

  324. 	else
    325. 

  325. 		reg_correction = 0x00;
    326. 

  326. 

  327. 	for (regbits = REGMASK_BITS(instr), rd = 0; regbits;
    328. 

  328. 	     regbits >>= 1, rd += 1)
    329. 

  329. 		if (regbits & 1) {
    330. 

  330. 			if (LDST_L_BIT(instr))
    331. 

  331. 				get32_unaligned_check(regs->
    332. 

  332. 					uregs[rd + reg_correction], eaddr);
    333. 

  333. 			else
    334. 

  334. 				put32_unaligned_check(regs->
    335. 

  335. 					uregs[rd + reg_correction], eaddr);
    336. 

  336. 			eaddr += 4;
    337. 

  337. 		}
    338. 

  338. 

  339. 	if (LDST_W_BIT(instr))
    340. 

  340. 		regs->uregs[rn] = newaddr;
    341. 

  341. 	return TYPE_DONE;
    342. 

  342. 

  343. fault:
    344. 

  344. 	regs->UCreg_pc -= pc_correction;
    345. 

  345. 	return TYPE_FAULT;
    346. 

  346. 

  347. bad:
    348. 

  348. 	printk(KERN_ERR "Alignment trap: not handling ldm with s-bit set\n");
    349. 

  349. 	return TYPE_ERROR;
    350. 

  350. }
    351. 

  351. 

  352. static int
    353. 

  353. do_alignment(unsigned long addr, unsigned int error_code, struct pt_regs *regs)
    354. 

  354. {
    355. 

  355. 	union offset_union offset;
    356. 

  356. 	unsigned long instr, instrptr;
    357. 

  357. 	int (*handler) (unsigned long addr, unsigned long instr,
    358. 

  358. 			struct pt_regs *regs);
    359. 

  359. 	unsigned int type;
    360. 

  360. 

  361. 	instrptr = instruction_pointer(regs);
    362. 

  362. 	if (instrptr >= PAGE_OFFSET)
    363. 

  363. 		instr = *(unsigned long *)instrptr;
    364. 

  364. 	else {
    365. 

  365. 		__asm__ __volatile__(
    366. 

  366. 				"ldw.u	%0, [%1]\n"
    367. 

  367. 				: "=&r"(instr)
    368. 

  368. 				: "r"(instrptr));
    369. 

  369. 	}
    370. 

  370. 

  371. 	regs->UCreg_pc += 4;
    372. 

  372. 

  373. 	switch (CODING_BITS(instr)) {
    374. 

  374. 	case 0x40000120:	/* ldrh or strh */
    375. 

  375. 		if (LDSTH_I_BIT(instr))
    376. 

  376. 			offset.un = (instr & 0x3e00) >> 4 | (instr & 31);
    377. 

  377. 		else
    378. 

  378. 			offset.un = regs->uregs[RM_BITS(instr)];
    379. 

  379. 		handler = do_alignment_ldrhstrh;
    380. 

  380. 		break;
    381. 

  381. 

  382. 	case 0x60000000:	/* ldr or str immediate */
    383. 

  383. 	case 0x60000100:	/* ldr or str immediate */
    384. 

  384. 	case 0x60000020:	/* ldr or str immediate */
    385. 

  385. 	case 0x60000120:	/* ldr or str immediate */
    386. 

  386. 		offset.un = OFFSET_BITS(instr);
    387. 

  387. 		handler = do_alignment_ldrstr;
    388. 

  388. 		break;
    389. 

  389. 

  390. 	case 0x40000000:	/* ldr or str register */
    391. 

  391. 		offset.un = regs->uregs[RM_BITS(instr)];
    392. 

  392. 		{
    393. 

  393. 			unsigned int shiftval = SHIFT_BITS(instr);
    394. 

  394. 

  395. 			switch (SHIFT_TYPE(instr)) {
    396. 

  396. 			case SHIFT_LSL:
    397. 

  397. 				offset.un <<= shiftval;
    398. 

  398. 				break;
    399. 

  399. 

  400. 			case SHIFT_LSR:
    401. 

  401. 				offset.un >>= shiftval;
    402. 

  402. 				break;
    403. 

  403. 

  404. 			case SHIFT_ASR:
    405. 

  405. 				offset.sn >>= shiftval;
    406. 

  406. 				break;
    407. 

  407. 

  408. 			case SHIFT_RORRRX:
    409. 

  409. 				if (shiftval == 0) {
    410. 

  410. 					offset.un >>= 1;
    411. 

  411. 					if (regs->UCreg_asr & PSR_C_BIT)
    412. 

  412. 						offset.un |= 1 << 31;
    413. 

  413. 				} else
    414. 

  414. 					offset.un = offset.un >> shiftval |
    415. 

  415. 					    offset.un << (32 - shiftval);
    416. 

  416. 				break;
    417. 

  417. 			}
    418. 

  418. 		}
    419. 

  419. 		handler = do_alignment_ldrstr;
    420. 

  420. 		break;
    421. 

  421. 

  422. 	case 0x80000000:	/* ldm or stm */
    423. 

  423. 	case 0x80000020:	/* ldm or stm */
    424. 

  424. 		handler = do_alignment_ldmstm;
    425. 

  425. 		break;
    426. 

  426. 

  427. 	default:
    428. 

  428. 		goto bad;
    429. 

  429. 	}
    430. 

  430. 

  431. 	type = handler(addr, instr, regs);
    432. 

  432. 

  433. 	if (type == TYPE_ERROR || type == TYPE_FAULT)
    434. 

  434. 		goto bad_or_fault;
    435. 

  435. 

  436. 	if (type == TYPE_LDST)
    437. 

  437. 		do_alignment_finish_ldst(addr, instr, regs, offset);
    438. 

  438. 

  439. 	return 0;
    440. 

  440. 

  441. bad_or_fault:
    442. 

  442. 	if (type == TYPE_ERROR)
    443. 

  443. 		goto bad;
    444. 

  444. 	regs->UCreg_pc -= 4;
    445. 

  445. 	/*
    446. 

  446. 	 * We got a fault - fix it up, or die.
    447. 

  447. 	 */
    448. 

  448. 	do_bad_area(addr, error_code, regs);
    449. 

  449. 	return 0;
    450. 

  450. 

  451. bad:
    452. 

  452. 	/*
    453. 

  453. 	 * Oops, we didn't handle the instruction.
    454. 

  454. 	 * However, we must handle fpu instr firstly.
    455. 

  455. 	 */
    456. 

  456. #ifdef CONFIG_UNICORE_FPU_F64
    457. 

  457. 	/* handle co.load/store */
    458. 

  458. #define CODING_COLS                0xc0000000
    459. 

  459. #define COLS_OFFSET_BITS(i)	(i & 0x1FF)
    460. 

  460. #define COLS_L_BITS(i)		(i & (1<<24))
    461. 

  461. #define COLS_FN_BITS(i)		((i>>14) & 31)
    462. 

  462. 	if ((instr & 0xe0000000) == CODING_COLS) {
    463. 

  463. 		unsigned int fn = COLS_FN_BITS(instr);
    464. 

  464. 		unsigned long val = 0;
    465. 

  465. 		if (COLS_L_BITS(instr)) {
    466. 

  466. 			get32t_unaligned_check(val, addr);
    467. 

  467. 			switch (fn) {
    468. 

  468. #define ASM_MTF(n)	case n:						\
    469. 

  469. 			__asm__ __volatile__("MTF %0, F" __stringify(n)	\
    470. 

  470. 				: : "r"(val));				\
    471. 

  471. 			break;
    472. 

  472. 			ASM_MTF(0); ASM_MTF(1); ASM_MTF(2); ASM_MTF(3);
    473. 

  473. 			ASM_MTF(4); ASM_MTF(5); ASM_MTF(6); ASM_MTF(7);
    474. 

  474. 			ASM_MTF(8); ASM_MTF(9); ASM_MTF(10); ASM_MTF(11);
    475. 

  475. 			ASM_MTF(12); ASM_MTF(13); ASM_MTF(14); ASM_MTF(15);
    476. 

  476. 			ASM_MTF(16); ASM_MTF(17); ASM_MTF(18); ASM_MTF(19);
    477. 

  477. 			ASM_MTF(20); ASM_MTF(21); ASM_MTF(22); ASM_MTF(23);
    478. 

  478. 			ASM_MTF(24); ASM_MTF(25); ASM_MTF(26); ASM_MTF(27);
    479. 

  479. 			ASM_MTF(28); ASM_MTF(29); ASM_MTF(30); ASM_MTF(31);
    480. 

  480. #undef ASM_MTF
    481. 

  481. 			}
    482. 

  482. 		} else {
    483. 

  483. 			switch (fn) {
    484. 

  484. #define ASM_MFF(n)	case n:						\
    485. 

  485. 			__asm__ __volatile__("MFF %0, F" __stringify(n)	\
    486. 

  486. 				: : "r"(val));				\
    487. 

  487. 			break;
    488. 

  488. 			ASM_MFF(0); ASM_MFF(1); ASM_MFF(2); ASM_MFF(3);
    489. 

  489. 			ASM_MFF(4); ASM_MFF(5); ASM_MFF(6); ASM_MFF(7);
    490. 

  490. 			ASM_MFF(8); ASM_MFF(9); ASM_MFF(10); ASM_MFF(11);
    491. 

  491. 			ASM_MFF(12); ASM_MFF(13); ASM_MFF(14); ASM_MFF(15);
    492. 

  492. 			ASM_MFF(16); ASM_MFF(17); ASM_MFF(18); ASM_MFF(19);
    493. 

  493. 			ASM_MFF(20); ASM_MFF(21); ASM_MFF(22); ASM_MFF(23);
    494. 

  494. 			ASM_MFF(24); ASM_MFF(25); ASM_MFF(26); ASM_MFF(27);
    495. 

  495. 			ASM_MFF(28); ASM_MFF(29); ASM_MFF(30); ASM_MFF(31);
    496. 

  496. #undef ASM_MFF
    497. 

  497. 			}
    498. 

  498. 			put32t_unaligned_check(val, addr);
    499. 

  499. 		}
    500. 

  500. 		return TYPE_COLS;
    501. 

  501. 	}
    502. 

  502. fault:
    503. 

  503. 	return TYPE_FAULT;
    504. 

  504. #endif
    505. 

  505. 	printk(KERN_ERR "Alignment trap: not handling instruction "
    506. 

  506. 	       "%08lx at [<%08lx>]\n", instr, instrptr);
    507. 

  507. 	return 1;
    508. 

  508. }
    509. 

  509. 

  510. /*
    511. 

  511.  * This needs to be done after sysctl_init, otherwise sys/ will be
    512. 

  512.  * overwritten.  Actually, this shouldn't be in sys/ at all since
    513. 

  513.  * it isn't a sysctl, and it doesn't contain sysctl information.
    514. 

  514.  */
    515. 

  515. static int __init alignment_init(void)
    516. 

  516. {
    517. 

  517. 	hook_fault_code(1, do_alignment, SIGBUS, BUS_ADRALN,
    518. 

  518. 			"alignment exception");
    519. 

  519. 

  520. 	return 0;
    521. 

  521. }
    522. 

  522. 

  523. fs_initcall(alignment_init);
    524.