1 /* 2 * Copyright (C) 2017 Imagination Technologies 3 * Author: Paul Burton <paul.burton@mips.com> 4 * 5 * This program is free software; you can redistribute it and/or modify it 6 * under the terms of the GNU General Public License as published by the 7 * Free Software Foundation; either version 2 of the License, or (at your 8 * option) any later version. 9 */ 10 11 #include <linux/bitops.h> 12 #include <asm/cmpxchg.h> 13 14 unsigned long __xchg_small(volatile void *ptr, unsigned long val, unsigned int size) 15 { 16 u32 old32, new32, load32, mask; 17 volatile u32 *ptr32; 18 unsigned int shift; 19 20 /* Check that ptr is naturally aligned */ 21 WARN_ON((unsigned long)ptr & (size - 1)); 22 23 /* Mask value to the correct size. */ 24 mask = GENMASK((size * BITS_PER_BYTE) - 1, 0); 25 val &= mask; 26 27 /* 28 * Calculate a shift & mask that correspond to the value we wish to 29 * exchange within the naturally aligned 4 byte integerthat includes 30 * it. 31 */ 32 shift = (unsigned long)ptr & 0x3; 33 if (IS_ENABLED(CONFIG_CPU_BIG_ENDIAN)) 34 shift ^= sizeof(u32) - size; 35 shift *= BITS_PER_BYTE; 36 mask <<= shift; 37 38 /* 39 * Calculate a pointer to the naturally aligned 4 byte integer that 40 * includes our byte of interest, and load its value. 41 */ 42 ptr32 = (volatile u32 *)((unsigned long)ptr & ~0x3); 43 load32 = *ptr32; 44 45 do { 46 old32 = load32; 47 new32 = (load32 & ~mask) | (val << shift); 48 load32 = cmpxchg(ptr32, old32, new32); 49 } while (load32 != old32); 50 51 return (load32 & mask) >> shift; 52 } 53 54 unsigned long __cmpxchg_small(volatile void *ptr, unsigned long old, 55 unsigned long new, unsigned int size) 56 { 57 u32 mask, old32, new32, load32, load; 58 volatile u32 *ptr32; 59 unsigned int shift; 60 61 /* Check that ptr is naturally aligned */ 62 WARN_ON((unsigned long)ptr & (size - 1)); 63 64 /* Mask inputs to the correct size. */ 65 mask = GENMASK((size * BITS_PER_BYTE) - 1, 0); 66 old &= mask; 67 new &= mask; 68 69 /* 70 * Calculate a shift & mask that correspond to the value we wish to 71 * compare & exchange within the naturally aligned 4 byte integer 72 * that includes it. 73 */ 74 shift = (unsigned long)ptr & 0x3; 75 if (IS_ENABLED(CONFIG_CPU_BIG_ENDIAN)) 76 shift ^= sizeof(u32) - size; 77 shift *= BITS_PER_BYTE; 78 mask <<= shift; 79 80 /* 81 * Calculate a pointer to the naturally aligned 4 byte integer that 82 * includes our byte of interest, and load its value. 83 */ 84 ptr32 = (volatile u32 *)((unsigned long)ptr & ~0x3); 85 load32 = *ptr32; 86 87 while (true) { 88 /* 89 * Ensure the byte we want to exchange matches the expected 90 * old value, and if not then bail. 91 */ 92 load = (load32 & mask) >> shift; 93 if (load != old) 94 return load; 95 96 /* 97 * Calculate the old & new values of the naturally aligned 98 * 4 byte integer that include the byte we want to exchange. 99 * Attempt to exchange the old value for the new value, and 100 * return if we succeed. 101 */ 102 old32 = (load32 & ~mask) | (old << shift); 103 new32 = (load32 & ~mask) | (new << shift); 104 load32 = cmpxchg(ptr32, old32, new32); 105 if (load32 == old32) 106 return old; 107 } 108 } 109