From 0c427444fcfa96e2db667f1df9fdb9183262a786 Mon Sep 17 00:00:00 2001
From: winlin <winlinvip@gmail.com>
Date: Sat, 16 Mar 2024 19:33:32 +0800
Subject: [PATCH] ST: Support save RBP and backtrace stack.

---
 trunk/3rdparty/st-srs/md.h    |  4 +++-
 trunk/3rdparty/st-srs/sched.c | 36 +++++++++++++++++++++++++++++++++--
 2 files changed, 37 insertions(+), 3 deletions(-)

diff --git a/trunk/3rdparty/st-srs/md.h b/trunk/3rdparty/st-srs/md.h
index d3158db3f..416b5b954 100644
--- a/trunk/3rdparty/st-srs/md.h
+++ b/trunk/3rdparty/st-srs/md.h
@@ -181,15 +181,17 @@ extern void _st_md_cxt_restore(_st_jmp_buf_t env, int val);
 
     #if defined(__amd64__) || defined(__x86_64__)
         #define MD_GET_SP(_t) *((long *)&((_t)->context[0].__jmpbuf[6]))
+        #define MD_GET_BP(_t) *((long *)&((_t)->context[0].__jmpbuf[1]))
     #else
         #error Unknown CPU architecture
     #endif
 
-    #define MD_INIT_CONTEXT(_thread, _sp, _main) \
+    #define MD_INIT_CONTEXT(_thread, _sp, _bp, _main) \
         ST_BEGIN_MACRO                             \
         if (MD_SETJMP((_thread)->context))         \
             _main();                                 \
         MD_GET_SP(_thread) = (long) (_sp);         \
+        MD_GET_BP(_thread) = (long) (_bp);         \
         ST_END_MACRO
 
     #define MD_GET_UTIME()            \
diff --git a/trunk/3rdparty/st-srs/sched.c b/trunk/3rdparty/st-srs/sched.c
index 176e9d1a1..061437715 100644
--- a/trunk/3rdparty/st-srs/sched.c
+++ b/trunk/3rdparty/st-srs/sched.c
@@ -633,7 +633,16 @@ _st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg, int joinabl
     _st_stack_t *stack;
     void **ptds;
     char *sp;
-    
+
+    void *stack_bp;             /* Frame pointer for this function */
+    void *stack_pbp;            /* Frame pointer for parent function */
+    unsigned long crbp;         /* Current RBP register value */
+    unsigned long crsp;         /* Current RSP register value */
+    unsigned long prbp;         /* Parent RBP register value */
+    int home_space_size;
+    asm volatile("mov %%rbp, %0" : "=r"(crbp));
+    asm volatile("mov %%rsp, %0" : "=r"(crsp));
+
     /* Adjust stack size */
     if (stk_size == 0)
         stk_size = ST_DEFAULT_STACK_SIZE;
@@ -663,7 +672,30 @@ _st_thread_t *st_thread_create(void *(*start)(void *arg), void *arg, int joinabl
     thread->start = start;
     thread->arg = arg;
 
-    _ST_INIT_CONTEXT(thread, stack->sp, _st_thread_main);
+    /* Initialize RBP and return address for parent coroutine. */
+    prbp = *(unsigned long*)crbp;
+    stack->sp = (char*)stack->sp - 0x10; /* For ra and rbp. */
+    *(((unsigned long*)stack->sp) + 1) = *(((unsigned long*)prbp) + 1); /* Save parent RA(return address) */
+    *(unsigned long*)stack->sp = *(unsigned long*)prbp; /* Save parent RBP */
+
+    stack_pbp = stack->sp;
+    home_space_size = (int)(prbp - crbp - 0x10);
+    stack->sp = (char*)stack->sp - home_space_size; /* Save parent home space */
+    memcpy(stack_pbp - home_space_size, (void*)(prbp - home_space_size), home_space_size);
+
+    /* Initialize RBP and return address for new coroutine. */
+    stack->sp = (char*)stack->sp - 0x10; /* For ra and rbp. */
+    *(((unsigned long*)stack->sp) + 1) = *(((unsigned long*)crbp)+1); /* Simulate instruction call, store RA(return address) */
+    *(unsigned long*)stack->sp = *(unsigned long*)crbp; /* Save RBP: push rbp */
+    *(unsigned long*)stack->sp = (unsigned long)stack_pbp; /* Overwrite parent RBP */
+
+    stack_bp = stack->sp; /* Save RSP as RBP: mov rbp, rsp */
+    home_space_size = (int)(crbp - crsp);
+    stack->sp = (char*)stack->sp - home_space_size; /* For home space: sub rsp, 0x20 */
+    memcpy(stack_bp - home_space_size, (void*)(crbp - home_space_size), home_space_size);
+
+    /* Initialize thread context. */
+    _ST_INIT_CONTEXT(thread, stack->sp, stack_bp, _st_thread_main);
 
     /* If thread is joinable, allocate a termination condition variable */
     if (joinable) {