1 /* 2 * Copyright (c) 2015, STMicroelectronics International N.V. 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright notice, 9 * this list of conditions and the following disclaimer. 10 * 11 * 2. Redistributions in binary form must reproduce the above copyright notice, 12 * this list of conditions and the following disclaimer in the documentation 13 * and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 19 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 * POSSIBILITY OF SUCH DAMAGE. 26 */ 27 28 /* struct lqr - stores qutient/remainder to handle divmod EABI interfaces. */ 29 struct lqr { 30 unsigned long long q; /* computed quotient */ 31 unsigned long long r; /* computed remainder */ 32 unsigned q_n; /* specficies if quotient shall be negative */ 33 unsigned r_n; /* specficies if remainder shall be negative */ 34 }; 35 36 static void ul_div_qr(unsigned long long numerator, 37 unsigned long long denominator, struct lqr *qr); 38 39 40 static void division_lqr(unsigned long long n, unsigned long long p, 41 struct lqr *qr) 42 { 43 unsigned long long i = 1, q = 0; 44 if (p == 0) { 45 qr->r = 0xFFFFFFFFFFFFFFFFULL; /* division by 0 */ 46 return; 47 } 48 49 while ((p >> 63) == 0) { 50 i = i << 1; /* count the max division steps */ 51 p = p << 1; /* increase p until it has maximum size*/ 52 } 53 54 while (i > 0) { 55 q = q << 1; /* write bit in q at index (size-1) */ 56 if (n >= p) { 57 n -= p; 58 q++; 59 } 60 p = p >> 1; /* decrease p */ 61 i = i >> 1; /* decrease remaining size in q */ 62 } 63 qr->r = n; 64 qr->q = q; 65 } 66 67 static void ul_div_qr(unsigned long long numerator, 68 unsigned long long denominator, struct lqr *qr) 69 { 70 71 division_lqr(numerator, denominator, qr); 72 73 /* negate quotient and/or remainder according to requester */ 74 if (qr->q_n) 75 qr->q = -qr->q; 76 if (qr->r_n) 77 qr->r = -qr->r; 78 } 79 80 struct asm_ulqr { 81 unsigned long long v0; 82 unsigned long long v1; 83 }; 84 85 /* called from assembly function __aeabi_uldivmod */ 86 void __ul_divmod(struct asm_ulqr *asm_ulqr); 87 void __ul_divmod(struct asm_ulqr *asm_ulqr) 88 { 89 unsigned long long numerator = asm_ulqr->v0; 90 unsigned long long denominator = asm_ulqr->v1; 91 struct lqr qr = { .q_n = 0, .r_n = 0 }; 92 93 ul_div_qr(numerator, denominator, &qr); 94 95 asm_ulqr->v0 = qr.q; 96 asm_ulqr->v1 = qr.r; 97 } 98 99 struct asm_lqr { 100 long long v0; 101 long long v1; 102 }; 103 104 /* called from assembly function __aeabi_ldivmod */ 105 void __l_divmod(struct asm_lqr *asm_lqr); 106 void __l_divmod(struct asm_lqr *asm_lqr) 107 { 108 long long numerator = asm_lqr->v0; 109 long long denominator = asm_lqr->v1; 110 struct lqr qr = { .q_n = 0, .r_n = 0 }; 111 112 if (((numerator < 0) && (denominator > 0)) || 113 ((numerator > 0) && (denominator < 0))) 114 qr.q_n = 1; /* quotient shall be negate */ 115 if (numerator < 0) { 116 numerator = -numerator; 117 qr.r_n = 1; /* remainder shall be negate */ 118 } 119 if (denominator < 0) 120 denominator = -denominator; 121 122 ul_div_qr(numerator, denominator, &qr); 123 124 asm_lqr->v0 = qr.q; 125 asm_lqr->v1 = qr.r; 126 } 127