1 /* $NetBSD: copyin.c,v 1.2 2011/01/18 01:02:52 matt Exp $ */ 2 3 /*- 4 * Copyright (c) 2010, 2011 The NetBSD Foundation, Inc. 5 * All rights reserved. 6 * 7 * This code is derived from software contributed to The NetBSD Foundation 8 * by Raytheon BBN Technologies Corp and Defense Advanced Research Projects 9 * Agency and which was developed by Matt Thomas of 3am Software Foundry. 10 * 11 * This material is based upon work supported by the Defense Advanced Research 12 * Projects Agency and Space and Naval Warfare Systems Center, Pacific, under 13 * Contract No. N66001-09-C-2073. 14 * Approved for Public Release, Distribution Unlimited 15 * 16 * Redistribution and use in source and binary forms, with or without 17 * modification, are permitted provided that the following conditions 18 * are met: 19 * 1. Redistributions of source code must retain the above copyright 20 * notice, this list of conditions and the following disclaimer. 21 * 2. Redistributions in binary form must reproduce the above copyright 22 * notice, this list of conditions and the following disclaimer in the 23 * documentation and/or other materials provided with the distribution. 24 * 25 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS 26 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 27 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 28 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS 29 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 */ 37 38 #include <sys/cdefs.h> 39 __KERNEL_RCSID(0, "$NetBSD: copyin.c,v 1.2 2011/01/18 01:02:52 matt Exp $"); 40 41 #include <sys/param.h> 42 #include <sys/lwp.h> 43 44 #include <machine/pcb.h> 45 46 static inline uint8_t 47 copyin_byte(const uint8_t * const usaddr8, register_t ds_msr) 48 { 49 register_t msr; 50 uint8_t data; 51 __asm volatile( 52 "mfmsr %[msr]; " /* Save MSR */ 53 "mtmsr %[ds_msr]; sync; isync; " /* DS on */ 54 "lbz %[data],0(%[usaddr8]); " /* fetch user byte */ 55 "mtmsr %[msr]; sync; isync; " /* DS off */ 56 : [msr] "=&r" (msr), [data] "=r" (data) 57 : [ds_msr] "r" (ds_msr), [usaddr8] "b" (usaddr8)); 58 return data; 59 } 60 61 static inline uint16_t 62 copyin_halfword(const uint16_t * const usaddr16, register_t ds_msr) 63 { 64 register_t msr; 65 uint16_t data; 66 __asm volatile( 67 "mfmsr %[msr]; " /* Save MSR */ 68 "mtmsr %[ds_msr]; sync; isync; " /* DS on */ 69 "lhz %[data],0(%[usaddr16]); " /* fetch user byte */ 70 "mtmsr %[msr]; sync; isync; " /* DS off */ 71 : [msr] "=&r" (msr), [data] "=r" (data) 72 : [ds_msr] "r" (ds_msr), [usaddr16] "b" (usaddr16)); 73 return data; 74 } 75 76 static inline uint32_t 77 copyin_word(const uint32_t * const usaddr32, register_t ds_msr) 78 { 79 register_t msr; 80 uint32_t data; 81 __asm volatile( 82 "mfmsr %[msr]; " /* Save MSR */ 83 "mtmsr %[ds_msr]; sync; isync; " /* DS on */ 84 "lwz %[data],0(%[usaddr32]); " /* load user byte */ 85 "mtmsr %[msr]; sync; isync; " /* DS off */ 86 : [msr] "=&r" (msr), [data] "=r" (data) 87 : [ds_msr] "r" (ds_msr), [usaddr32] "b" (usaddr32)); 88 return data; 89 } 90 91 static inline uint32_t 92 copyin_word_bswap(const uint32_t * const usaddr32, register_t ds_msr) 93 { 94 register_t msr; 95 uint32_t data; 96 __asm volatile( 97 "mfmsr %[msr]; " /* Save MSR */ 98 "mtmsr %[ds_msr]; sync; isync; " /* DS on */ 99 "lwbrx %[data],0,%[usaddr32]; " /* load user LE word */ 100 "mtmsr %[msr]; sync; isync; " /* DS off */ 101 : [msr] "=&r" (msr), [data] "=r" (data) 102 : [ds_msr] "r" (ds_msr), [usaddr32] "b" (usaddr32)); 103 return data; 104 } 105 106 static inline void 107 copyin_8words(const uint32_t *usaddr32, uint32_t *kdaddr32, register_t ds_msr) 108 { 109 register_t msr; 110 //uint32_t data[8]; 111 __asm volatile( 112 "mfmsr %[msr]" /* Save MSR */ 113 "\n\t" "mtmsr %[ds_msr]; sync; isync" /* DS on */ 114 "\n\t" "lwz %[data0],0(%[usaddr32])" /* fetch user data */ 115 "\n\t" "lwz %[data1],4(%[usaddr32])" /* fetch user data */ 116 "\n\t" "lwz %[data2],8(%[usaddr32])" /* fetch user data */ 117 "\n\t" "lwz %[data3],12(%[usaddr32])" /* fetch user data */ 118 "\n\t" "lwz %[data4],16(%[usaddr32])" /* fetch user data */ 119 "\n\t" "lwz %[data5],20(%[usaddr32])" /* fetch user data */ 120 "\n\t" "lwz %[data6],24(%[usaddr32])" /* fetch user data */ 121 "\n\t" "lwz %[data7],28(%[usaddr32])" /* fetch user data */ 122 "\n\t" "mtmsr %[msr]; sync; isync" /* DS off */ 123 : [msr] "=&r" (msr), 124 [data0] "=&r" (kdaddr32[0]), [data1] "=&r" (kdaddr32[1]), 125 [data2] "=&r" (kdaddr32[2]), [data3] "=&r" (kdaddr32[3]), 126 [data4] "=&r" (kdaddr32[4]), [data5] "=&r" (kdaddr32[5]), 127 [data6] "=&r" (kdaddr32[6]), [data7] "=&r" (kdaddr32[7]) 128 : [ds_msr] "r" (ds_msr), [usaddr32] "b" (usaddr32)); 129 } 130 131 static inline void 132 copyin_16words(const uint32_t *usaddr32, uint32_t *kdaddr32, register_t ds_msr) 133 { 134 register_t msr; 135 __asm volatile( 136 "mfmsr %[msr]" /* Save MSR */ 137 "\n\t" "mtmsr %[ds_msr]; sync; isync" /* DS on */ 138 "\n\t" "lwz %[data0],0(%[usaddr32])" /* fetch user data */ 139 "\n\t" "lwz %[data1],4(%[usaddr32])" /* fetch user data */ 140 "\n\t" "lwz %[data2],8(%[usaddr32])" /* fetch user data */ 141 "\n\t" "lwz %[data3],12(%[usaddr32])" /* fetch user data */ 142 "\n\t" "lwz %[data4],16(%[usaddr32])" /* fetch user data */ 143 "\n\t" "lwz %[data5],20(%[usaddr32])" /* fetch user data */ 144 "\n\t" "lwz %[data6],24(%[usaddr32])" /* fetch user data */ 145 "\n\t" "lwz %[data7],28(%[usaddr32])" /* fetch user data */ 146 "\n\t" "lwz %[data8],32(%[usaddr32])" /* fetch user data */ 147 "\n\t" "lwz %[data9],36(%[usaddr32])" /* fetch user data */ 148 "\n\t" "lwz %[data10],40(%[usaddr32])" /* fetch user data */ 149 "\n\t" "lwz %[data11],44(%[usaddr32])" /* fetch user data */ 150 "\n\t" "lwz %[data12],48(%[usaddr32])" /* fetch user data */ 151 "\n\t" "lwz %[data13],52(%[usaddr32])" /* fetch user data */ 152 "\n\t" "lwz %[data14],56(%[usaddr32])" /* fetch user data */ 153 "\n\t" "lwz %[data15],60(%[usaddr32])" /* fetch user data */ 154 "\n\t" "mtmsr %[msr]; sync; isync" /* DS off */ 155 : [msr] "=&r" (msr), 156 [data0] "=&r" (kdaddr32[0]), [data1] "=&r" (kdaddr32[1]), 157 [data2] "=&r" (kdaddr32[2]), [data3] "=&r" (kdaddr32[3]), 158 [data4] "=&r" (kdaddr32[4]), [data5] "=&r" (kdaddr32[5]), 159 [data6] "=&r" (kdaddr32[6]), [data7] "=&r" (kdaddr32[7]), 160 [data8] "=&r" (kdaddr32[8]), [data9] "=&r" (kdaddr32[9]), 161 [data10] "=&r" (kdaddr32[10]), [data11] "=&r" (kdaddr32[11]), 162 [data12] "=&r" (kdaddr32[12]), [data13] "=&r" (kdaddr32[13]), 163 [data14] "=&r" (kdaddr32[14]), [data15] "=&r" (kdaddr32[15]) 164 : [ds_msr] "r" (ds_msr), [usaddr32] "b" (usaddr32)); 165 } 166 static inline void 167 copyin_bytes(vaddr_t usaddr, vaddr_t kdaddr, size_t len, register_t ds_msr) 168 { 169 const uint8_t *usaddr8 = (void *)usaddr; 170 uint8_t *kdaddr8 = (void *)kdaddr; 171 while (len-- > 0) { 172 *kdaddr8++ = copyin_byte(usaddr8++, ds_msr); 173 } 174 } 175 176 static inline void 177 copyin_words(vaddr_t usaddr, vaddr_t kdaddr, size_t len, register_t ds_msr) 178 { 179 KASSERT((kdaddr & 3) == 0); 180 KASSERT((usaddr & 3) == 0); 181 const uint32_t *usaddr32 = (void *)usaddr; 182 uint32_t *kdaddr32 = (void *)kdaddr; 183 len >>= 2; 184 while (len >= 16) { 185 copyin_16words(usaddr32, kdaddr32, ds_msr); 186 usaddr32 += 16, kdaddr32 += 16, len -= 16; 187 } 188 KASSERT(len < 16); 189 if (len >= 8) { 190 copyin_8words(usaddr32, kdaddr32, ds_msr); 191 usaddr32 += 8, kdaddr32 += 8, len -= 8; 192 } 193 while (len-- > 0) { 194 *kdaddr32++ = copyin_word(usaddr32++, ds_msr); 195 } 196 } 197 198 int 199 copyin(const void *vusaddr, void *vkdaddr, size_t len) 200 { 201 struct pcb * const pcb = lwp_getpcb(curlwp); 202 struct faultbuf env; 203 vaddr_t usaddr = (vaddr_t) vusaddr; 204 vaddr_t kdaddr = (vaddr_t) vkdaddr; 205 206 if (__predict_false(len == 0)) { 207 return 0; 208 } 209 210 const register_t ds_msr = mfmsr() | PSL_DS; 211 212 int rv = setfault(&env); 213 if (rv != 0) { 214 pcb->pcb_onfault = NULL; 215 return rv; 216 } 217 218 if (__predict_false(len < 4)) { 219 copyin_bytes(usaddr, kdaddr, len, ds_msr); 220 pcb->pcb_onfault = NULL; 221 return 0; 222 } 223 224 const size_t alignment = (usaddr ^ kdaddr) & 3; 225 if (__predict_true(alignment == 0)) { 226 size_t slen; 227 if (__predict_false(kdaddr & 3)) { 228 slen = 4 - (kdaddr & 3); 229 copyin_bytes(usaddr, kdaddr, slen, ds_msr); 230 usaddr += slen, kdaddr += slen, len -= slen; 231 } 232 slen = len & ~3; 233 if (__predict_true(slen >= 4)) { 234 copyin_words(usaddr, kdaddr, slen, ds_msr); 235 usaddr += slen, kdaddr += slen, len -= slen; 236 } 237 } 238 if (len > 0) { 239 copyin_bytes(usaddr, kdaddr, len, ds_msr); 240 } 241 pcb->pcb_onfault = NULL; 242 return 0; 243 } 244 245 int 246 copyinstr(const void *usaddr, void *kdaddr, size_t len, size_t *done) 247 { 248 struct pcb * const pcb = lwp_getpcb(curlwp); 249 struct faultbuf env; 250 251 if (__predict_false(len == 0)) { 252 if (done) 253 *done = 0; 254 return 0; 255 } 256 257 int rv = setfault(&env); 258 if (rv != 0) { 259 pcb->pcb_onfault = NULL; 260 if (done) 261 *done = 0; 262 return rv; 263 } 264 265 const register_t ds_msr = mfmsr() | PSL_DS; 266 const uint32_t *usaddr32 = (const void *)((uintptr_t)usaddr & ~3); 267 uint8_t *kdaddr8 = kdaddr; 268 size_t copylen, wlen; 269 uint32_t data; 270 size_t uoff = (uintptr_t)usaddr & 3; 271 wlen = 4 - uoff; 272 /* 273 * We need discard any leading bytes if the address was 274 * unaligned. We read the words byteswapped so that the LSB 275 * contains the lowest address byte. 276 */ 277 data = copyin_word_bswap(usaddr32++, ds_msr) >> (8 * uoff); 278 for (copylen = 0; copylen < len; copylen++, wlen--, data >>= 8) { 279 if (wlen == 0) { 280 /* 281 * If we've depleted the data in the word, fetch the 282 * next one. 283 */ 284 data = copyin_word_bswap(usaddr32++, ds_msr); 285 wlen = 4; 286 } 287 *kdaddr8++ = data; 288 if ((uint8_t) data == 0) { 289 copylen++; 290 break; 291 } 292 } 293 294 pcb->pcb_onfault = NULL; 295 if (done) 296 *done = copylen; 297 return 0; 298 } 299