misaligned.c (5462B)
1/* 2 * linux/arch/nios2/kernel/misaligned.c 3 * 4 * basic emulation for mis-aligned accesses on the NIOS II cpu 5 * modelled after the version for arm in arm/alignment.c 6 * 7 * Brad Parker <brad@heeltoe.com> 8 * Copyright (C) 2010 Ambient Corporation 9 * Copyright (c) 2010 Altera Corporation, San Jose, California, USA. 10 * Copyright (c) 2010 Arrow Electronics, Inc. 11 * 12 * This file is subject to the terms and conditions of the GNU General 13 * Public License. See the file COPYING in the main directory of 14 * this archive for more details. 15 */ 16 17#include <linux/errno.h> 18#include <linux/string.h> 19#include <linux/proc_fs.h> 20#include <linux/init.h> 21#include <linux/sched.h> 22#include <linux/uaccess.h> 23#include <linux/seq_file.h> 24 25#include <asm/traps.h> 26#include <asm/unaligned.h> 27 28/* instructions we emulate */ 29#define INST_LDHU 0x0b 30#define INST_STH 0x0d 31#define INST_LDH 0x0f 32#define INST_STW 0x15 33#define INST_LDW 0x17 34 35static unsigned int ma_usermode; 36#define UM_WARN 0x01 37#define UM_FIXUP 0x02 38#define UM_SIGNAL 0x04 39#define KM_WARN 0x08 40 41/* see arch/nios2/include/asm/ptrace.h */ 42static u8 sys_stack_frame_reg_offset[] = { 43 /* struct pt_regs */ 44 8, 9, 10, 11, 12, 13, 14, 15, 1, 2, 3, 4, 5, 6, 7, 0, 45 /* struct switch_stack */ 46 16, 17, 18, 19, 20, 21, 22, 23, 0, 0, 0, 0, 0, 0, 0, 0 47}; 48 49static int reg_offsets[32]; 50 51static inline u32 get_reg_val(struct pt_regs *fp, int reg) 52{ 53 u8 *p = ((u8 *)fp) + reg_offsets[reg]; 54 return *(u32 *)p; 55} 56 57static inline void put_reg_val(struct pt_regs *fp, int reg, u32 val) 58{ 59 u8 *p = ((u8 *)fp) + reg_offsets[reg]; 60 *(u32 *)p = val; 61} 62 63/* 64 * (mis)alignment handler 65 */ 66asmlinkage void handle_unaligned_c(struct pt_regs *fp, int cause) 67{ 68 u32 isn, addr, val; 69 int in_kernel; 70 u8 a, b, d0, d1, d2, d3; 71 s16 imm16; 72 unsigned int fault; 73 74 /* back up one instruction */ 75 fp->ea -= 4; 76 77 if (fixup_exception(fp)) { 78 return; 79 } 80 81 in_kernel = !user_mode(fp); 82 83 isn = *(unsigned long *)(fp->ea); 84 85 fault = 0; 86 87 /* do fixup if in kernel or mode turned on */ 88 if (in_kernel || (ma_usermode & UM_FIXUP)) { 89 /* decompose instruction */ 90 a = (isn >> 27) & 0x1f; 91 b = (isn >> 22) & 0x1f; 92 imm16 = (isn >> 6) & 0xffff; 93 addr = get_reg_val(fp, a) + imm16; 94 95 /* do fixup to saved registers */ 96 switch (isn & 0x3f) { 97 case INST_LDHU: 98 fault |= __get_user(d0, (u8 *)(addr+0)); 99 fault |= __get_user(d1, (u8 *)(addr+1)); 100 val = (d1 << 8) | d0; 101 put_reg_val(fp, b, val); 102 break; 103 case INST_STH: 104 val = get_reg_val(fp, b); 105 d1 = val >> 8; 106 d0 = val >> 0; 107 if (in_kernel) { 108 *(u8 *)(addr+0) = d0; 109 *(u8 *)(addr+1) = d1; 110 } else { 111 fault |= __put_user(d0, (u8 *)(addr+0)); 112 fault |= __put_user(d1, (u8 *)(addr+1)); 113 } 114 break; 115 case INST_LDH: 116 fault |= __get_user(d0, (u8 *)(addr+0)); 117 fault |= __get_user(d1, (u8 *)(addr+1)); 118 val = (short)((d1 << 8) | d0); 119 put_reg_val(fp, b, val); 120 break; 121 case INST_STW: 122 val = get_reg_val(fp, b); 123 d3 = val >> 24; 124 d2 = val >> 16; 125 d1 = val >> 8; 126 d0 = val >> 0; 127 if (in_kernel) { 128 *(u8 *)(addr+0) = d0; 129 *(u8 *)(addr+1) = d1; 130 *(u8 *)(addr+2) = d2; 131 *(u8 *)(addr+3) = d3; 132 } else { 133 fault |= __put_user(d0, (u8 *)(addr+0)); 134 fault |= __put_user(d1, (u8 *)(addr+1)); 135 fault |= __put_user(d2, (u8 *)(addr+2)); 136 fault |= __put_user(d3, (u8 *)(addr+3)); 137 } 138 break; 139 case INST_LDW: 140 fault |= __get_user(d0, (u8 *)(addr+0)); 141 fault |= __get_user(d1, (u8 *)(addr+1)); 142 fault |= __get_user(d2, (u8 *)(addr+2)); 143 fault |= __get_user(d3, (u8 *)(addr+3)); 144 val = (d3 << 24) | (d2 << 16) | (d1 << 8) | d0; 145 put_reg_val(fp, b, val); 146 break; 147 } 148 } 149 150 addr = RDCTL(CTL_BADADDR); 151 cause >>= 2; 152 153 if (fault) { 154 if (in_kernel) { 155 pr_err("fault during kernel misaligned fixup @ %#lx; addr 0x%08x; isn=0x%08x\n", 156 fp->ea, (unsigned int)addr, 157 (unsigned int)isn); 158 } else { 159 pr_err("fault during user misaligned fixup @ %#lx; isn=%08x addr=0x%08x sp=0x%08lx pid=%d\n", 160 fp->ea, 161 (unsigned int)isn, addr, fp->sp, 162 current->pid); 163 164 _exception(SIGSEGV, fp, SEGV_MAPERR, fp->ea); 165 return; 166 } 167 } 168 169 /* 170 * kernel mode - 171 * note exception and skip bad instruction (return) 172 */ 173 if (in_kernel) { 174 fp->ea += 4; 175 176 if (ma_usermode & KM_WARN) { 177 pr_err("kernel unaligned access @ %#lx; BADADDR 0x%08x; cause=%d, isn=0x%08x\n", 178 fp->ea, 179 (unsigned int)addr, cause, 180 (unsigned int)isn); 181 /* show_regs(fp); */ 182 } 183 184 return; 185 } 186 187 /* 188 * user mode - 189 * possibly warn, 190 * possibly send SIGBUS signal to process 191 */ 192 if (ma_usermode & UM_WARN) { 193 pr_err("user unaligned access @ %#lx; isn=0x%08lx ea=0x%08lx ra=0x%08lx sp=0x%08lx\n", 194 (unsigned long)addr, (unsigned long)isn, 195 fp->ea, fp->ra, fp->sp); 196 } 197 198 if (ma_usermode & UM_SIGNAL) 199 _exception(SIGBUS, fp, BUS_ADRALN, fp->ea); 200 else 201 fp->ea += 4; /* else advance */ 202} 203 204static void __init misaligned_calc_reg_offsets(void) 205{ 206 int i, r, offset; 207 208 /* pre-calc offsets of registers on sys call stack frame */ 209 offset = 0; 210 211 /* struct pt_regs */ 212 for (i = 0; i < 16; i++) { 213 r = sys_stack_frame_reg_offset[i]; 214 reg_offsets[r] = offset; 215 offset += 4; 216 } 217 218 /* struct switch_stack */ 219 offset = -sizeof(struct switch_stack); 220 for (i = 16; i < 32; i++) { 221 r = sys_stack_frame_reg_offset[i]; 222 reg_offsets[r] = offset; 223 offset += 4; 224 } 225} 226 227 228static int __init misaligned_init(void) 229{ 230 /* default mode - silent fix */ 231 ma_usermode = UM_FIXUP | KM_WARN; 232 233 misaligned_calc_reg_offsets(); 234 235 return 0; 236} 237 238fs_initcall(misaligned_init);