1 // SPDX-License-Identifier: GPL-2.0 2 // Copyright (C) 2018 Hangzhou C-SKY Microsystems co.,ltd. 3 4 #include <linux/kernel.h> 5 #include <linux/uaccess.h> 6 #include <linux/ptrace.h> 7 8 static int align_enable = 1; 9 static int align_count; 10 11 static inline uint32_t get_ptreg(struct pt_regs *regs, uint32_t rx) 12 { 13 return rx == 15 ? regs->lr : *((uint32_t *)&(regs->a0) - 2 + rx); 14 } 15 16 static inline void put_ptreg(struct pt_regs *regs, uint32_t rx, uint32_t val) 17 { 18 if (rx == 15) 19 regs->lr = val; 20 else 21 *((uint32_t *)&(regs->a0) - 2 + rx) = val; 22 } 23 24 /* 25 * Get byte-value from addr and set it to *valp. 26 * 27 * Success: return 0 28 * Failure: return 1 29 */ 30 static int ldb_asm(uint32_t addr, uint32_t *valp) 31 { 32 uint32_t val; 33 int err; 34 35 if (!access_ok((void *)addr, 1)) 36 return 1; 37 38 asm volatile ( 39 "movi %0, 0\n" 40 "1:\n" 41 "ldb %1, (%2)\n" 42 "br 3f\n" 43 "2:\n" 44 "movi %0, 1\n" 45 "br 3f\n" 46 ".section __ex_table,\"a\"\n" 47 ".align 2\n" 48 ".long 1b, 2b\n" 49 ".previous\n" 50 "3:\n" 51 : "=&r"(err), "=r"(val) 52 : "r" (addr) 53 ); 54 55 *valp = val; 56 57 return err; 58 } 59 60 /* 61 * Put byte-value to addr. 62 * 63 * Success: return 0 64 * Failure: return 1 65 */ 66 static int stb_asm(uint32_t addr, uint32_t val) 67 { 68 int err; 69 70 if (!access_ok((void *)addr, 1)) 71 return 1; 72 73 asm volatile ( 74 "movi %0, 0\n" 75 "1:\n" 76 "stb %1, (%2)\n" 77 "br 3f\n" 78 "2:\n" 79 "movi %0, 1\n" 80 "br 3f\n" 81 ".section __ex_table,\"a\"\n" 82 ".align 2\n" 83 ".long 1b, 2b\n" 84 ".previous\n" 85 "3:\n" 86 : "=&r"(err) 87 : "r"(val), "r" (addr) 88 ); 89 90 return err; 91 } 92 93 /* 94 * Get half-word from [rx + imm] 95 * 96 * Success: return 0 97 * Failure: return 1 98 */ 99 static int ldh_c(struct pt_regs *regs, uint32_t rz, uint32_t addr) 100 { 101 uint32_t byte0, byte1; 102 103 if (ldb_asm(addr, &byte0)) 104 return 1; 105 addr += 1; 106 if (ldb_asm(addr, &byte1)) 107 return 1; 108 109 byte0 |= byte1 << 8; 110 put_ptreg(regs, rz, byte0); 111 112 return 0; 113 } 114 115 /* 116 * Store half-word to [rx + imm] 117 * 118 * Success: return 0 119 * Failure: return 1 120 */ 121 static int sth_c(struct pt_regs *regs, uint32_t rz, uint32_t addr) 122 { 123 uint32_t byte0, byte1; 124 125 byte0 = byte1 = get_ptreg(regs, rz); 126 127 byte0 &= 0xff; 128 129 if (stb_asm(addr, byte0)) 130 return 1; 131 132 addr += 1; 133 byte1 = (byte1 >> 8) & 0xff; 134 if (stb_asm(addr, byte1)) 135 return 1; 136 137 return 0; 138 } 139 140 /* 141 * Get word from [rx + imm] 142 * 143 * Success: return 0 144 * Failure: return 1 145 */ 146 static int ldw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr) 147 { 148 uint32_t byte0, byte1, byte2, byte3; 149 150 if (ldb_asm(addr, &byte0)) 151 return 1; 152 153 addr += 1; 154 if (ldb_asm(addr, &byte1)) 155 return 1; 156 157 addr += 1; 158 if (ldb_asm(addr, &byte2)) 159 return 1; 160 161 addr += 1; 162 if (ldb_asm(addr, &byte3)) 163 return 1; 164 165 byte0 |= byte1 << 8; 166 byte0 |= byte2 << 16; 167 byte0 |= byte3 << 24; 168 169 put_ptreg(regs, rz, byte0); 170 171 return 0; 172 } 173 174 /* 175 * Store word to [rx + imm] 176 * 177 * Success: return 0 178 * Failure: return 1 179 */ 180 static int stw_c(struct pt_regs *regs, uint32_t rz, uint32_t addr) 181 { 182 uint32_t byte0, byte1, byte2, byte3; 183 184 byte0 = byte1 = byte2 = byte3 = get_ptreg(regs, rz); 185 186 byte0 &= 0xff; 187 188 if (stb_asm(addr, byte0)) 189 return 1; 190 191 addr += 1; 192 byte1 = (byte1 >> 8) & 0xff; 193 if (stb_asm(addr, byte1)) 194 return 1; 195 196 addr += 1; 197 byte2 = (byte2 >> 16) & 0xff; 198 if (stb_asm(addr, byte2)) 199 return 1; 200 201 addr += 1; 202 byte3 = (byte3 >> 24) & 0xff; 203 if (stb_asm(addr, byte3)) 204 return 1; 205 206 align_count++; 207 208 return 0; 209 } 210 211 extern int fixup_exception(struct pt_regs *regs); 212 213 #define OP_LDH 0xc000 214 #define OP_STH 0xd000 215 #define OP_LDW 0x8000 216 #define OP_STW 0x9000 217 218 void csky_alignment(struct pt_regs *regs) 219 { 220 int ret; 221 uint16_t tmp; 222 uint32_t opcode = 0; 223 uint32_t rx = 0; 224 uint32_t rz = 0; 225 uint32_t imm = 0; 226 uint32_t addr = 0; 227 228 if (!user_mode(regs)) 229 goto bad_area; 230 231 ret = get_user(tmp, (uint16_t *)instruction_pointer(regs)); 232 if (ret) { 233 pr_err("%s get_user failed.\n", __func__); 234 goto bad_area; 235 } 236 237 opcode = (uint32_t)tmp; 238 239 rx = opcode & 0xf; 240 imm = (opcode >> 4) & 0xf; 241 rz = (opcode >> 8) & 0xf; 242 opcode &= 0xf000; 243 244 if (rx == 0 || rx == 1 || rz == 0 || rz == 1) 245 goto bad_area; 246 247 switch (opcode) { 248 case OP_LDH: 249 addr = get_ptreg(regs, rx) + (imm << 1); 250 ret = ldh_c(regs, rz, addr); 251 break; 252 case OP_LDW: 253 addr = get_ptreg(regs, rx) + (imm << 2); 254 ret = ldw_c(regs, rz, addr); 255 break; 256 case OP_STH: 257 addr = get_ptreg(regs, rx) + (imm << 1); 258 ret = sth_c(regs, rz, addr); 259 break; 260 case OP_STW: 261 addr = get_ptreg(regs, rx) + (imm << 2); 262 ret = stw_c(regs, rz, addr); 263 break; 264 } 265 266 if (ret) 267 goto bad_area; 268 269 regs->pc += 2; 270 271 return; 272 273 bad_area: 274 if (!user_mode(regs)) { 275 if (fixup_exception(regs)) 276 return; 277 278 bust_spinlocks(1); 279 pr_alert("%s opcode: %x, rz: %d, rx: %d, imm: %d, addr: %x.\n", 280 __func__, opcode, rz, rx, imm, addr); 281 show_regs(regs); 282 bust_spinlocks(0); 283 do_exit(SIGKILL); 284 } 285 286 force_sig_fault(SIGBUS, BUS_ADRALN, (void __user *)addr, current); 287 } 288 289 static struct ctl_table alignment_tbl[4] = { 290 { 291 .procname = "enable", 292 .data = &align_enable, 293 .maxlen = sizeof(align_enable), 294 .mode = 0666, 295 .proc_handler = &proc_dointvec 296 }, 297 { 298 .procname = "count", 299 .data = &align_count, 300 .maxlen = sizeof(align_count), 301 .mode = 0666, 302 .proc_handler = &proc_dointvec 303 }, 304 {} 305 }; 306 307 static struct ctl_table sysctl_table[2] = { 308 { 309 .procname = "csky_alignment", 310 .mode = 0555, 311 .child = alignment_tbl}, 312 {} 313 }; 314 315 static struct ctl_path sysctl_path[2] = { 316 {.procname = "csky"}, 317 {} 318 }; 319 320 static int __init csky_alignment_init(void) 321 { 322 register_sysctl_paths(sysctl_path, sysctl_table); 323 return 0; 324 } 325 326 arch_initcall(csky_alignment_init); 327