fpu.c 3.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142
  1. /* vim: tabstop=4 shiftwidth=4 noexpandtab
  2. * This file is part of ToaruOS and is released under the terms
  3. * of the NCSA / University of Illinois License - see LICENSE.md
  4. * Copyright (C) 2011-2013 Kevin Lange
  5. *
  6. * FPU and SSE context handling.
  7. *
  8. * FPU context is kept through context switches,
  9. * but the FPU is disabled. When an FPU instruction
  10. * is executed, it will trap here and the context
  11. * will be saved to its original owner and the context
  12. * for the current process will be loaded or the FPU
  13. * will be reset for the new process.
  14. *
  15. * FPU states are per kernel thread.
  16. *
  17. */
  18. #include <kernel/system.h>
  19. #include <kernel/logging.h>
  20. #define NO_LAZY_FPU
  21. process_t * fpu_thread = NULL;
  22. /**
  23. * Set the FPU control word
  24. *
  25. * @param cw What to set the control word to.
  26. */
  27. void
  28. set_fpu_cw(const uint16_t cw) {
  29. asm volatile("fldcw %0" :: "m"(cw));
  30. }
  31. /**
  32. * Enable the FPU and SSE
  33. */
  34. void enable_fpu(void) {
  35. asm volatile ("clts");
  36. size_t t;
  37. asm volatile ("mov %%cr0, %0" : "=r"(t));
  38. t &= ~(1 << 2);
  39. t |= (1 << 1);
  40. asm volatile ("mov %0, %%cr0" :: "r"(t));
  41. asm volatile ("mov %%cr4, %0" : "=r"(t));
  42. t |= 3 << 9;
  43. asm volatile ("mov %0, %%cr4" :: "r"(t));
  44. }
  45. /**
  46. * Disable FPU and SSE so it traps to the kernel
  47. */
  48. void disable_fpu(void) {
  49. size_t t;
  50. asm volatile ("mov %%cr0, %0" : "=r"(t));
  51. t |= 1 << 3;
  52. asm volatile ("mov %0, %%cr0" :: "r"(t));
  53. }
  54. /* Temporary aligned buffer for copying around FPU contexts */
  55. uint8_t saves[512] __attribute__((aligned(16)));
  56. /**
  57. * Restore the FPU for a process
  58. */
  59. void restore_fpu(process_t * proc) {
  60. memcpy(&saves,(uint8_t *)&proc->thread.fp_regs,512);
  61. asm volatile ("fxrstor (%0)" :: "r"(saves));
  62. }
  63. /**
  64. * Save the FPU for a process
  65. */
  66. void save_fpu(process_t * proc) {
  67. asm volatile ("fxsave (%0)" :: "r"(saves));
  68. memcpy((uint8_t *)&proc->thread.fp_regs,&saves,512);
  69. }
  70. /**
  71. * Initialize the FPU
  72. */
  73. void init_fpu(void) {
  74. asm volatile ("fninit");
  75. }
  76. /**
  77. * Kernel trap for FPU usage when FPU is disabled
  78. */
  79. void invalid_op(struct regs * r) {
  80. /* First, turn the FPU on */
  81. enable_fpu();
  82. if (fpu_thread == current_process) {
  83. /* If this is the thread that last used the FPU, do nothing */
  84. return;
  85. }
  86. if (fpu_thread) {
  87. /* If there is a thread that was using the FPU, save its state */
  88. save_fpu(fpu_thread);
  89. }
  90. fpu_thread = (process_t *)current_process;
  91. if (!fpu_thread->thread.fpu_enabled) {
  92. /*
  93. * If the FPU has not been used in this thread previously,
  94. * we need to initialize it.
  95. */
  96. init_fpu();
  97. fpu_thread->thread.fpu_enabled = 1;
  98. return;
  99. }
  100. /* Otherwise we restore the context for this thread. */
  101. restore_fpu(fpu_thread);
  102. }
  103. /* Called during a context switch; disable the FPU */
  104. void switch_fpu(void) {
  105. #ifdef NO_LAZY_FPU
  106. save_fpu((process_t *)current_process);
  107. #else
  108. disable_fpu();
  109. #endif
  110. }
  111. void unswitch_fpu(void) {
  112. #ifdef NO_LAZY_FPU
  113. restore_fpu((process_t *)current_process);
  114. #endif
  115. }
  116. /* Enable the FPU context handling */
  117. void fpu_install(void) {
  118. #ifdef NO_LAZY_FPU
  119. enable_fpu();
  120. init_fpu();
  121. save_fpu((void*)current_process);
  122. #else
  123. enable_fpu();
  124. disable_fpu();
  125. isrs_install_handler(7, &invalid_op);
  126. #endif
  127. }