570 lines
15 KiB
Text
570 lines
15 KiB
Text
/* ARM assembly AARCH64 Raspberry PI 3B */
|
|
/* program diffAngle64.s */
|
|
|
|
/************************************/
|
|
/* Constantes */
|
|
/************************************/
|
|
.include "../includeConstantesARM64.inc"
|
|
|
|
/*********************************/
|
|
/* Initialized data */
|
|
/*********************************/
|
|
.data
|
|
szCarriageReturn: .asciz "\n"
|
|
szMessResult: .asciz "Difference between @ and @ = @ \n"
|
|
|
|
.align 8
|
|
fB1: .double 0F20.0
|
|
fB2: .double 0F45.0
|
|
fB3: .double 0F-45.0
|
|
fB4: .double 0F-85.0
|
|
fB5: .double 90.0
|
|
fB6: .double -95.0
|
|
fB7: .double 125.0
|
|
fB8: .double 145.0
|
|
fB9: .double 0F29.4803
|
|
fB10: .double 0F-88.6381
|
|
fB11: .double 0F-78.3251
|
|
fB12: .double 0F-159.036
|
|
fB13: .double 0F-70099.74233810938
|
|
fB14: .double 0F29840.67437876723
|
|
|
|
/*********************************/
|
|
/* UnInitialized data */
|
|
/*********************************/
|
|
.bss
|
|
sZoneConv: .skip 24
|
|
/*********************************/
|
|
/* code section */
|
|
/*********************************/
|
|
.text
|
|
.global main
|
|
main:
|
|
ldr x0,qAdrfB1
|
|
ldr x1,qAdrfB2
|
|
bl testComputeAngle
|
|
//b 100f
|
|
ldr x0,qAdrfB3
|
|
ldr x1,qAdrfB2
|
|
bl testComputeAngle
|
|
|
|
ldr x0,qAdrfB4
|
|
ldr x1,qAdrfB5
|
|
bl testComputeAngle
|
|
|
|
ldr x0,qAdrfB6
|
|
ldr x1,qAdrfB5
|
|
bl testComputeAngle
|
|
|
|
ldr x0,qAdrfB3
|
|
ldr x1,qAdrfB7
|
|
bl testComputeAngle
|
|
|
|
ldr x0,qAdrfB3
|
|
ldr x1,qAdrfB8
|
|
bl testComputeAngle
|
|
|
|
ldr x0,qAdrfB9
|
|
ldr x1,qAdrfB10
|
|
bl testComputeAngle
|
|
|
|
ldr x0,qAdrfB11
|
|
ldr x1,qAdrfB12
|
|
bl testComputeAngle
|
|
|
|
ldr x0,qAdrfB13
|
|
ldr x1,qAdrfB14
|
|
bl testComputeAngle
|
|
|
|
100: // standard end of the program
|
|
mov x0, #0 // return code
|
|
mov x8,EXIT
|
|
svc #0 // perform the system call
|
|
qAdrszCarriageReturn: .quad szCarriageReturn
|
|
qAdrsZoneConv: .quad sZoneConv
|
|
qAdrfB1: .quad fB1
|
|
qAdrfB2: .quad fB2
|
|
qAdrfB3: .quad fB3
|
|
qAdrfB4: .quad fB4
|
|
qAdrfB5: .quad fB5
|
|
qAdrfB6: .quad fB6
|
|
qAdrfB7: .quad fB7
|
|
qAdrfB8: .quad fB8
|
|
qAdrfB9: .quad fB9
|
|
qAdrfB10: .quad fB10
|
|
qAdrfB11: .quad fB11
|
|
qAdrfB12: .quad fB12
|
|
qAdrfB13: .quad fB13
|
|
qAdrfB14: .quad fB14
|
|
/******************************************************************/
|
|
/* compute difference and display result */
|
|
/******************************************************************/
|
|
/* s0 contains bearing 1 */
|
|
/* s1 contains bearing 2 */
|
|
testComputeAngle:
|
|
stp x1,lr,[sp,-16]! // save registers
|
|
stp x2,x3,[sp,-16]! // save registers
|
|
ldr d0,[x0]
|
|
fmov d2,d0
|
|
ldr d1,[x1]
|
|
bl computeDiffAngle
|
|
fmov d3,d0
|
|
fmov d0,d2
|
|
ldr x0,qAdrsZoneConv
|
|
bl convertirFloat
|
|
ldr x0,qAdrszMessResult
|
|
ldr x1,qAdrsZoneConv
|
|
bl strInsertAtCharInc
|
|
mov x3,x0
|
|
fmov d0,d1
|
|
ldr x0,qAdrsZoneConv
|
|
bl convertirFloat
|
|
mov x0,x3
|
|
ldr x1,qAdrsZoneConv
|
|
bl strInsertAtCharInc
|
|
mov x3,x0
|
|
fmov d0,d3
|
|
ldr x0,qAdrsZoneConv
|
|
bl convertirFloat
|
|
mov x0,x3
|
|
ldr x1,qAdrsZoneConv
|
|
bl strInsertAtCharInc
|
|
bl affichageMess
|
|
|
|
100:
|
|
ldp x2,x3,[sp],16 // restaur registers
|
|
ldp x1,lr,[sp],16 // restaur registers
|
|
ret
|
|
qAdrszMessResult: .quad szMessResult
|
|
/******************************************************************/
|
|
/* compute difference of two bearing */
|
|
/******************************************************************/
|
|
/* d0 contains bearing 1 */
|
|
/* d1 contains bearing 2 */
|
|
computeDiffAngle:
|
|
stp x1,lr,[sp,-16]! // save registers
|
|
stp x2,x3,[sp,-16]! // save registers
|
|
stp x4,x5,[sp,-16]! // save registers
|
|
stp d1,d2,[sp,-16]! // save registres
|
|
stp d3,d4,[sp,-16]! // save registres
|
|
mov x1,#360
|
|
mov x4,#0 // top positive/negative
|
|
fcvtzs d4,d0 // conversion.integer
|
|
scvtf d2,d4 // conversion float
|
|
fsub d2,d0,d2 // partie décimale
|
|
fmov x0,d4 // partie entière
|
|
cmp x0,#0 // negative ?
|
|
bge 1f
|
|
neg x0,x0 // yes -> inversion
|
|
mov x4,#1
|
|
1:
|
|
udiv x2,x0,x1 // divide by 360
|
|
msub x3,x2,x1,x0
|
|
cmp x4,#0 // value negative ?
|
|
neg x5,x3
|
|
csel x3,x5,x3,ne // inversion remainder
|
|
fmov d3,x3
|
|
scvtf d3,d3 // and conversion float
|
|
fadd d0,d3,d2 // add decimal part
|
|
|
|
mov x4,#0 // bearing 2
|
|
fcvtzs d4,d1 // conversion integer
|
|
scvtf d2,d4 // conversion float
|
|
fsub d2,d1,d2 // partie décimale
|
|
fmov x0,d4
|
|
cmp x0,#0
|
|
bge 2f
|
|
neg x0,x0
|
|
mov x4,#1
|
|
2:
|
|
udiv x2,x0,x1 // divide by 360
|
|
msub x3,x2,x1,x0
|
|
cmp x4,#0
|
|
neg x5,x3
|
|
csel x3,x5,x3,ne // inversion remainder
|
|
fmov d3,x3
|
|
scvtf d3,d3 // conversion float
|
|
fadd d1,d3,d2
|
|
|
|
fsub d0,d1,d0 // calculate the difference between the 2 values
|
|
|
|
mov x0,180
|
|
fmov d3,x0
|
|
scvtf d3,d3 // conversion float 180
|
|
fmov d4,x1 // 360
|
|
scvtf d4,d4 // conversion float 360
|
|
fcmp d0,#0.0 // difference is negative ?
|
|
blt 2f
|
|
// difference is positive
|
|
fcmp d0,d4 // difference > 360
|
|
ble 3f
|
|
fsub d0,d0,d4 // yes -> difference - 360
|
|
3:
|
|
fcmp d0,d3 // compare difference and 180
|
|
ble 100f
|
|
fsub d0,d4,d0 // > 180 calculate 360 - difference
|
|
fneg d0,d0 // and negate
|
|
b 100f
|
|
|
|
2: // différence is négative
|
|
fneg d2,d4 // -360
|
|
fcmp d0,d2 // compare différence et - 360
|
|
ble 3f
|
|
fsub d0,d0,d4 // sub 360 to différence
|
|
3:
|
|
fneg d3,d3 // -180
|
|
fcmp d0,d3 // compare difference and -180
|
|
bge 100f
|
|
fadd d0,d4,d0 // calculate 360 + différence
|
|
|
|
100:
|
|
ldp d3,d4,[sp],16 // restaur registers
|
|
ldp d1,d2,[sp],16 // restaur registers
|
|
ldp x4,x5,[sp],16 // restaur registers
|
|
ldp x2,x3,[sp],16 // restaur registers
|
|
ldp x1,lr,[sp],16 // restaur registers
|
|
ret
|
|
|
|
/******************************************************************/
|
|
/* Conversion Float */
|
|
/******************************************************************/
|
|
/* d0 contains Float */
|
|
/* x0 contains address conversion area mini 20 charactèrs */
|
|
/* x0 return result length */
|
|
/* see https://blog.benoitblanchon.fr/lightweight-float-to-string/ */
|
|
convertirFloat:
|
|
stp x1,lr,[sp,-16]! // save registres
|
|
stp x2,x3,[sp,-16]! // save registres
|
|
stp x4,x5,[sp,-16]! // save registres
|
|
stp x6,x7,[sp,-16]! // save registres
|
|
stp x8,x9,[sp,-16]! // save registres
|
|
stp d1,d2,[sp,-16]! // save registres
|
|
mov x6,x0 // save area address
|
|
fmov x0,d0
|
|
mov x8,#0 // result length
|
|
mov x3,#'+'
|
|
strb w3,[x6] // signe + forcing
|
|
mov x2,x0
|
|
tbz x2,63,1f
|
|
mov x2,1
|
|
lsl x2,x2,63
|
|
bic x0,x0,x2
|
|
mov x3,#'-' // sign -
|
|
strb w3,[x6]
|
|
1:
|
|
adds x8,x8,#1 // next position
|
|
cmp x0,#0 // case 0 positive or negative
|
|
bne 2f
|
|
mov x3,#'0'
|
|
strb w3,[x6,x8] // store character 0
|
|
adds x8,x8,#1
|
|
strb wzr,[x6,x8] // store 0 final
|
|
mov x0,x8 // return length
|
|
b 100f
|
|
2:
|
|
ldr x2,iMaskExposant
|
|
mov x1,x0
|
|
and x1,x1,x2 // exposant
|
|
cmp x1,x2
|
|
bne 4f
|
|
tbz x0,51,3f // test bit 51 to zéro
|
|
mov x2,#'N' // case Nan. store byte no possible store integer
|
|
strb w2,[x6] // area no aligned
|
|
mov x2,#'a'
|
|
strb w2,[x6,#1]
|
|
mov x2,#'n'
|
|
strb w2,[x6,#2]
|
|
mov x2,#0 // 0 final
|
|
strb w2,[x6,#3]
|
|
mov x0,#3
|
|
b 100f
|
|
3: // case infini positive or négative
|
|
mov x2,#'I'
|
|
strb w2,[x6,x8]
|
|
adds x8,x8,#1
|
|
mov x2,#'n'
|
|
strb w2,[x6,x8]
|
|
adds x8,x8,#1
|
|
mov x2,#'f'
|
|
strb w2,[x6,x8]
|
|
adds x8,x8,#1
|
|
mov x2,#0
|
|
strb w2,[x6,x8]
|
|
mov x0,x8
|
|
b 100f
|
|
4:
|
|
bl normaliserFloat
|
|
mov x5,x0 // save exposant
|
|
fcvtzu d2,d0
|
|
fmov x0,d2 // part integer
|
|
scvtf d1,d2 // conversion float
|
|
fsub d1,d0,d1 // extraction part fractional
|
|
ldr d2,dConst1
|
|
fmul d1,d2,d1 // to crop it in full
|
|
fcvtzu d1,d1 // convertion integer
|
|
fmov x4,d1 // fract value
|
|
// conversion part integer to x0
|
|
mov x2,x6 // save address begin area
|
|
adds x6,x6,x8
|
|
mov x1,x6
|
|
bl conversion10
|
|
add x6,x6,x0
|
|
mov x3,#','
|
|
strb w3,[x6]
|
|
adds x6,x6,#1
|
|
|
|
mov x0,x4 // conversion part fractionnaire
|
|
mov x1,x6
|
|
bl conversion10SP
|
|
add x6,x6,x0
|
|
sub x6,x6,#1
|
|
// remove trailing zeros
|
|
5:
|
|
ldrb w0,[x6]
|
|
cmp w0,#'0'
|
|
bne 6f
|
|
sub x6,x6,#1
|
|
b 5b
|
|
6:
|
|
cmp w0,#','
|
|
bne 7f
|
|
sub x6,x6,#1
|
|
7:
|
|
cmp x5,#0 // if exposant = 0 no display
|
|
bne 8f
|
|
add x6,x6,#1
|
|
b 10f
|
|
8:
|
|
add x6,x6,#1
|
|
mov x3,#'E'
|
|
strb w3,[x6]
|
|
add x6,x6,#1
|
|
mov x0,x5 // conversion exposant
|
|
mov x3,x0
|
|
tbz x3,63,9f // exposant negative ?
|
|
neg x0,x0
|
|
mov x3,#'-'
|
|
strb w3,[x6]
|
|
adds x6,x6,#1
|
|
9:
|
|
mov x1,x6
|
|
bl conversion10
|
|
add x6,x6,x0
|
|
10:
|
|
strb wzr,[x6] // store 0 final
|
|
adds x6,x6,#1
|
|
mov x0,x6
|
|
subs x0,x0,x2 // retour de la longueur de la zone
|
|
subs x0,x0,#1 // sans le 0 final
|
|
|
|
100:
|
|
ldp d1,d2,[sp],16 // restaur registres
|
|
ldp x8,x9,[sp],16 // restaur registres
|
|
ldp x6,x7,[sp],16 // restaur registres
|
|
ldp x4,x5,[sp],16 // restaur registres
|
|
ldp x2,x3,[sp],16 // restaur registres
|
|
ldp x1,lr,[sp],16 // restaur registres
|
|
ret
|
|
|
|
iMaskExposant: .quad 0x7FF<<52
|
|
dConst1: .double 0f1E17
|
|
|
|
/***************************************************/
|
|
/* normaliser float */
|
|
/***************************************************/
|
|
/* x0 contain float value (always positive value and <> Nan) */
|
|
/* d0 return new value */
|
|
/* x0 return exposant */
|
|
normaliserFloat:
|
|
stp x1,lr,[sp,-16]! // save registers
|
|
fmov d0,x0 // value float
|
|
mov x0,#0 // exposant
|
|
ldr d1,dConstE7 // no normalisation for value < 1E7
|
|
fcmp d0,d1
|
|
blo 10f // if d0 < dConstE7
|
|
|
|
ldr d1,dConstE256
|
|
fcmp d0,d1
|
|
blo 1f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#256
|
|
1:
|
|
|
|
ldr d1,dConstE128
|
|
fcmp d0,d1
|
|
blo 1f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#128
|
|
1:
|
|
ldr d1,dConstE64
|
|
fcmp d0,d1
|
|
blo 1f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#64
|
|
1:
|
|
ldr d1,dConstE32
|
|
fcmp d0,d1
|
|
blo 1f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#32
|
|
1:
|
|
ldr d1,dConstE16
|
|
fcmp d0,d1
|
|
blo 2f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#16
|
|
2:
|
|
ldr d1,dConstE8
|
|
fcmp d0,d1
|
|
blo 3f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#8
|
|
3:
|
|
ldr d1,dConstE4
|
|
fcmp d0,d1
|
|
blo 4f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#4
|
|
4:
|
|
ldr d1,dConstE2
|
|
fcmp d0,d1
|
|
blo 5f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#2
|
|
5:
|
|
ldr d1,dConstE1
|
|
fcmp d0,d1
|
|
blo 10f
|
|
fdiv d0,d0,d1
|
|
adds x0,x0,#1
|
|
|
|
10:
|
|
ldr d1,dConstME5 // pas de normalisation pour les valeurs > 1E-5
|
|
fcmp d0,d1
|
|
bhi 100f // fin
|
|
|
|
ldr d1,dConstME255
|
|
fcmp d0,d1
|
|
bhi 11f
|
|
ldr d1,dConstE256
|
|
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#256
|
|
11:
|
|
|
|
ldr d1,dConstME127
|
|
fcmp d0,d1
|
|
bhi 11f
|
|
ldr d1,dConstE128
|
|
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#128
|
|
11:
|
|
|
|
ldr d1,dConstME63
|
|
fcmp d0,d1
|
|
bhi 11f
|
|
ldr d1,dConstE64
|
|
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#64
|
|
11:
|
|
|
|
ldr d1,dConstME31
|
|
fcmp d0,d1
|
|
bhi 11f
|
|
ldr d1,dConstE32
|
|
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#32
|
|
11:
|
|
ldr d1,dConstME15
|
|
fcmp d0,d1
|
|
bhi 12f
|
|
ldr d1,dConstE16
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#16
|
|
12:
|
|
ldr d1,dConstME7
|
|
fcmp d0,d1
|
|
bhi 13f
|
|
ldr d1,dConstE8
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#8
|
|
13:
|
|
ldr d1,dConstME3
|
|
fcmp d0,d1
|
|
bhi 14f
|
|
ldr d1,dConstE4
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#4
|
|
14:
|
|
ldr d1,dConstME1
|
|
fcmp d0,d1
|
|
bhi 15f
|
|
ldr d1,dConstE2
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#2
|
|
15:
|
|
ldr d1,dConstE0
|
|
fcmp d0,d1
|
|
bhi 100f
|
|
ldr d1,dConstE1
|
|
fmul d0,d0,d1
|
|
subs x0,x0,#1
|
|
|
|
100: // fin standard de la fonction
|
|
ldp x1,lr,[sp],16 // restaur registres
|
|
ret
|
|
.align 2
|
|
dConstE7: .double 0f1E7
|
|
dConstE256: .double 0f1E256
|
|
dConstE128: .double 0f1E128
|
|
dConstE64: .double 0f1E64
|
|
dConstE32: .double 0f1E32
|
|
dConstE16: .double 0f1E16
|
|
dConstE8: .double 0f1E8
|
|
dConstE4: .double 0f1E4
|
|
dConstE2: .double 0f1E2
|
|
dConstE1: .double 0f1E1
|
|
dConstME5: .double 0f1E-5
|
|
dConstME255: .double 0f1E-255
|
|
dConstME127: .double 0f1E-127
|
|
dConstME63: .double 0f1E-63
|
|
dConstME31: .double 0f1E-31
|
|
dConstME15: .double 0f1E-15
|
|
dConstME7: .double 0f1E-7
|
|
dConstME3: .double 0f1E-3
|
|
dConstME1: .double 0f1E-1
|
|
dConstE0: .double 0f1E0
|
|
|
|
/******************************************************************/
|
|
/* Décimal Conversion */
|
|
/******************************************************************/
|
|
/* x0 contain value et x1 address conversion area */
|
|
conversion10SP:
|
|
stp x1,lr,[sp,-16]! // save registers
|
|
stp x2,x3,[sp,-16]! // save registers
|
|
stp x4,x5,[sp,-16]! // save registers
|
|
mov x5,x1
|
|
mov x4,#16
|
|
mov x2,x0
|
|
mov x1,#10 // décimal conversion
|
|
1: // conversion loop
|
|
mov x0,x2 // copy begin number or quotient
|
|
udiv x2,x0,x1 // division by 10
|
|
msub x3,x1,x2,x0 // compute remainder
|
|
add x3,x3,#48 // compute digit
|
|
strb w3,[x5,x4] // store byte address area (x5) + offset (x4)
|
|
subs x4,x4,#1 // position precedente
|
|
bge 1b
|
|
strb wzr,[x5,16] // 0 final
|
|
100:
|
|
ldp x4,x5,[sp],16 // restaur registers
|
|
ldp x2,x3,[sp],16 // restaur registers
|
|
ldp x1,lr,[sp],16 // restaur registers
|
|
ret
|
|
/***************************************************/
|
|
/* ROUTINES INCLUDE */
|
|
/***************************************************/
|
|
.include "../includeARM64.inc"
|