开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 嵌入式/单片机编程 > 嵌入式Linux > 查看源码
unaligned.c
资源名称：SBC-2410X_kernel.tar.gz　[点击查看]
上传用户：lgb322
上传日期：2013-02-24
资源大小：30529k
文件大小：20k
源码类别：

嵌入式Linux

开发平台：

Unix_Linux

unaligned.c：源码内容
  1. /* $Id: unaligned.c,v 1.23 2001/04/09 04:29:03 davem Exp $
  2.  * unaligned.c: Unaligned load/store trap handling with special
  3.  *              cases for the kernel to do them more quickly.
  4.  *
  5.  * Copyright (C) 1996 David S. Miller (davem@caip.rutgers.edu)
  6.  * Copyright (C) 1996,1997 Jakub Jelinek (jj@sunsite.mff.cuni.cz)
  7.  */
  10. #include <linux/kernel.h>
  11. #include <linux/sched.h>
  12. #include <linux/mm.h>
  13. #include <asm/asi.h>
  14. #include <asm/ptrace.h>
  15. #include <asm/pstate.h>
  16. #include <asm/processor.h>
  17. #include <asm/system.h>
  18. #include <asm/uaccess.h>
  19. #include <linux/smp.h>
  20. #include <linux/smp_lock.h>
  21. #include <asm/fpumacro.h>
  22. #include <asm/bitops.h>
  24. /* #define DEBUG_MNA */
  26. enum direction {
  27. load,    /* ld, ldd, ldh, ldsh */
  28. store,   /* st, std, sth, stsh */
  29. both,    /* Swap, ldstub, cas, ... */
  30. fpld,
  31. fpst,
  32. invalid,
  33. };
  35. #ifdef DEBUG_MNA
  36. static char *dirstrings[] = {
  37.   "load", "store", "both", "fpload", "fpstore", "invalid"
  38. };
  39. #endif
  41. static inline enum direction decode_direction(unsigned int insn)
  42. {
  43. unsigned long tmp = (insn >> 21) & 1;
  45. if(!tmp)
  46. return load;
  47. else {
  48. switch ((insn>>19)&0xf) {
  49. case 15: /* swap* */
  50. return both;
  51. default:
  52. return store;
  53. }
  54. }
  55. }
  57. /* 16 = double-word, 8 = extra-word, 4 = word, 2 = half-word */
  58. static inline int decode_access_size(unsigned int insn)
  59. {
  60. unsigned int tmp;
  62. tmp = ((insn >> 19) & 0xf);
  63. if (tmp == 11 || tmp == 14) /* ldx/stx */
  64. return 8;
  65. tmp &= 3;
  66. if(!tmp)
  67. return 4;
  68. else if(tmp == 3)
  69. return 16; /* ldd/std - Although it is actually 8 */
  70. else if(tmp == 2)
  71. return 2;
  72. else {
  73. printk("Impossible unaligned trap. insn=%08xn", insn);
  74. die_if_kernel("Byte sized unaligned access?!?!", current->thread.kregs);
  75. }
  76. }
  78. static inline int decode_asi(unsigned int insn, struct pt_regs *regs)
  79. {
  80. if (insn & 0x800000) {
  81. if (insn & 0x2000)
  82. return (unsigned char)(regs->tstate >> 24); /* %asi */
  83. else
  84. return (unsigned char)(insn >> 5); /* imm_asi */
  85. } else
  86. return ASI_P;
  87. }
  89. /* 0x400000 = signed, 0 = unsigned */
  90. static inline int decode_signedness(unsigned int insn)
  91. {
  92. return (insn & 0x400000);
  93. }
  95. static inline void maybe_flush_windows(unsigned int rs1, unsigned int rs2,
  96.        unsigned int rd, int from_kernel)
  97. {
  98. if(rs2 >= 16 || rs1 >= 16 || rd >= 16) {
  99. if(from_kernel != 0)
  100. __asm__ __volatile__("flushw");
  101. else
  102. flushw_user();
  103. }
  104. }
  106. static inline long sign_extend_imm13(long imm)
  107. {
  108. return imm << 51 >> 51;
  109. }
  111. static unsigned long fetch_reg(unsigned int reg, struct pt_regs *regs)
  112. {
  113. unsigned long value;
  115. if(reg < 16)
  116. return (!reg ? 0 : regs->u_regs[reg]);
  117. if (regs->tstate & TSTATE_PRIV) {
  118. struct reg_window *win;
  119. win = (struct reg_window *)(regs->u_regs[UREG_FP] + STACK_BIAS);
  120. value = win->locals[reg - 16];
  121. } else if (current->thread.flags & SPARC_FLAG_32BIT) {
  122. struct reg_window32 *win32;
  123. win32 = (struct reg_window32 *)((unsigned long)((u32)regs->u_regs[UREG_FP]));
  124. get_user(value, &win32->locals[reg - 16]);
  125. } else {
  126. struct reg_window *win;
  127. win = (struct reg_window *)(regs->u_regs[UREG_FP] + STACK_BIAS);
  128. get_user(value, &win->locals[reg - 16]);
  129. }
  130. return value;
  131. }
  133. static unsigned long *fetch_reg_addr(unsigned int reg, struct pt_regs *regs)
  134. {
  135. if(reg < 16)
  136. return &regs->u_regs[reg];
  137. if (regs->tstate & TSTATE_PRIV) {
  138. struct reg_window *win;
  139. win = (struct reg_window *)(regs->u_regs[UREG_FP] + STACK_BIAS);
  140. return &win->locals[reg - 16];
  141. } else if (current->thread.flags & SPARC_FLAG_32BIT) {
  142. struct reg_window32 *win32;
  143. win32 = (struct reg_window32 *)((unsigned long)((u32)regs->u_regs[UREG_FP]));
  144. return (unsigned long *)&win32->locals[reg - 16];
  145. } else {
  146. struct reg_window *win;
  147. win = (struct reg_window *)(regs->u_regs[UREG_FP] + STACK_BIAS);
  148. return &win->locals[reg - 16];
  149. }
  150. }
  152. static inline unsigned long compute_effective_address(struct pt_regs *regs,
  153.       unsigned int insn, unsigned int rd)
  154. {
  155. unsigned int rs1 = (insn >> 14) & 0x1f;
  156. unsigned int rs2 = insn & 0x1f;
  157. int from_kernel = (regs->tstate & TSTATE_PRIV) != 0;
  159. if(insn & 0x2000) {
  160. maybe_flush_windows(rs1, 0, rd, from_kernel);
  161. return (fetch_reg(rs1, regs) + sign_extend_imm13(insn));
  162. } else {
  163. maybe_flush_windows(rs1, rs2, rd, from_kernel);
  164. return (fetch_reg(rs1, regs) + fetch_reg(rs2, regs));
  165. }
  166. }
  168. /* This is just to make gcc think die_if_kernel does return... */
  169. static void unaligned_panic(char *str, struct pt_regs *regs)
  170. {
  171. die_if_kernel(str, regs);
  172. }
  174. #define do_integer_load(dest_reg, size, saddr, is_signed, asi, errh) ({
  175. __asm__ __volatile__ (
  176. "wr %4, 0, %%asint"
  177. "cmp %1, 8nt"
  178. "bge,pn %%icc, 9fnt"
  179. " cmp %1, 4nt"
  180. "be,pt %%icc, 6fn"
  181. "4:t" " lduba [%2] %%asi, %%l1n"
  182. "5:t" "lduba [%2 + 1] %%asi, %%l2nt"
  183. "sll %%l1, 8, %%l1nt"
  184. "brz,pt %3, 3fnt"
  185. " add %%l1, %%l2, %%l1nt"
  186. "sllx %%l1, 48, %%l1nt"
  187. "srax %%l1, 48, %%l1n"
  188. "3:t" "ba,pt %%xcc, 0fnt"
  189. " stx %%l1, [%0]n"
  190. "6:t" "lduba [%2 + 1] %%asi, %%l2nt"
  191. "sll %%l1, 24, %%l1n"
  192. "7:t" "lduba [%2 + 2] %%asi, %%g7nt"
  193. "sll %%l2, 16, %%l2n"
  194. "8:t" "lduba [%2 + 3] %%asi, %%g1nt"
  195. "sll %%g7, 8, %%g7nt"
  196. "or %%l1, %%l2, %%l1nt"
  197. "or %%g7, %%g1, %%g7nt"
  198. "or %%l1, %%g7, %%l1nt"
  199. "brnz,a,pt %3, 3fnt"
  200. " sra %%l1, 0, %%l1n"
  201. "3:t" "ba,pt %%xcc, 0fnt"
  202. " stx %%l1, [%0]n"
  203. "9:t" "lduba [%2] %%asi, %%l1n"
  204. "10:t" "lduba [%2 + 1] %%asi, %%l2nt"
  205. "sllx %%l1, 56, %%l1n"
  206. "11:t" "lduba [%2 + 2] %%asi, %%g7nt"
  207. "sllx %%l2, 48, %%l2n"
  208. "12:t" "lduba [%2 + 3] %%asi, %%g1nt"
  209. "sllx %%g7, 40, %%g7nt"
  210. "sllx %%g1, 32, %%g1nt"
  211. "or %%l1, %%l2, %%l1nt"
  212. "or %%g7, %%g1, %%g7n"
  213. "13:t" "lduba [%2 + 4] %%asi, %%l2nt"
  214. "or %%l1, %%g7, %%g7n"
  215. "14:t" "lduba [%2 + 5] %%asi, %%g1nt"
  216. "sllx %%l2, 24, %%l2n"
  217. "15:t" "lduba [%2 + 6] %%asi, %%l1nt"
  218. "sllx %%g1, 16, %%g1nt"
  219. "or %%g7, %%l2, %%g7n"
  220. "16:t" "lduba [%2 + 7] %%asi, %%l2nt"
  221. "sllx %%l1, 8, %%l1nt"
  222. "or %%g7, %%g1, %%g7nt"
  223. "or %%l1, %%l2, %%l1nt"
  224. "or %%g7, %%l1, %%g7nt"
  225. "cmp %1, 8nt"
  226. "be,a,pt %%icc, 0fnt"
  227. " stx %%g7, [%0]nt"
  228. "srlx %%g7, 32, %%l1nt"
  229. "sra %%g7, 0, %%g7nt"
  230. "stx %%l1, [%0]nt"
  231. "stx %%g7, [%0 + 8]n"
  232. "0:nt"
  233. "wr %%g0, %5, %%asinnt"
  234. ".section __ex_tablent"
  235. ".word 4b, " #errh "nt"
  236. ".word 5b, " #errh "nt"
  237. ".word 6b, " #errh "nt"
  238. ".word 7b, " #errh "nt"
  239. ".word 8b, " #errh "nt"
  240. ".word 9b, " #errh "nt"
  241. ".word 10b, " #errh "nt"
  242. ".word 11b, " #errh "nt"
  243. ".word 12b, " #errh "nt"
  244. ".word 13b, " #errh "nt"
  245. ".word 14b, " #errh "nt"
  246. ".word 15b, " #errh "nt"
  247. ".word 16b, " #errh "nnt"
  248. ".previousnt"
  249. : : "r" (dest_reg), "r" (size), "r" (saddr), "r" (is_signed),
  250.   "r" (asi), "i" (ASI_AIUS)
  251. : "l1", "l2", "g7", "g1", "cc");
  252. })
  254. #define store_common(dst_addr, size, src_val, asi, errh) ({
  255. __asm__ __volatile__ (
  256. "wr %3, 0, %%asint"
  257. "ldx [%2], %%l1n"
  258. "cmp %1, 2nt"
  259. "be,pn %%icc, 2fnt"
  260. " cmp %1, 4nt"
  261. "be,pt %%icc, 1fnt"
  262. " srlx %%l1, 24, %%l2nt"
  263. "srlx %%l1, 56, %%g1nt"
  264. "srlx %%l1, 48, %%g7n"
  265. "4:t" "stba %%g1, [%0] %%asint"
  266. "srlx %%l1, 40, %%g1n"
  267. "5:t" "stba %%g7, [%0 + 1] %%asint"
  268. "srlx %%l1, 32, %%g7n"
  269. "6:t" "stba %%g1, [%0 + 2] %%asin"
  270. "7:t" "stba %%g7, [%0 + 3] %%asint"
  271. "srlx %%l1, 16, %%g1n"
  272. "8:t" "stba %%l2, [%0 + 4] %%asint"
  273. "srlx %%l1, 8, %%g7n"
  274. "9:t" "stba %%g1, [%0 + 5] %%asin"
  275. "10:t" "stba %%g7, [%0 + 6] %%asint"
  276. "ba,pt %%xcc, 0fn"
  277. "11:t" " stba %%l1, [%0 + 7] %%asin"
  278. "1:t" "srl %%l1, 16, %%g7n"
  279. "12:t" "stba %%l2, [%0] %%asint"
  280. "srl %%l1, 8, %%l2n"
  281. "13:t" "stba %%g7, [%0 + 1] %%asin"
  282. "14:t" "stba %%l2, [%0 + 2] %%asint"
  283. "ba,pt %%xcc, 0fn"
  284. "15:t" " stba %%l1, [%0 + 3] %%asin"
  285. "2:t" "srl %%l1, 8, %%l2n"
  286. "16:t" "stba %%l2, [%0] %%asin"
  287. "17:t" "stba %%l1, [%0 + 1] %%asin"
  288. "0:nt"
  289. "wr %%g0, %4, %%asinnt"
  290. ".section __ex_tablent"
  291. ".word 4b, " #errh "nt"
  292. ".word 5b, " #errh "nt"
  293. ".word 6b, " #errh "nt"
  294. ".word 7b, " #errh "nt"
  295. ".word 8b, " #errh "nt"
  296. ".word 9b, " #errh "nt"
  297. ".word 10b, " #errh "nt"
  298. ".word 11b, " #errh "nt"
  299. ".word 12b, " #errh "nt"
  300. ".word 13b, " #errh "nt"
  301. ".word 14b, " #errh "nt"
  302. ".word 15b, " #errh "nt"
  303. ".word 16b, " #errh "nt"
  304. ".word 17b, " #errh "nnt"
  305. ".previousnt"
  306. : : "r" (dst_addr), "r" (size), "r" (src_val), "r" (asi), "i" (ASI_AIUS)
  307. : "l1", "l2", "g7", "g1", "cc");
  308. })
  310. #define do_integer_store(reg_num, size, dst_addr, regs, asi, errh) ({
  311. unsigned long zero = 0;
  312. unsigned long *src_val = &zero;
  314. if (size == 16) {
  315. size = 8;
  316. zero = (((long)(reg_num ? 
  317.         (unsigned)fetch_reg(reg_num, regs) : 0)) << 32) |
  318. (unsigned)fetch_reg(reg_num + 1, regs);
  319. } else if (reg_num) src_val = fetch_reg_addr(reg_num, regs);
  320. store_common(dst_addr, size, src_val, asi, errh);
  321. })
  323. /* XXX Need to capture/release other cpu's for SMP around this. */
  324. #define do_atomic(srcdest_reg, mem, errh) ({
  325. unsigned long flags, tmp;
  327. save_and_cli(flags);
  328. tmp = *srcdest_reg;
  329. do_integer_load(srcdest_reg, 4, mem, 0, errh);
  330. store_common(mem, 4, &tmp, errh);
  331. restore_flags(flags);
  332. })
  334. static inline void advance(struct pt_regs *regs)
  335. {
  336. regs->tpc   = regs->tnpc;
  337. regs->tnpc += 4;
  338. if ((current->thread.flags & SPARC_FLAG_32BIT) != 0) {
  339. regs->tpc &= 0xffffffff;
  340. regs->tnpc &= 0xffffffff;
  341. }
  342. }
  344. static inline int floating_point_load_or_store_p(unsigned int insn)
  345. {
  346. return (insn >> 24) & 1;
  347. }
  349. static inline int ok_for_kernel(unsigned int insn)
  350. {
  351. return !floating_point_load_or_store_p(insn);
  352. }
  354. void kernel_mna_trap_fault(struct pt_regs *regs, unsigned int insn) __asm__ ("kernel_mna_trap_fault");
  356. void kernel_mna_trap_fault(struct pt_regs *regs, unsigned int insn)
  357. {
  358. unsigned long g2 = regs->u_regs [UREG_G2];
  359. unsigned long fixup = search_exception_table (regs->tpc, &g2);
  361. if (!fixup) {
  362. unsigned long address = compute_effective_address(regs, insn, ((insn >> 25) & 0x1f));
  363.          if(address < PAGE_SIZE) {
  364.                  printk(KERN_ALERT "Unable to handle kernel NULL pointer dereference in mna handler");
  365.          } else
  366.                  printk(KERN_ALERT "Unable to handle kernel paging request in mna handler");
  367.         printk(KERN_ALERT " at virtual address %016lxn",address);
  368. printk(KERN_ALERT "current->{mm,active_mm}->context = %016lxn",
  369. (current->mm ? current->mm->context :
  370. current->active_mm->context));
  371. printk(KERN_ALERT "current->{mm,active_mm}->pgd = %016lxn",
  372. (current->mm ? (unsigned long) current->mm->pgd :
  373. (unsigned long) current->active_mm->pgd));
  374.         die_if_kernel("Oops", regs);
  375. /* Not reached */
  376. }
  377. regs->tpc = fixup;
  378. regs->tnpc = regs->tpc + 4;
  379. regs->u_regs [UREG_G2] = g2;
  381. regs->tstate &= ~TSTATE_ASI;
  382. regs->tstate |= (ASI_AIUS << 24UL);
  383. }
  385. asmlinkage void kernel_unaligned_trap(struct pt_regs *regs, unsigned int insn, unsigned long sfar, unsigned long sfsr)
  386. {
  387. enum direction dir = decode_direction(insn);
  388. int size = decode_access_size(insn);
  390. if(!ok_for_kernel(insn) || dir == both) {
  391. printk("Unsupported unaligned load/store trap for kernel at <%016lx>.n",
  392.        regs->tpc);
  393. unaligned_panic("Kernel does fpu/atomic unaligned load/store.", regs);
  395. __asm__ __volatile__ ("n"
  396. "kernel_unaligned_trap_fault:nt"
  397. "mov %0, %%o0nt"
  398. "call kernel_mna_trap_faultnt"
  399. " mov %1, %%o1nt"
  400. :
  401. : "r" (regs), "r" (insn)
  402. : "o0", "o1", "o2", "o3", "o4", "o5", "o7",
  403.   "g1", "g2", "g3", "g4", "g5", "g7", "cc");
  404. } else {
  405. unsigned long addr = compute_effective_address(regs, insn, ((insn >> 25) & 0x1f));
  407. #ifdef DEBUG_MNA
  408. printk("KMNA: pc=%016lx [dir=%s addr=%016lx size=%d] retpc[%016lx]n",
  409.        regs->tpc, dirstrings[dir], addr, size, regs->u_regs[UREG_RETPC]);
  410. #endif
  411. switch(dir) {
  412. case load:
  413. do_integer_load(fetch_reg_addr(((insn>>25)&0x1f), regs),
  414. size, (unsigned long *) addr,
  415. decode_signedness(insn), decode_asi(insn, regs),
  416. kernel_unaligned_trap_fault);
  417. break;
  419. case store:
  420. do_integer_store(((insn>>25)&0x1f), size,
  421.  (unsigned long *) addr, regs,
  422.  decode_asi(insn, regs),
  423.  kernel_unaligned_trap_fault);
  424. break;
  425. #if 0 /* unsupported */
  426. case both:
  427. do_atomic(fetch_reg_addr(((insn>>25)&0x1f), regs),
  428.   (unsigned long *) addr,
  429.   kernel_unaligned_trap_fault);
  430. break;
  431. #endif
  432. default:
  433. panic("Impossible kernel unaligned trap.");
  434. /* Not reached... */
  435. }
  436. advance(regs);
  437. }
  438. }
  440. static char popc_helper[] = {
  441. 0, 1, 1, 2, 1, 2, 2, 3,
  442. 1, 2, 2, 3, 2, 3, 3, 4, 
  443. };
  445. int handle_popc(u32 insn, struct pt_regs *regs)
  446. {
  447. u64 value;
  448. int ret, i, rd = ((insn >> 25) & 0x1f);
  449. int from_kernel = (regs->tstate & TSTATE_PRIV) != 0;
  450.                         
  451. if (insn & 0x2000) {
  452. maybe_flush_windows(0, 0, rd, from_kernel);
  453. value = sign_extend_imm13(insn);
  454. } else {
  455. maybe_flush_windows(0, insn & 0x1f, rd, from_kernel);
  456. value = fetch_reg(insn & 0x1f, regs);
  457. }
  458. for (ret = 0, i = 0; i < 16; i++) {
  459. ret += popc_helper[value & 0xf];
  460. value >>= 4;
  461. }
  462. if(rd < 16) {
  463. if (rd)
  464. regs->u_regs[rd] = ret;
  465. } else {
  466. if (current->thread.flags & SPARC_FLAG_32BIT) {
  467. struct reg_window32 *win32;
  468. win32 = (struct reg_window32 *)((unsigned long)((u32)regs->u_regs[UREG_FP]));
  469. put_user(ret, &win32->locals[rd - 16]);
  470. } else {
  471. struct reg_window *win;
  472. win = (struct reg_window *)(regs->u_regs[UREG_FP] + STACK_BIAS);
  473. put_user(ret, &win->locals[rd - 16]);
  474. }
  475. }
  476. advance(regs);
  477. return 1;
  478. }
  480. extern void do_fpother(struct pt_regs *regs);
  481. extern void do_privact(struct pt_regs *regs);
  482. extern void data_access_exception(struct pt_regs *regs);
  484. int handle_ldf_stq(u32 insn, struct pt_regs *regs)
  485. {
  486. unsigned long addr = compute_effective_address(regs, insn, 0);
  487. int freg = ((insn >> 25) & 0x1e) | ((insn >> 20) & 0x20);
  488. struct fpustate *f = FPUSTATE;
  489. int asi = decode_asi(insn, regs);
  490. int flag = (freg < 32) ? FPRS_DL : FPRS_DU;
  492. save_and_clear_fpu();
  493. current->thread.xfsr[0] &= ~0x1c000;
  494. if (freg & 3) {
  495. current->thread.xfsr[0] |= (6 << 14) /* invalid_fp_register */;
  496. do_fpother(regs);
  497. return 0;
  498. }
  499. if (insn & 0x200000) {
  500. /* STQ */
  501. u64 first = 0, second = 0;
  503. if (current->thread.fpsaved[0] & flag) {
  504. first = *(u64 *)&f->regs[freg];
  505. second = *(u64 *)&f->regs[freg+2];
  506. }
  507. if (asi < 0x80) {
  508. do_privact(regs);
  509. return 1;
  510. }
  511. switch (asi) {
  512. case ASI_P:
  513. case ASI_S: break;
  514. case ASI_PL:
  515. case ASI_SL: 
  516. {
  517. /* Need to convert endians */
  518. u64 tmp = __swab64p(&first);
  520. first = __swab64p(&second);
  521. second = tmp;
  522. break;
  523. }
  524. default:
  525. data_access_exception(regs);
  526. return 1;
  527. }
  528. if (put_user (first >> 32, (u32 *)addr) ||
  529.     __put_user ((u32)first, (u32 *)(addr + 4)) ||
  530.     __put_user (second >> 32, (u32 *)(addr + 8)) ||
  531.     __put_user ((u32)second, (u32 *)(addr + 12))) {
  532.      data_access_exception(regs);
  533.      return 1;
  534. }
  535. } else {
  536. /* LDF, LDDF, LDQF */
  537. u32 data[4] __attribute__ ((aligned(8)));
  538. int size, i;
  539. int err;
  541. if (asi < 0x80) {
  542. do_privact(regs);
  543. return 1;
  544. } else if (asi > ASI_SNFL) {
  545. data_access_exception(regs);
  546. return 1;
  547. }
  548. switch (insn & 0x180000) {
  549. case 0x000000: size = 1; break;
  550. case 0x100000: size = 4; break;
  551. default: size = 2; break;
  552. }
  553. for (i = 0; i < size; i++)
  554. data[i] = 0;
  556. err = get_user (data[0], (u32 *)addr);
  557. if (!err) {
  558. for (i = 1; i < size; i++)
  559. err |= __get_user (data[i], (u32 *)(addr + 4*i));
  560. }
  561. if (err && !(asi & 0x2 /* NF */)) {
  562. data_access_exception(regs);
  563. return 1;
  564. }
  565. if (asi & 0x8) /* Little */ {
  566. u64 tmp;
  568. switch (size) {
  569. case 1: data[0] = le32_to_cpup(data + 0); break;
  570. default:*(u64 *)(data + 0) = le64_to_cpup((u64 *)(data + 0));
  571. break;
  572. case 4: tmp = le64_to_cpup((u64 *)(data + 0));
  573. *(u64 *)(data + 0) = le64_to_cpup((u64 *)(data + 2));
  574. *(u64 *)(data + 2) = tmp;
  575. break;
  576. }
  577. }
  578. if (!(current->thread.fpsaved[0] & FPRS_FEF)) {
  579. current->thread.fpsaved[0] = FPRS_FEF;
  580. current->thread.gsr[0] = 0;
  581. }
  582. if (!(current->thread.fpsaved[0] & flag)) {
  583. if (freg < 32)
  584. memset(f->regs, 0, 32*sizeof(u32));
  585. else
  586. memset(f->regs+32, 0, 32*sizeof(u32));
  587. }
  588. memcpy(f->regs + freg, data, size * 4);
  589. current->thread.fpsaved[0] |= flag;
  590. }
  591. advance(regs);
  592. return 1;
  593. }
  595. void handle_ld_nf(u32 insn, struct pt_regs *regs)
  596. {
  597. int rd = ((insn >> 25) & 0x1f);
  598. int from_kernel = (regs->tstate & TSTATE_PRIV) != 0;
  599. unsigned long *reg;
  600.                         
  601. maybe_flush_windows(0, 0, rd, from_kernel);
  602. reg = fetch_reg_addr(rd, regs);
  603. if (from_kernel || rd < 16) {
  604. reg[0] = 0;
  605. if ((insn & 0x780000) == 0x180000)
  606. reg[1] = 0;
  607. } else if (current->thread.flags & SPARC_FLAG_32BIT) {
  608. put_user(0, (int *)reg);
  609. if ((insn & 0x780000) == 0x180000)
  610. put_user(0, ((int *)reg) + 1);
  611. } else {
  612. put_user(0, reg);
  613. if ((insn & 0x780000) == 0x180000)
  614. put_user(0, reg + 1);
  615. }
  616. advance(regs);
  617. }
  619. void handle_lddfmna(struct pt_regs *regs, unsigned long sfar, unsigned long sfsr)
  620. {
  621. unsigned long pc = regs->tpc;
  622. unsigned long tstate = regs->tstate;
  623. u32 insn;
  624. u32 first, second;
  625. u64 value;
  626. u8 asi, freg;
  627. int flag;
  628. struct fpustate *f = FPUSTATE;
  630. if(tstate & TSTATE_PRIV)
  631. die_if_kernel("lddfmna from kernel", regs);
  632. if(current->thread.flags & SPARC_FLAG_32BIT)
  633. pc = (u32)pc;
  634. if (get_user(insn, (u32 *)pc) != -EFAULT) {
  635. asi = sfsr >> 16;
  636. if ((asi > ASI_SNFL) ||
  637.     (asi < ASI_P))
  638. goto daex;
  639. if (get_user(first, (u32 *)sfar) ||
  640.      get_user(second, (u32 *)(sfar + 4))) {
  641. if (asi & 0x2) /* NF */ {
  642. first = 0; second = 0;
  643. } else
  644. goto daex;
  645. }
  646. save_and_clear_fpu();
  647. freg = ((insn >> 25) & 0x1e) | ((insn >> 20) & 0x20);
  648. value = (((u64)first) << 32) | second;
  649. if (asi & 0x8) /* Little */
  650. value = __swab64p(&value);
  651. flag = (freg < 32) ? FPRS_DL : FPRS_DU;
  652. if (!(current->thread.fpsaved[0] & FPRS_FEF)) {
  653. current->thread.fpsaved[0] = FPRS_FEF;
  654. current->thread.gsr[0] = 0;
  655. }
  656. if (!(current->thread.fpsaved[0] & flag)) {
  657. if (freg < 32)
  658. memset(f->regs, 0, 32*sizeof(u32));
  659. else
  660. memset(f->regs+32, 0, 32*sizeof(u32));
  661. }
  662. *(u64 *)(f->regs + freg) = value;
  663. current->thread.fpsaved[0] |= flag;
  664. } else {
  665. daex: data_access_exception(regs);
  666. return;
  667. }
  668. advance(regs);
  669. return;
  670. }
  672. void handle_stdfmna(struct pt_regs *regs, unsigned long sfar, unsigned long sfsr)
  673. {
  674. unsigned long pc = regs->tpc;
  675. unsigned long tstate = regs->tstate;
  676. u32 insn;
  677. u64 value;
  678. u8 asi, freg;
  679. int flag;
  680. struct fpustate *f = FPUSTATE;
  682. if(tstate & TSTATE_PRIV)
  683. die_if_kernel("stdfmna from kernel", regs);
  684. if(current->thread.flags & SPARC_FLAG_32BIT)
  685. pc = (u32)pc;
  686. if (get_user(insn, (u32 *)pc) != -EFAULT) {
  687. freg = ((insn >> 25) & 0x1e) | ((insn >> 20) & 0x20);
  688. asi = sfsr >> 16;
  689. value = 0;
  690. flag = (freg < 32) ? FPRS_DL : FPRS_DU;
  691. if ((asi > ASI_SNFL) ||
  692.     (asi < ASI_P))
  693. goto daex;
  694. save_and_clear_fpu();
  695. if (current->thread.fpsaved[0] & flag)
  696. value = *(u64 *)&f->regs[freg];
  697. switch (asi) {
  698. case ASI_P:
  699. case ASI_S: break;
  700. case ASI_PL:
  701. case ASI_SL: 
  702. value = __swab64p(&value); break;
  703. default: goto daex;
  704. }
  705. if (put_user (value >> 32, (u32 *)sfar) ||
  706.     __put_user ((u32)value, (u32 *)(sfar + 4)))
  707. goto daex;
  708. } else {
  709. daex: data_access_exception(regs);
  710. return;
  711. }
  712. advance(regs);
  713. return;
  714. }