1 files changed, 26 insertions, 1 deletions

diff --git a/cpu/simple_cpu/simple_cpu.cc b/cpu/simple_cpu/simple_cpu.cc
index 60d704604..814902390 100644
--- a/cpu/simple_cpu/simple_cpu.cc
+++ b/cpu/simple_cpu/simple_cpu.cc
@@ -627,7 +627,32 @@ SimpleCPU::tick()
         xc->func_exe_insn++;
 
         fault = si->execute(this, xc, traceData);
-
+#ifdef FS_MEASURE
+        if (!(xc->misspeculating()) && (xc->system->bin)) {
+            SWContext *ctx = xc->swCtx;
+            if (ctx && !ctx->callStack.empty()) {
+                if (si->isCall()) {
+                    ctx->calls++;
+                }
+                if (si->isReturn()) {
+                     if (ctx->calls == 0) {
+                        fnCall *top = ctx->callStack.top();
+                        DPRINTF(TCPIP, "Removing %s from callstack.\n", top->name);
+                        delete top;
+                        ctx->callStack.pop();
+                        if (ctx->callStack.empty())
+                            xc->system->nonPath->activate();
+                        else
+                            ctx->callStack.top()->myBin->activate();
+
+                        xc->system->dumpState(xc);
+                    } else {
+                        ctx->calls--;
+                    }
+                }
+            }
+        }
+#endif
         if (si->isMemRef()) {
             numMemRefs++;
         }