Kernel/x86 - Added SSE/FPU state saving, not fully tested

diff --git a/Kernel/arch/x86/errors.c b/Kernel/arch/x86/errors.c
index 15fe22b..c66e49d 100644 (file)
--- a/Kernel/arch/x86/errors.c
+++ b/Kernel/arch/x86/errors.c
@@ -16,6 +16,8 @@ extern void   Threads_Dump(void);
 extern void    Threads_Fault(int Num);
 extern int     GetCPUNum(void);
 extern void    MM_DumpTables(tVAddr, tVAddr);
+extern void    Proc_EnableSSE(void);
+extern void    Proc_RestoreSSE(Uint32 Data);
 
 // === PROTOTYPES ===
 void   __stack_chk_fail(void);
@@ -82,6 +84,24 @@ void ErrorHandler(tRegs *Regs)
                MM_PageFault( cr, Regs->err_code, Regs );
                return ;
        }
+
+       // #NM - Coprocessor unavaliable
+       if(Regs->int_num == 7)
+       {
+               tThread *thread = Proc_GetCurThread();
+               if(!thread->SavedState.bSSEModified)
+               {
+                       Proc_EnableSSE();
+                       if(!thread->SavedState.SSE)
+                               thread->SavedState.SSE = malloc(sizeof(tSSEState) + 0xF);
+                       else
+                               Proc_RestoreSSE( ((Uint)thread->SavedState.SSE + 0xF) & ~0xF );
+                       thread->SavedState.bSSEModified = 1;
+                       __asm__ __volatile__ ("sti");
+                       return ;
+               }
+               // oops, SSE enabled but a #NM, bad news
+       }
        
        // VM8086 GPF
        if(Regs->int_num == 13 && Regs->eflags & 0x20000)

diff --git a/Kernel/arch/x86/include/arch.h b/Kernel/arch/x86/include/arch.h
index 9a4f98d..95a734f 100644 (file)
--- a/Kernel/arch/x86/include/arch.h
+++ b/Kernel/arch/x86/include/arch.h
@@ -123,19 +123,6 @@ typedef struct {
        Uint    Resvd4[1];      // SS
 } tSyscallRegs;
 
-typedef struct {
-       #if USE_PAE
-       Uint    PDPT[4];
-       #else
-       Uint    CR3;
-       #endif
-} tMemoryState;
-
-typedef struct {
-       Uint    EIP, ESP, EBP;
-       Uint32  UserCS, UserEIP;
-} tTaskState;
-
 // === FUNCTIONS ===
 extern void    Debug_PutCharDebug(char ch);
 extern void    Debug_PutStringDebug(const char *String);

diff --git a/Kernel/arch/x86/include/proc.h b/Kernel/arch/x86/include/proc.h
index 73bddbb..9ecd98b 100644 (file)
--- a/Kernel/arch/x86/include/proc.h
+++ b/Kernel/arch/x86/include/proc.h
@@ -5,8 +5,6 @@
 #ifndef _PROC_H
 #define _PROC_H
 
-#include <threads_int.h>
-
 // === TYPES ==
 typedef struct sTSS {
        Uint32  Link;
@@ -23,6 +21,29 @@ typedef struct sTSS {
        Uint16  Resvd, IOPB;    // IO Permissions Bitmap
 } __attribute__((packed)) tTSS;
 
+typedef struct {
+       #if USE_PAE
+       Uint    PDPT[4];
+       #else
+       Uint32  CR3;
+       #endif
+} tMemoryState;
+
+// 512 bytes, 16 byte aligned
+typedef struct sSSEState
+{
+       char    data[512];
+} tSSEState;
+
+typedef struct {
+       Uint    EIP, ESP;
+       Uint32  UserCS, UserEIP;
+       tSSEState       *SSE;
+        int    bSSEModified;
+} tTaskState;
+
+#include <threads_int.h>
+
 #define USER_MAX       KERNEL_BASE
 
 #endif

diff --git a/Kernel/arch/x86/proc.asm b/Kernel/arch/x86/proc.asm
index 9c1cabd..4e7d160 100644 (file)
--- a/Kernel/arch/x86/proc.asm
+++ b/Kernel/arch/x86/proc.asm
@@ -3,6 +3,8 @@
 
 [bits 32]
 
+%define SAVEFLAG_FPU   0x1
+
 KERNEL_BASE    equ 0xC0000000
 
 KSTACK_USERSTATE_SIZE  equ     (4+8+1+5)*4     ; SRegs, GPRegs, CPU, IRET
@@ -61,20 +63,54 @@ SwitchTasks:
        mov ecx, [esp+0x20+12]  ; New IP
        mov eax, [esp+0x20+20]  ; New CR3
        mov esp, [esp+0x20+ 4]  ; New SP
+       
        test eax, eax
        jz .setState
        mov cr3, eax
        invlpg [esp]
        invlpg [esp+0x1000]
 .setState:
-;      xchg bx, bx
        jmp ecx
 
 .restore:
+
        popa
        xor eax, eax
        ret
 
+[global Proc_InitialiseSSE]
+Proc_InitialiseSSE:
+       mov eax, cr4
+       or eax, (1 << 9)|(1 << 10)      ; Set OSFXSR and OSXMMEXCPT
+       mov cr4, eax
+       mov eax, cr0
+       and ax, ~(1 << 2)       ; Clear EM
+       or eax, (1 << 1)        ; Set MP
+       mov eax, cr0
+       ret
+[global Proc_DisableSSE]
+Proc_DisableSSE:
+       mov eax, cr0
+       or ax, 1 << 3   ; Set TS
+       mov cr0, eax
+       ret
+[global Proc_EnableSSE]
+Proc_EnableSSE:
+       mov eax, cr0
+       and ax, ~(1 << 3)       ; Clear TS
+       mov cr0, eax
+       ret
+
+[global Proc_SaveSSE]
+Proc_SaveSSE:
+       mov eax, [esp+4]
+       fxsave [eax]
+       ret
+[global Proc_RestoreSSE]
+Proc_RestoreSSE:
+       mov eax, [esp+4]
+       fxrstor [eax]
+       ret
 
 %if USE_MP
 [extern giMP_TimerCount]

diff --git a/Kernel/arch/x86/proc.c b/Kernel/arch/x86/proc.c
index 14eaf3a..8efe2a8 100644 (file)
--- a/Kernel/arch/x86/proc.c
+++ b/Kernel/arch/x86/proc.c
@@ -45,7 +45,7 @@ extern void   APStartup(void);        // 16-bit AP startup code
 extern Uint    GetEIP(void);   // start.asm
 extern Uint    GetEIP_Sched(void);     // proc.asm
 extern void    NewTaskHeader(tThread *Thread, void *Fcn, int nArgs, ...);      // Actually takes cdecl args
-extern Uint    Proc_CloneInt(Uint *ESP, Uint *CR3);
+extern Uint    Proc_CloneInt(Uint *ESP, Uint32 *CR3);
 extern Uint32  gaInitPageDir[1024];    // start.asm
 extern char    Kernel_Stack_Top[];
 extern tShortSpinlock  glThreadListLock;
@@ -60,6 +60,9 @@ extern char   IRQCommon_handled[];    // IRQCommon call return location
 extern char    GetEIP_Sched_ret[];     // GetEIP call return location
 extern void    Threads_AddToDelete(tThread *Thread);
 extern void    SwitchTasks(Uint NewSP, Uint *OldSP, Uint NewIP, Uint *OldIO, Uint CR3);
+extern void    Proc_InitialiseSSE(void);
+extern void    Proc_SaveSSE(Uint DestPtr);
+extern void    Proc_DisableSSE(void);
 
 // === PROTOTYPES ===
 //void ArchThreads_Init(void);
@@ -371,6 +374,9 @@ void ArchThreads_Init(void)
        {
                Panic("OOM - No space for initial Per-Process Config");
        }
+
+       // Initialise SSE support
+       Proc_InitialiseSSE();
        
        // Change Stacks
        Proc_ChangeStack();
@@ -579,6 +585,7 @@ int Proc_NewKThread(void (*Fcn)(void*), void *Data)
        
        newThread->SavedState.ESP = esp;
        newThread->SavedState.EIP = (Uint)&NewTaskHeader;
+       newThread->SavedState.SSE = NULL;
        Log("New (KThread) %p, esp = %p\n", newThread->SavedState.EIP, newThread->SavedState.ESP);
        
 //     MAGIC_BREAK();  
@@ -616,6 +623,8 @@ int Proc_Clone(Uint Flags)
                return 0;
        }
        newThread->SavedState.EIP = eip;
+       newThread->SavedState.SSE = NULL;
+       newThread->SavedState.bSSEModified = 0;
        
        // Check for errors
        if( newThread->MemState.CR3 == 0 ) {
@@ -656,8 +665,9 @@ int Proc_SpawnWorker(void (*Fcn)(void*), void *Data)
 
        // Save core machine state
        new->SavedState.ESP = new->KernelStack - sizeof(stack_contents);
-       new->SavedState.EBP = 0;
        new->SavedState.EIP = (Uint)NewTaskHeader;
+       new->SavedState.SSE = NULL;
+       new->SavedState.bSSEModified = 0;
        
        // Mark as active
        new->Status = THREAD_STAT_PREINIT;
@@ -849,7 +859,6 @@ void Proc_DumpThreadCPUState(tThread *Thread)
                return ;
        }
        
-       #if 1
        tVAddr  diffFromScheduler = Thread->SavedState.EIP - (tVAddr)SwitchTasks;
        tVAddr  diffFromClone = Thread->SavedState.EIP - (tVAddr)Proc_CloneInt;
        tVAddr  diffFromSpawn = Thread->SavedState.EIP - (tVAddr)NewTaskHeader;
@@ -867,24 +876,6 @@ void Proc_DumpThreadCPUState(tThread *Thread)
        }
        
        if( diffFromScheduler > 0 && diffFromScheduler < 128 )  // When I last checked, GetEIP was at .+0x30
-       #else
-       Uint32  data[3];
-       MM_ReadFromAddrSpace(Thread->MemState.CR3, Thread->SavedState.EBP, data, 12);
-       if( data[1] == (Uint32)&IRQCommon + 25 )
-       {
-               tRegs   *regs = (void *) data[2];
-               Log("  oldebp = 0x%08x, ret = 0x%08x, regs = 0x%x",
-                       data[0], data[1], data[2]
-                       );
-               // [EBP] = old EBP
-               // [EBP+0x04] = Return Addr
-               // [EBP+0x08] = Arg 1 (CPU Number)
-               // [EBP+0x0C] = Arg 2 (Thread)
-               // [EBP+0x10] = GS (start of tRegs)
-               Log("  IRQ%i from %02x:%08x", regs->int_num regs->cs, regs->eip);
-       }
-       if( stack[1] == (Uint32)&scheduler_return )
-       #endif
        {
                // Scheduled out
                Log("  At %04x:%08x", Thread->SavedState.UserCS, Thread->SavedState.UserEIP);
@@ -899,7 +890,7 @@ void Proc_Reschedule(void)
        tThread *nextthread, *curthread;
         int    cpu = GetCPUNum();
 
-       // TODO: Wait for it?
+       // TODO: Wait for the lock?
        if(IS_LOCKED(&glThreadListLock))        return;
        
        curthread = Proc_GetCurThread();
@@ -925,11 +916,20 @@ void Proc_Reschedule(void)
        gTSSs[cpu].ESP0 = nextthread->KernelStack-4;
        __asm__ __volatile__("mov %0, %%db0\n\t" : : "r"(nextthread) );
 
+       // Save FPU/MMX/XMM/SSE state
+       if( curthread->SavedState.SSE )
+       {
+               Proc_SaveSSE( ((Uint)curthread->SavedState.SSE + 0xF) & ~0xF );
+               curthread->SavedState.bSSEModified = 0;
+               Proc_DisableSSE();
+       }
+
        SwitchTasks(
                nextthread->SavedState.ESP, &curthread->SavedState.ESP,
                nextthread->SavedState.EIP, &curthread->SavedState.EIP,
                nextthread->MemState.CR3
                );
+
        return ;
 }