vcpu example problem

[雷雷健]

Hi,
       I  am trying  to modify the vcpu example of l4re-snapshot to
switch registers to execute my C function.But It doesn't work.
       I have read some papers about l4linux tranplantion.and follow it .
       To switch the process, I fill the vcpu_state_t structure with
its registers.and call vcpu_resume_commit action.Then Jump to the ASM
Code.
       But problem arises when I want to jump from asm code to C
function. What's the Problem? The Code is as following:
/*
 * (c) 2009 Technische Universität Dresden
 * This file is part of TUD:OS, which is distributed under the terms of the
 * GNU General Public License 2. Please see the COPYING file for details.
 */
#include <l4/sys/ipc.h>
#include <l4/sys/thread>
#include <l4/sys/factory>
#include <l4/sys/scheduler>
#include <l4/sys/utcb.h>
#include <l4/sys/kdebug.h>
#include <l4/util/util.h>
#include <l4/re/env>
#include <l4/re/util/cap_alloc>
#include <l4/re/util/kumem_alloc>
#include <l4/sys/debugger.h>
#include <l4/vcpu/vcpu>
#include <l4/cxx/iostream>

#include <l4/re/error_helper>

#include <l4/sys/task>
#include <l4/sys/irq>
#include <l4/sys/vcpu.h>

#include <cstdlib>
#include <cstdio>
#include <cstring>
#define PSR_T_BIT       0x00000000
#define SVC_MODE	0x00000013
#define PSR_E_BIT	0x00000200
#define PSR_I_BIT	0x00000000
#define USR_MODE	0x00000010
using L4Re::chksys;
using L4Re::chkcap;

static L4::Cap<L4::Irq> irq;

static char thread_stack[8 << 10];
static char hdl_stack[8 << 10];
static char idle_stack[8<<10];
static L4::Cap<L4::Task> vcpu_task;
static L4vcpu::Vcpu *vcpu;

const l4_addr_t super_code_map_addr = 0x10000;
const l4_addr_t idle_code_map_addr  = 0x20000;
const l4_addr_t stack_map_addr = 0x12000;
const l4_addr_t exit_map_addr=0x13000;
extern char my_super_code[];
extern char my_super_code_excp[];
extern char my_super_code_excp_after[];
extern char exit_code[];



void idle_code(void)
{
;
while(1)
{
__asm__ volatile("swi 0");
 l4_sleep(500);
}
}
asm(
".global exit_code \t\n"
"exit_code: \t\n"
"b exit_code \t\n"
);

asm volatile
(
  ".pushsection .text\n"
  ".p2align 12         \t\n"
  ".global my_super_code  \t\n"
  "my_super_code:   \t\n"
"	msr	cpsr_c, r7\t\n"
"	mov	lr, r6   \t\n"
"	mov	pc, r5   \t\n"
//"       b       my_super_code \t\n"
".popsection"
);
static void setup_user_state_arch(L4vcpu::Vcpu *) { }
static void handler_prolog() {}




static void handler(void)
{
  handler_prolog();

  vcpu->state()->clear(L4_VCPU_F_EXCEPTIONS);
  printf("hello handler\n");
    //vcpu->print_state();

  // very simple page-fault handling
  // we're just replying with the only page we have, without checking any
  // values
  if (vcpu->is_page_fault_entry())
    {
      printf("page fault\n");
      vcpu_task->map(L4Re::This_task, l4_fpage((l4_addr_t)my_super_code,
                                                L4_PAGESHIFT, L4_FPAGE_RWX),
                                               super_code_map_addr);

     vcpu_task->map(L4Re::This_task, l4_fpage((l4_addr_t)idle_code,
                                                L4_PAGESHIFT, L4_FPAGE_RWX),
                                               idle_code_map_addr);
      vcpu_task->map(L4Re::This_task, l4_fpage((l4_addr_t)exit_code,
                                                L4_PAGESHIFT, L4_FPAGE_RWX),
                                               exit_map_addr);

      vcpu->saved_state()->add(L4_VCPU_F_PAGE_FAULTS);
    }
  else if (vcpu->is_irq_entry())
    {
      // We use the label 2000 for our IRQ
      if (vcpu->i()->label == 2000)
        printf("Our triggered IRQ\n");
      else if (vcpu->i()->label == 0)
        // direct IPC message to vCPU without
        // going through an IPCgate, label is set to 0
        printf("IPC: %lx\n", vcpu->i()->tag.label());
      else
        printf("Unclassifiable message\n");
    }
    else
        printf("unhandled exception\n");


  printf("resume\n");
  L4::Cap<L4::Thread> self;
  self->vcpu_resume_commit(self->vcpu_resume_start());
  //vcpu->state()->set(0);
  while(1)
    ;
}

static void vcpu_thread(void)
{
  printf("Hello vCPU\n");
    l4_umword_t label;
  memset(hdl_stack, 0, sizeof(hdl_stack));
  //memset(super_stack,0,sizeof(super_stack));
//  memset(idle_stack,0,sizeof(idle_stack));
  setup_user_state_arch(vcpu);
  l4_touch_rw(stack,sizeof(stack));
  l4_touch_rw(idle_stack,sizeof(idle_stack));
  vcpu->saved_state()->set(L4_VCPU_F_USER_MODE
                           | L4_VCPU_F_EXCEPTIONS
                           | L4_VCPU_F_PAGE_FAULTS
                           | L4_VCPU_F_IRQ);
  vcpu->r()->flags=(l4_umword_t)SVC_MODE;
  vcpu->r()->r[7]=(l4_umword_t)SVC_MODE;
  vcpu->r()->r[0]=(l4_umword_t)0;
  vcpu->r()->r[4]=(l4_umword_t)(idle_stack+sizeof(idle_stack)-1);
  vcpu->r()->r[6]=(l4_umword_t)exit_map_addr;
  vcpu->r()->ip =(l4_umword_t)super_code_map_addr;
  vcpu->r()->r[5] = (l4_umword_t)idle_code_map_addr;
  vcpu->r()->sp = (l4_umword_t)0x30000;
  L4::Cap<L4::Thread> self;
  printf("IRET\n");
  vcpu->task(vcpu_task);
  self->vcpu_resume_commit(self->vcpu_resume_start());
  printf("IRET: failed!\n");
  while(1);

}

int run(void)
{
  l4_utcb_t *u = l4_utcb();
  L4::Cap<L4::Thread> vcpu_cap;

  printf("vCPU example\n");

  l4_debugger_set_object_name(l4re_env()->main_thread, "vcputest");

  // new task
  vcpu_task = chkcap(L4Re::Util::cap_alloc.alloc<L4::Task>(),
                     "Task cap alloc");

  chksys(L4Re::Env::env()->factory()->create_task(vcpu_task,
                                                  l4_fpage_invalid()),
         "create task");
  l4_debugger_set_object_name(vcpu_task.cap(), "vcpu 'user' task");

  /* new thread/vCPU */
  vcpu_cap = chkcap(L4Re::Util::cap_alloc.alloc<L4::Thread>(),
                    "vCPU cap alloc");

  l4_touch_rw(thread_stack, sizeof(thread_stack));
  //l4_touch_rw(hdl_stack,sizeof(hdl_stack));
  chksys(L4Re::Env::env()->factory()->create_thread(vcpu_cap), "create thread");
  l4_debugger_set_object_name(vcpu_cap.cap(), "vcpu thread");

  // get an IRQ
  irq = chkcap(L4Re::Util::cap_alloc.alloc<L4::Irq>(),
               "Irq cap alloc");
  chksys(L4Re::Env::env()->factory()->create_irq(irq), "irq");
  l4_debugger_set_object_name(irq.cap(), "some irq");

  // get memory for vCPU state
  l4_addr_t kumem;
  if (0)
    kumem = (l4_addr_t)l4re_env()->first_free_utcb;
  else
    {
      if (L4Re::Util::kumem_alloc(&kumem, 0))
        exit(1);
    }
  l4_utcb_t *vcpu_utcb = (l4_utcb_t *)kumem;
  vcpu = L4vcpu::Vcpu::cast(kumem + L4_UTCB_OFFSET);
  vcpu->entry_sp((l4_umword_t)hdl_stack + sizeof(hdl_stack));
  vcpu->entry_ip((l4_umword_t)handler);

  printf("VCPU: utcb = %p, vcpu = %p\n", vcpu_utcb, vcpu);

  // Create and start vCPU thread
  L4::Thread::Attr attr;
  attr.pager(L4::cap_reinterpret_cast<L4::Thread>(L4Re::Env::env()->rm()));
  attr.exc_handler(L4Re::Env::env()->main_thread());
  attr.bind(vcpu_utcb, L4Re::This_task);
  chksys(vcpu_cap->control(attr), "control");
  chksys(vcpu_cap->vcpu_control((l4_addr_t)vcpu), "enable VCPU");

  chksys(vcpu_cap->ex_regs((l4_umword_t)vcpu_thread,
                           (l4_umword_t)thread_stack + sizeof(thread_stack),
                           0));

  chksys(L4Re::Env::env()->scheduler()->run_thread(vcpu_cap,
                                                   l4_sched_param(2)));

  // Attach irq to our vCPU thread
  chksys(irq->attach(2000, vcpu_cap));

  // Send some IPCs to the vCPU
   l4_sleep(10);
  while (1)
    {
   //  printf("triger irq\n");
     // irq->trigger();
      l4_sleep(500);
    }


  l4_sleep_forever();
  return 0;
}

int main()
{
  try { return run(); }
  catch (L4::Runtime_error &e)
    {
      L4::cerr << "FATAL uncought exception: " << e
               << "\nterminating...\n";
    }
  catch (...)
    {
      L4::cerr << "FATAL uncought exception of unknown type\n"
               << "terminating...\n";
    }
  return 1;
}