00001
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045 #include "dsp.h"
00046 #include "preprocessor.h"
00047
00048 #if !defined(FORCE_ALL_GENERICS) && \
00049 !defined(FORCE_GENERIC_OP16_SQRT) && \
00050 defined(TARGET_SPECIFIC_OP16_SQRT)
00051
00052 #define DSP16_SQRT_ONE_POINT_FIVE (3 << (DSP16_QB-1))
00053
00054 #define DSP16_SQRT_NEWTON_ITERATION(x_num, data) \
00055 "mul r1, r0, r0\n\t" \
00056 "mul r1, r12, r1\n\t" \
00057 "lsr r1, r1, "ASTRINGZ(DSP16_QB+1)"\n\t" \
00058 "sub r1, r2, r1\n\t" \
00059 "mul r0, r0, r1\n\t" \
00060 "lsr r0, r0, "ASTRINGZ(DSP16_QB)"\n\t"
00061
00062 #if (defined __GNUC__)
00063 # define ASM_INSTRUCT_COMPACKED(str) str
00064 # define ASM_INSTRUCT_EXTENDED(str) str
00065 #elif __ICCAVR32__
00066 # define ASM_INSTRUCT_COMPACKED(str) str":C"
00067 # define ASM_INSTRUCT_EXTENDED(str) str":E"
00068 #endif
00069
00070
00071 #if (defined __GNUC__)
00072 # define CST_ZERO "%[CST_ZERO__]"
00073 #elif __ICCAVR32__
00074 # define CST_ZERO ASTRINGZ(0)
00075 #endif
00076
00077
00078
00080 #if (defined __GNUC__)
00081 __attribute__((__naked__))
00082 __attribute__((__noinline__))
00083 #elif __ICCAVR32__
00084 # pragma shadow_registers=full
00085 # pragma optimize=none no_inline
00086 #endif
00087 dsp16_t dsp16_op_sqrt(dsp16_t num)
00088 {
00089 __asm__ __volatile__ (
00090 "pushm r0-r3, lr\n\t"
00091
00092
00093 "cp.w r12, 0\n\t"
00094 ASM_INSTRUCT_COMPACKED("brlt __dsp16_sqrt_end_err")"\n\t"
00095
00096
00097 "clz r0, r12\n\t"
00098 "lsr r0, r0, 1\n\t"
00099 "sub r0, r0, "ASTRINGZ(-(DSP16_QB-16))"\n\t"
00100 "mov r1, 1\n\t"
00101
00102
00103 "lsl r0, r1, r0\n\t"
00104
00105
00106 "mov r2, "ASTRINGZ(DSP16_SQRT_ONE_POINT_FIVE)"\n\t"
00107
00108
00109 MREPEAT(4, DSP16_SQRT_NEWTON_ITERATION, "")
00110
00111 #if (DSP16_QB%2 == 1)
00112
00113 "mov r2, "ASTRINGZ(46340 >> DSP16_QA)"\n\t"
00114 "mul r12, r12, r2\n\t"
00115 "lsr r12, r12, "ASTRINGZ(DSP16_QB)"\n\t"
00116 #endif
00117
00118
00119 "mul r12, r0, r12\n\t"
00120
00121 "lsr r12, r12, "ASTRINGZ(DSP16_QB >> 1)"\n"
00122
00123 "__dsp16_sqrt_end_fct:\n\t"
00124 "popm r0-r3, pc\n"
00125
00126
00127 "__dsp16_sqrt_end_err:\n\t"
00128 "mov r12, "CST_ZERO"\n\t"
00129 "popm r0-r3, pc\n\t"
00130 #if (defined __GNUC__)
00131 :
00132 :
00133 [CST_ZERO__] "i" (DSP16_Q(0.))
00134 #endif
00135 );
00136
00137 return 0;
00138 }
00139
00140 #endif
00141