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