kernel/fiasco/src/kern/arm/thread-arm-hyp.cpp

   1 IMPLEMENTATION [arm && hyp]:
   2
   3 IMPLEMENT_OVERRIDE
   4 void
   5 Thread::arch_init_vcpu_state(Vcpu_state *vcpu_state, bool ext)
   6 {
   7   if (!ext || (state() & Thread_ext_vcpu_enabled))
   8     return;
   9
  10   assert (check_for_current_cpu());
  11
  12   Vm_state *v = vm_state(vcpu_state);
  13   v->hcr = 0;
  14   v->csselr = 0;
  15   v->sctlr = (Cpu::Cp15_c1_generic | Cpu::Cp15_c1_cache_bits) & ~(Cpu::Cp15_c1_mmu | (1 << 28));
  16   v->actlr = 0;
  17   v->cpacr = 0x5555555;
  18   v->fcseidr = 0;
  19   v->contextidr = 0;
  20   v->vbar = 0;
  21   v->amair0 = 0;
  22   v->amair1 = 0;
  23
  24   v->guest_regs.hcr = Cpu::Hcr_tge;
  25   v->guest_regs.sctlr = 0;
  26
  27   v->host_regs.hcr = 0;
  28   v->host_regs.svc.lr = regs()->ulr;
  29   v->host_regs.svc.sp = regs()->sp();
  30   v->svc.lr = regs()->ulr;
  31   v->svc.sp = regs()->sp();
  32
  33   v->gic.hcr = Gic_h::Hcr(0);
  34   v->gic.apr = 0;
  35
  36   if (current() == this)
  37     {
  38       asm volatile ("mcr p15, 4, %0, c1, c1, 0"
  39                     : : "r"((1 << 2) | Cpu::Hcr_dc | Cpu::Hcr_must_set_bits));
  40       asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r"(v->sctlr));
  41       asm volatile ("msr SP_svc, %0" : : "r"(v->host_regs.svc.sp));
  42       asm volatile ("msr LR_svc, %0" : : "r"(v->host_regs.svc.lr));
  43     }
  44
  45   if (exception_triggered())
  46     _exc_cont.flags(regs(), _exc_cont.flags(regs()) | Proc::PSR_m_svc);
  47   else
  48     regs()->psr |=  Proc::PSR_m_svc;
  49 }
  50
  51 extern "C" void slowtrap_entry(Trap_state *ts);
  52 extern "C" Mword pagefault_entry(const Mword pfa, Mword error_code,
  53                                  const Mword pc, Return_frame *ret_frame);
  54
  55 PUBLIC static inline template<typename T>
  56 T
  57 Thread::peek_user(T const *adr, Context *c)
  58 {
  59   Address pa;
  60   asm ("mcr p15, 0, %1, c7, c8, 6 \n"
  61        "mrc p15, 0, %0, c7, c4, 0 \n"
  62        : "=r" (pa) : "r"(adr) );
  63   if (EXPECT_TRUE(!(pa & 1)))
  64     return *reinterpret_cast<T const *>(cxx::mask_lsb(pa, 12)
  65                                         | cxx::get_lsb((Address)adr, 12));
  66
  67   c->set_kernel_mem_op_hit();
  68   return T(~0);
  69 }
  70
  71 namespace {
  72     static Mword get_lr_for_mode(Return_frame const *rf)
  73     {
  74       Mword ret;
  75       switch (rf->psr & 0x1f)
  76         {
  77         case Proc::PSR_m_usr:
  78         case Proc::PSR_m_sys:
  79           return rf->ulr;
  80         case Proc::PSR_m_irq:
  81           asm ("mrs %0, lr_irq" : "=r" (ret)); return ret;
  82         case Proc::PSR_m_fiq:
  83           asm ("mrs %0, lr_fiq" : "=r" (ret)); return ret;
  84         case Proc::PSR_m_abt:
  85           asm ("mrs %0, lr_abt" : "=r" (ret)); return ret;
  86         case Proc::PSR_m_svc:
  87           asm ("mrs %0, lr_svc" : "=r" (ret)); return ret;
  88         case Proc::PSR_m_und:
  89           asm ("mrs %0, lr_und" : "=r" (ret)); return ret;
  90         default:
  91           assert(false); // wrong processor mode
  92           return ~0UL;
  93         }
  94     }
  95 };
  96
  97 extern "C" void hyp_mode_fault(Mword abort_type, Trap_state *ts)
  98 {
  99   Mword v;
 100   switch (abort_type)
 101     {
 102     case 0:
 103     case 1:
 104       ts->hsr().ec() = abort_type ? 0x11 : 0;
 105       printf("KERNEL%d: %s fault at %lx\n",
 106              cxx::int_value<Cpu_number>(current_cpu()),
 107              abort_type ? "SWI" : "Undefined instruction",
 108              ts->km_lr);
 109       break;
 110     case 2:
 111       ts->hsr().ec() = 0x21;
 112       asm volatile("mrc p15, 4, %0, c6, c0, 2" : "=r"(v));
 113       printf("KERNEL%d: Instruction abort at %lx\n",
 114              cxx::int_value<Cpu_number>(current_cpu()),
 115              v);
 116       break;
 117     case 3:
 118       ts->hsr().ec() = 0x25;
 119       asm volatile("mrc p15, 4, %0, c6, c0, 0" : "=r"(v));
 120       printf("KERNEL%d: Data abort: pc=%lx pfa=%lx\n",
 121              cxx::int_value<Cpu_number>(current_cpu()),
 122              ts->ip(), v);
 123       break;
 124     default:
 125       printf("KERNEL%d: Unknown hyp fault at %lx\n",
 126              cxx::int_value<Cpu_number>(current_cpu()),
 127              ts->ip());
 128       break;
 129     };
 130
 131   ts->dump();
 132
 133   kdb_ke("In-kernel fault");
 134 }
 135
 136 //-----------------------------------------------------------------------------
 137 IMPLEMENTATION [arm && hyp && fpu]:
 138
 139 PUBLIC static
 140 bool
 141 Thread::handle_fpu_trap(Trap_state *ts)
 142 {
 143   unsigned cond = ts->hsr().cv() ? ts->hsr().cond() : 0xe;
 144   if (!Thread::condition_valid(cond, ts->psr))
 145     {
 146       // FPU insns are 32bit, even for thumb
 147       assert (ts->hsr().il());
 148       ts->pc += 4;
 149       return true;
 150     }
 151
 152   assert (!Fpu::is_enabled());
 153
 154   if (current_thread()->switchin_fpu())
 155     return true;
 156
 157   // emulate the ARM exception entry PC
 158   ts->pc += ts->psr & Proc::Status_thumb ? 2 : 4;
 159
 160   return false;
 161 }
 162
 163 //-----------------------------------------------------------------------------
 164 IMPLEMENTATION [arm && hyp]:
 165
 166 #include "irq_mgr.h"
 167
 168 PUBLIC inline
 169 void
 170 Thread::vcpu_vgic_upcall(unsigned virq)
 171 {
 172   assert (state() & Thread_ext_vcpu_enabled);
 173   assert (state() & Thread_vcpu_user);
 174   assert (!_exc_cont.valid(regs()));
 175
 176   Vcpu_state *vcpu = vcpu_state().access();
 177   assert (vcpu_exceptions_enabled(vcpu));
 178
 179   Trap_state *ts = static_cast<Trap_state *>((Return_frame *)regs());
 180
 181   // Before entering kernel mode to have original fpu state before
 182   // enabling FPU
 183   save_fpu_state_to_utcb(ts, utcb().access());
 184
 185   check (vcpu_enter_kernel_mode(vcpu));
 186   vcpu = vcpu_state().access();
 187
 188   vcpu->_regs.s.hsr().ec() = 0x3d;
 189   vcpu->_regs.s.hsr().svc_imm() = virq;
 190
 191   vcpu_save_state_and_upcall();
 192 }
 193
 194
 195 class Arm_ppi_virt : public Irq_base
 196 {
 197 public:
 198
 199   Arm_ppi_virt(unsigned irq, unsigned virq) : _virq(virq), _irq(irq)
 200   {
 201     set_hit(handler_wrapper<Arm_ppi_virt>);
 202   }
 203
 204   void alloc()
 205   {
 206     printf("Allocate ARM PPI %d to virtual %d\n", _irq, _virq);
 207     check (Irq_mgr::mgr->alloc(this, _irq));
 208     chip()->unmask(pin());
 209   }
 210
 211 private:
 212   void switch_mode(bool) {}
 213
 214   unsigned _virq;
 215   unsigned _irq;
 216 };
 217
 218 PUBLIC inline FIASCO_FLATTEN
 219 void
 220 Arm_ppi_virt::handle(Upstream_irq const *ui)
 221 {
 222   current_thread()->vcpu_vgic_upcall(_virq);
 223   chip()->ack(pin());
 224   ui->ack();
 225 }
 226
 227 class Arm_vtimer_ppi : public Irq_base
 228 {
 229 public:
 230   Arm_vtimer_ppi(unsigned irq) : _irq(irq)
 231   {
 232     set_hit(handler_wrapper<Arm_vtimer_ppi>);
 233   }
 234
 235   void alloc()
 236   {
 237     printf("Allocate ARM PPI %d to virtual %d\n", _irq, 1);
 238     check (Irq_mgr::mgr->alloc(this, _irq));
 239     chip()->unmask(pin());
 240   }
 241
 242 private:
 243   void switch_mode(bool) {}
 244   unsigned _irq;
 245 };
 246
 247 PUBLIC inline FIASCO_FLATTEN
 248 void
 249 Arm_vtimer_ppi::handle(Upstream_irq const *ui)
 250 {
 251   Mword v;
 252   asm volatile("mrc p15, 0, %0, c14, c3, 1\n"
 253                "orr %0, #0x2              \n"
 254                "mcr p15, 0, %0, c14, c3, 1\n" : "=r" (v));
 255   current_thread()->vcpu_vgic_upcall(1);
 256   chip()->ack(pin());
 257   ui->ack();
 258 }
 259
 260 static Arm_ppi_virt __vgic_irq(25, 0);  // virtual GIC
 261 static Arm_vtimer_ppi __vtimer_irq(27); // virtual timer
 262
 263 namespace {
 264 struct Local_irq_init
 265 {
 266   Local_irq_init()
 267   {
 268     __vgic_irq.alloc();
 269     __vtimer_irq.alloc();
 270   }
 271 };
 272 DEFINE_PER_CPU_LATE static Per_cpu<Local_irq_init> local_irqs;
 273 }
 274
 275
 276 static inline
 277 bool
 278 is_syscall_pc(Address pc)
 279 {
 280   return Unsigned32(-0x2a) <= pc && pc <= Unsigned32(-0x08);
 281 }
 282
 283 static inline
 284 Address
 285 get_fault_ipa(Ts_error_code hsr, bool insn_abt, bool ext_vcpu)
 286 {
 287   Unsigned32 far;
 288   if (insn_abt)
 289     asm ("mrc p15, 4, %0, c6, c0, 2" : "=r" (far));
 290   else
 291     asm ("mrc p15, 4, %0, c6, c0, 0" : "=r" (far));
 292
 293   if (EXPECT_TRUE(!ext_vcpu))
 294     return far;
 295
 296   Unsigned32 sctlr;
 297   asm ("mrc p15, 0, %0, c1, c0, 0" : "=r" (sctlr));
 298   if (!(sctlr & 1)) // stage 1 mmu disabled
 299     return far;
 300
 301   if (hsr.pf_s1ptw()) // stage 1 walk
 302     {
 303       Unsigned32 ipa;
 304       asm ("mrc p15, 4, %0, c6, c0, 4" : "=r" (ipa));
 305       return ipa << 8;
 306     }
 307
 308   if ((hsr.pf_fsc() & 0x3c) != 0xc) // no permission fault
 309     {
 310       Unsigned32 ipa;
 311       asm ("mrc p15, 4, %0, c6, c0, 4" : "=r" (ipa));
 312       return (ipa << 8) | (far & 0xfff);
 313     }
 314
 315   Unsigned64 par;
 316   asm ("mcr p15, 0, %1, c7, c8, 0 \n"
 317        "mrrc p15, 0, %Q0, %R0, c7 \n" : "=r"(par) : "r"(far));
 318   if (par & 1)
 319     return ~0UL;
 320   return (par & 0xfffff000UL) | (far & 0xfff);
 321 }
 322
 323 extern "C" void arm_hyp_entry(Return_frame *rf)
 324 {
 325   Trap_state *ts = static_cast<Trap_state*>(rf);
 326   Thread *ct = current_thread();
 327
 328   Ts_error_code hsr;
 329   asm ("mrc p15, 4, %0, c5, c2, 0" : "=r" (hsr));
 330   ts->error_code = hsr.raw();
 331
 332   Unsigned32 tmp;
 333   Mword state = ct->state();
 334
 335   switch (hsr.ec())
 336     {
 337     case 0x20:
 338       tmp = get_fault_ipa(hsr, true, state & Thread_ext_vcpu_enabled);
 339       if (!pagefault_entry(tmp, hsr.raw(), rf->pc, rf))
 340         {
 341           Proc::cli();
 342           ts->pf_address = tmp;
 343           slowtrap_entry(ts);
 344         }
 345       return;
 346
 347     case 0x24:
 348       tmp = get_fault_ipa(hsr, false, state & Thread_ext_vcpu_enabled);
 349       if (!pagefault_entry(tmp, hsr.raw(), rf->pc, rf))
 350         {
 351           Proc::cli();
 352           ts->pf_address = tmp;
 353           slowtrap_entry(ts);
 354         }
 355       return;
 356
 357     case 0x12: // HVC
 358     case 0x11: // SVC
 359         {
 360           Unsigned32 pc = rf->pc;
 361           if (!is_syscall_pc(pc))
 362             {
 363               slowtrap_entry(ts);
 364               return;
 365             }
 366           rf->pc = get_lr_for_mode(rf);
 367           ct->state_del(Thread_cancel);
 368           if (state & (Thread_vcpu_user | Thread_alien))
 369             {
 370               if (state & Thread_dis_alien)
 371                 ct->state_del_dirty(Thread_dis_alien);
 372               else
 373                 {
 374                   slowtrap_entry(ts);
 375                   return;
 376                 }
 377             }
 378
 379           typedef void Syscall(void);
 380           extern Syscall *sys_call_table[];
 381           sys_call_table[(-pc) / 4]();
 382           return;
 383         }
 384
 385     case 0x00: // undef opcode with HCR.TGE=1
 386         {
 387           ct->state_del(Thread_cancel);
 388           Mword state = ct->state();
 389           Unsigned32 pc = rf->pc;
 390
 391           if (state & (Thread_vcpu_user | Thread_alien))
 392             {
 393               ts->pc += ts->psr & Proc::Status_thumb ? 2 : 4,
 394               ct->send_exception(ts);
 395               return;
 396             }
 397           else if (EXPECT_FALSE(!is_syscall_pc(pc + 4)))
 398             {
 399               ts->pc += ts->psr & Proc::Status_thumb ? 2 : 4,
 400               slowtrap_entry(ts);
 401               return;
 402             }
 403
 404           rf->pc = get_lr_for_mode(rf);
 405           ct->state_del(Thread_cancel);
 406           typedef void Syscall(void);
 407           extern Syscall *sys_call_table[];
 408           sys_call_table[-(pc + 4) / 4]();
 409           return;
 410         }
 411       break;
 412
 413     case 0x07:
 414         {
 415           if ((hsr.cpt_simd() || hsr.cpt_cpnr() == 10 || hsr.cpt_cpnr() == 11)
 416               && Thread::handle_fpu_trap(ts))
 417             return;
 418
 419           ct->send_exception(ts);
 420         }
 421       break;
 422
 423     case 0x03: // CP15 trapped
 424         if (hsr.mcr_coproc_register() == hsr.mrc_coproc_register(0, 1, 0, 1))
 425           {
 426             ts->r[hsr.mcr_rt()] = 1 << 6;
 427             ts->pc += 2 << hsr.il();
 428             return;
 429           }
 430       // fall through
 431
 432     default:
 433       ct->send_exception(ts);
 434       break;
 435     }
 436 }