1 /* 2 * processor control and status functions 3 * 4 * Copyright (C) 2014, Red Hat Inc, Andrew Jones <drjones@redhat.com> 5 * 6 * This work is licensed under the terms of the GNU LGPL, version 2. 7 */ 8 #include <libcflat.h> 9 #include <asm/ptrace.h> 10 #include <asm/processor.h> 11 #include <asm/esr.h> 12 13 static char *vector_names[] = { 14 "el1t_sync", 15 "el1t_irq", 16 "el1t_fiq", 17 "el1t_error", 18 "el1h_sync", 19 "el1h_irq", 20 "el1h_fiq", 21 "el1h_error", 22 "el0_sync_64", 23 "el0_irq_64", 24 "el0_fiq_64", 25 "el0_error_64", 26 "el0_sync_32", 27 "el0_irq_32", 28 "el0_fiq_32", 29 "el0_error_32", 30 }; 31 32 static char *ec_names[EC_MAX] = { 33 [ESR_EL1_EC_UNKNOWN] = "UNKNOWN", 34 [ESR_EL1_EC_WFI] = "WFI", 35 [ESR_EL1_EC_CP15_32] = "CP15_32", 36 [ESR_EL1_EC_CP15_64] = "CP15_64", 37 [ESR_EL1_EC_CP14_MR] = "CP14_MR", 38 [ESR_EL1_EC_CP14_LS] = "CP14_LS", 39 [ESR_EL1_EC_FP_ASIMD] = "FP_ASMID", 40 [ESR_EL1_EC_CP10_ID] = "CP10_ID", 41 [ESR_EL1_EC_CP14_64] = "CP14_64", 42 [ESR_EL1_EC_ILL_ISS] = "ILL_ISS", 43 [ESR_EL1_EC_SVC32] = "SVC32", 44 [ESR_EL1_EC_SVC64] = "SVC64", 45 [ESR_EL1_EC_SYS64] = "SYS64", 46 [ESR_EL1_EC_IABT_EL0] = "IABT_EL0", 47 [ESR_EL1_EC_IABT_EL1] = "IABT_EL1", 48 [ESR_EL1_EC_PC_ALIGN] = "PC_ALIGN", 49 [ESR_EL1_EC_DABT_EL0] = "DABT_EL0", 50 [ESR_EL1_EC_DABT_EL1] = "DABT_EL1", 51 [ESR_EL1_EC_SP_ALIGN] = "SP_ALIGN", 52 [ESR_EL1_EC_FP_EXC32] = "FP_EXC32", 53 [ESR_EL1_EC_FP_EXC64] = "FP_EXC64", 54 [ESR_EL1_EC_SERROR] = "SERROR", 55 [ESR_EL1_EC_BREAKPT_EL0] = "BREAKPT_EL0", 56 [ESR_EL1_EC_BREAKPT_EL1] = "BREAKPT_EL1", 57 [ESR_EL1_EC_SOFTSTP_EL0] = "SOFTSTP_EL0", 58 [ESR_EL1_EC_SOFTSTP_EL1] = "SOFTSTP_EL1", 59 [ESR_EL1_EC_WATCHPT_EL0] = "WATCHPT_EL0", 60 [ESR_EL1_EC_WATCHPT_EL1] = "WATCHPT_EL1", 61 [ESR_EL1_EC_BKPT32] = "BKPT32", 62 [ESR_EL1_EC_BRK64] = "BRK64", 63 }; 64 65 void show_regs(struct pt_regs *regs) 66 { 67 int i; 68 69 printf("pc : [<%016llx>] lr : [<%016llx>] pstate: %08llx\n", 70 regs->pc, regs->regs[30], regs->pstate); 71 printf("sp : %016llx\n", regs->sp); 72 73 for (i = 29; i >= 0; --i) { 74 printf("x%-2d: %016llx ", i, regs->regs[i]); 75 if (i % 2 == 0) 76 printf("\n"); 77 } 78 printf("\n"); 79 } 80 81 void *get_sp(void) 82 { 83 register unsigned long sp asm("sp"); 84 return (void *)sp; 85 } 86 87 static void bad_exception(enum vector v, struct pt_regs *regs, 88 unsigned int esr, bool bad_vector) 89 { 90 unsigned int ec = esr >> ESR_EL1_EC_SHIFT; 91 92 if (bad_vector) { 93 if (v < VECTOR_MAX) 94 printf("Unhandled vector %d (%s)\n", v, 95 vector_names[v]); 96 else 97 printf("Got bad vector=%d\n", v); 98 } else { 99 if (ec_names[ec]) 100 printf("Unhandled exception ec=0x%x (%s)\n", ec, 101 ec_names[ec]); 102 else 103 printf("Got bad ec=0x%x\n", ec); 104 } 105 106 printf("Vector: %d (%s)\n", v, vector_names[v]); 107 printf("ESR_EL1: %08lx, ec=0x%x (%s)\n", esr, ec, ec_names[ec]); 108 printf("Exception frame registers:\n"); 109 show_regs(regs); 110 abort(); 111 } 112 113 static exception_fn exception_handlers[VECTOR_MAX][EC_MAX]; 114 115 void install_exception_handler(enum vector v, unsigned int ec, exception_fn fn) 116 { 117 if (v < VECTOR_MAX && ec < EC_MAX) 118 exception_handlers[v][ec] = fn; 119 } 120 121 static void default_vector_handler(enum vector v, struct pt_regs *regs, 122 unsigned int esr) 123 { 124 unsigned int ec = esr >> ESR_EL1_EC_SHIFT; 125 126 if (ec < EC_MAX && exception_handlers[v][ec]) 127 exception_handlers[v][ec](regs, esr); 128 else 129 bad_exception(v, regs, esr, false); 130 } 131 132 static vector_fn vector_handlers[VECTOR_MAX] = { 133 [EL1H_SYNC] = default_vector_handler, 134 [EL1H_IRQ] = default_vector_handler, 135 [EL0_SYNC_64] = default_vector_handler, 136 [EL0_IRQ_64] = default_vector_handler, 137 }; 138 139 void do_handle_exception(enum vector v, struct pt_regs *regs, unsigned int esr) 140 { 141 if (v < VECTOR_MAX && vector_handlers[v]) 142 vector_handlers[v](v, regs, esr); 143 else 144 bad_exception(v, regs, esr, true); 145 } 146 147 void install_vector_handler(enum vector v, vector_fn fn) 148 { 149 if (v < VECTOR_MAX) 150 vector_handlers[v] = fn; 151 } 152 153 bool user_mode; 154 void start_usr(void (*func)(void *arg), void *arg, unsigned long sp_usr) 155 { 156 sp_usr &= (~15UL); /* stack ptr needs 16-byte alignment */ 157 158 user_mode = true; 159 160 asm volatile( 161 "mov x0, %0\n" 162 "msr sp_el0, %1\n" 163 "msr elr_el1, %2\n" 164 "mov x3, xzr\n" /* clear and "set" PSR_MODE_EL0t */ 165 "msr spsr_el1, x3\n" 166 "eret\n" 167 :: "r" (arg), "r" (sp_usr), "r" (func) : "x0", "x3"); 168 } 169