// so kann 'thumb2'- und 'arm'-Syntax verwendet werden und Assembler nimmt // bei Varianten den 'besseren' Befehl .syntax unified // **************** Konstantendefinitionen (nur hier sichtbar) ******************* @ Anzahl Samples für einen DMA, entspricht Dezimationsfaktor .set ADC_BUFLEN, 20 @ Länge der Sinustabelle in Bits (also Exponent der Länge als Zweierpotenz) .set AngleBits, 12 .set SinTabLength, 2<LIFCR = 32 @ Pointer auf Samplebuffer bestimmen, landet in r4 @ Hier muss der Buffer ausgewertet werden, der im letzten DMA beschrieben wurde ldr r1, [r0, #16] @ r1 = DMA2_Stream0->CR ldr r4, = ADC_DMA_Buffer @ r4 = Startadresse des DMA-Buffers ands r1, r1, #0x80000 @ 'CT'-Bit prüfen, zeigt an, welcher DMA-Buffer frei ist it eq // falls CT-Bit gelöscht ist, wird die zweite Bufferhälfte verwendet, also halbe Länge in Bytes // zur Startadresse addiert addeq r4, r4, #ADC_BUFLEN*2 @********* numerischer Oszillator und I/Q-Mischer ******************** mov r0, #ADC_BUFLEN @ Schleifenzähler ldr r1, = NCO_Frequenz ldr r2, = SinusTemp ldm r1!, {r7, r12, lr} @ r7 = Frequenz (Inkrement), r12 = Frequenz-Offset, @ lr = Phase, r1 zeigt danach auf Sinustabelle add r12, r12, r7 @ Frequenz-Offset addieren NCO_LOOP: add lr, lr, r12 @ Phase aufaddieren @ signifikante Bits des Winkels zur Startadresse der Sinustabelle addieren, @ dabei beachten, dass Tabellenwerte 32 Bit gross sind (16 Bit cos und 16 Bit sin), @ also vierfachen Wert addieren add r11, r1, lr, lsr 32-AngleBits-3 bic r11, r11, #3 @ Bit 0 und 1 löschen, damit Alignment stimmt (abrunden vom Winkel) ldr r7, [r11, #8] @ Cos und Sin lesen, untere 16 Bit von r7 = cos, obere 16 Bit von r7 = sin @ Sample lesen ldrh r5, [r4], #2 @ r5 = Sample sub r5, r5, #2048 @ Offset beseitigen smulbb r6, r5, r7 @ r6 = Sample * cos = I smulbt r7, r5, r7 @ r7 = Sample * sin = Q stm r2!, {r6, r7} @ I und Q speichern subs r0, r0, #1 @ Schleifenzähler dekrementieren bne NCO_LOOP @ Sprung zum Anfang des Loops, solange r0 > 0 str lr, [r1, #-4] @ Endwinkel speichern @******* Cos-Loop über die Samples vom DMA (ADC_BUFLEN), also Berechnung des I-Samples @ Vorbereitung ldr r1, = CIC_IntDiff ldr r0, = SinusTemp ldmia r1, {r2-r11} mov r12, #ADC_BUFLEN @ Schleifenzähler @ Loop, r2..r11 sind Integratoren, r0 zeigt auf Eingangs-Sample, @ r1 und lr für temporäre Werte CIC_Loop_Cos: @ ********* CIC-Filter ******************************* ldr lr, [r0], #8 @ I-Sample lesen // 1. Integratorstufe, Eingangssample zum 64-Bit-Integrator addieren @ Sample bei Addition auf 16 Bit umskalieren, Eingang hat nur 12 Bits, @ daher nur 12 Bits nach rechts schieben adds r2, r2, lr, asr #12 adc r3, r3, lr, asr #31 @ für High-Word nur Vorzeichen addieren // 2. Integratorstufe, 1. Integrator (64 Bit) addieren adds r4, r4, r2 adc r5, r5, r3 // 3. Integratorstufe, 2. Integrator addieren adds r6, r6, r4 adc r7, r7, r5 // 4. Integratorstufe, 3. Integrator addieren adds r8, r8, r6 adc r9, r9, r7 // 5. Integratorstufe, 4. Integrator addieren adds r10, r10, r8 adc r11, r11, r9 subs r12, #1 // Schleifenzähler dekrementieren bne CIC_Loop_Cos // Rücksprung an Loopanfang, wenn Zähler > 0 @ Integratoren speichern, r1 zeigt danach auf Differezierer stmia r1!, {r2-r11} @ Differenzieren, r10, r11 ist Input (Resultat der 5. Integratorstufe nach letztem Loopdurchgang) ldmia r1, {r2-r9, r12, lr} @ Differenzierer laden in r2..r9 und r12,lr (5 Differenzierer zu je 64 Bit, also 2 Register) stmia r1!, {r10, r11} @ neuen Input speichern, r1 zeigt danach auf den nächsten Differezierer subs r10, r10, r2 @ r10/r11 = Input - alter Input (64 Bit Subtraktion, daher 2 Befehle) sbc r11, r11, r3 stmia r1!, {r10, r11} @ neuen Input speichern, r1 zeigt danach auf den nächsten Differezierer subs r10, r10, r4 @ r10/r11 = Input - alter Input sbc r11, r11, r5 stmia r1!, {r10, r11} @ neuen Input speichern, r1 zeigt danach auf den nächsten Differezierer subs r10, r10, r6 @ r10/r11 = Input - alter Input sbc r11, r11, r7 stmia r1!, {r10, r11} @ neuen Input speichern, r1 zeigt danach auf den nächsten Differezierer subs r10, r10, r8 @ r10/r11 = Input - alter Input sbc r11, r11, r9 stmia r1!, {r10, r11} @ neuen Input speichern, r1 zeigt danach auf den nächsten Differezierer subs r10, r10, r12 @ r10/r11 = Input - alter Input sbc r11, r11, lr // Gain rückgängig machen, 9 Bit nach rechts schieben, // 64 Bit gross, daher zwei Befehle, Resultat hat in 32 Bit Platz // Gain: Mischer: 0.5 (16 Bit), CIC: knapp 25 Bit -> Resultat hat 16 - 1 + 25 Bit = 40 Bit // Somit hat man durch Rechtsschieben um 9 Bit ein Resultat mit 31 Bits und kann 16 Bit breite // Samples noch verarbeiten (falls ein externer 16bit-Wandler eingesetzt wird). mov r10, r10, lsr #9 orr r0, r10, r11, lsl #23 //11 @ r0 ist umskalierter, dezimierter I-Wert, auf Stack sichern push {r0} @******* Sin-Loop über die Samples vom DMA (ADC_BUFLEN), identisch zu cos-Loop @ Vorbereitung, r1 ist von oben korrekt, sin schliesst an cos an ldr r0, = SinusTemp+4 ldmia r1, {r2-r11} mov r12, #ADC_BUFLEN @ Schleifenzähler @ Loop, r2..r11 sind Integratoren, r0 zeigt auf Eingangs-Sample, r1 und lr sind frei CIC_Loop_Sin: @ ********* CIC-Filter ******************************* ldr lr, [r0], #8 @ Q-Sample lesen adds r2, r2, lr, asr #12 @ auf 16 Bit umskalieren adc r3, r3, lr, asr #31 adds r4, r4, r2 adc r5, r5, r3 adds r6, r6, r4 adc r7, r7, r5 adds r8, r8, r6 adc r9, r9, r7 adds r10, r10, r8 adc r11, r11, r9 subs r12, #1 bne CIC_Loop_Sin @ Integratoren speichern, r1 zeigt danach auf Differezierer stmia r1!, {r2-r11} @ Differenzieren, r10, r11 ist Input ldmia r1, {r2-r9, r12, lr} @ Differenzierer laden stmia r1!, {r10, r11} @ neuen Input speichern subs r10, r10, r2 @ r10/r11 = Input - alter Input sbc r11, r11, r3 stmia r1!, {r10, r11} @ neuen Input speichern subs r10, r10, r4 @ r10/r11 = Input - alter Input sbc r11, r11, r5 stmia r1!, {r10, r11} @ neuen Input speichern subs r10, r10, r6 @ r10/r11 = Input - alter Input sbc r11, r11, r7 stmia r1!, {r10, r11} @ neuen Input speichern subs r10, r10, r8 @ r10/r11 = Input - alter Input sbc r11, r11, r9 stmia r1!, {r10, r11} @ neuen Input speichern subs r10, r10, r12 @ r10/r11 = Input - alter Input sbc r11, r11, lr // Gain teilweise rückgängig machen, 9 Bit nach rechts schieben mov r10, r10, lsr #9 //21 orr r0, r10, r11, lsl #23 //11 @ r0 ist umskalierter, dezimierter Q-Wert pop {r1} // I-Wert vom Stack holen @ hier ist I in r1 und Q in r0 // *************** I/Q-Filterung ************** .global IIR_LP_PARAM .global Filter .global Biq_StateIQ // s12 = Q, s13 = I (Integer) vmov s12, s13, r0, r1 vcvt.F32.S32 s0, s13 // s0 = float(I) ldr r1, = Biq_StateIQ ldr r2, = Filter ldr r0, = IIR_LP_PARAM ldr r2, [r2] mov r3, #84 @ 21 Filterkoeffizienten zu 4 Bytes pro Filter mla r0, r2, r3, r0 @ r0 = Filter * 21 (*4) + &IIR_LP_PARAM = Adresse Filterkoeffizienten vldm r0!, {s14} @ s14 = Gain push {r0} @ Koeffizientenadresse sichern, wird für Q noch gebraucht // BiQuad-Berechnung, r0 und r1 werden automatisch inkrementiert, s0 ist jeweils in und out bl BiQuad_Stage_asm bl BiQuad_Stage_asm bl BiQuad_Stage_asm bl BiQuad_Stage_asm vmul.F32 s13, s0, s14 @ s13 = Gain * Filter-Output = gefiltertes I // Filterung Q pop {r0} @ Pointer auf Koeffizienten vcvt.F32.S32 s0, s12 // s0 = float(Q) // BiQuad-Berechnung, r0 und r1 werden automatisch inkrementiert, s0 ist jeweils in und out bl BiQuad_Stage_asm bl BiQuad_Stage_asm bl BiQuad_Stage_asm bl BiQuad_Stage_asm vmul.F32 s12, s0, s14 @ s13 = Gain * Filter-Output = gefiltertes I // Betrag bilden vmul.F32 s0, s13, s13 @ s0 = I^2 vmul.F32 s1, s12, s12 @ s1 = Q^2 vadd.F32 s0, s0, s1 @ s0 = I^2 + Q^2 vsqrt.F32 s0, s0 @ s0 = sqrt(I^2 + Q^2) // I und Q sichern .global ZF_IQ ldr r4, = ZF_IQ vstm r4!, {s12, s13} .global BFO_Frequenz // SSB-Demodulation ldr r2, = BFO_Frequenz ldm r2!, {r12, lr} @ r12 = Frequenz (Inkrement), lr = Phase ldr r1, = NCO_Sinus @ r1 zeigt auf Sinustabelle add lr, lr, r12 @ Phase aufaddieren str lr, [r2, #-4] @ Endwinkel speichern @ signifikante Bits des Winkels zur Startadresse der Sinustabelle addieren, @ dabei beachten, dass Tabellenwerte 32 Bit gross sind (16 Bit cos und 16 Bit sin), @ also vierfachen Wert addieren add r11, r1, lr, lsr 32-AngleBits-3 bic r11, r11, #3 @ Bit 0 und 1 löschen, damit Alignment stimmt (abrunden vom Winkel) ldr r7, [r11, #8] @ Cos und Sin lesen, untere 16 Bit von r7 = cos, obere 16 Bit von r7 = sin @ Multiplikation I*cos und Q*sin SXTH r0, r7 @ r0 = cos SXTH r1, r7, ror #16 @ r1 = sin vmov s4, s5, r0, r1 vcvt.F32.S32 s4, s4 // s4 = float(cos) vcvt.F32.S32 s5, s5 // s5 = float(sin) vmul.F32 s4, s4, s13 // s4 = I*cos vmul.F32 s5, s5, s12 // s5 = Q*sin vadd.F32 s2, s4, s5 // s2 = I*cos + Q*sin = USB vsub.F32 s3, s4, s5 // s3 = I*cos - Q*sin = LSB // Signale speichern vstm r4, {s2, s3} // Register restaurieren und Rücksprung pop {r4-r12, pc} // globales Label, damit Funktion von C aus aufgerufen werden kann .global BiQuad_Stage_asm BiQuad_Stage_asm: /* float BiQuad_Stage(float input, float *Biq_Par, float *Biq_State) { float y; y = Biq_Par[0] * input + Biq_Par[1] * Biq_State[0] + Biq_Par[2] * Biq_State[1] - Biq_Par[3] * Biq_State[2] - Biq_Par[4] * Biq_State[3]; Biq_State[1] = Biq_State[0]; Biq_State[0] = input; Biq_State[3] = Biq_State[2]; Biq_State[2] = y; return y; */ // Übergabeparameter: s0 = input, r0 = Biq_Par, r1 = Biq_State // Rückgabewert muss in s0 stehen // Achtung! 'vldm' erlaubt im Gegensatz zu 'ldm' nur einen lückenlosen Registerbereich, daher // sind die Registernummern so gewählt, damit der neue Zustand mit möglichst wenig Umkopieren // der Register mit einem einzigen 'vstm' zurückgeschrieben werden kann. // Für die 'MAC'-Operationen ist die 'fused'-Variante etwas genaueer als die 'normale', da // das exakte Resultat der Multiplikation für die Addition verwendet wird und nicht das gerundete. vmov s6, s0 @ s6 = input vldm r0!, {s1-s5} @ s1 = b0, s2 = b1, usw. s5 = a2 vldm r1, {s7-s10} @ s7 = Biq_State(0), s8 = Biq_State(1), s9 = Biq_State(2), s10 = Biq_State(3) vmul.F32 s11, s6, s1 @ s11 = input * b0 vfma.F32 s11, s7, s2 @ s11 = input * b0 + state(0) * b1 vfma.F32 s11, s8, s3 @ s11 = input * b0 + state(0) * b1 + state(1) * b2 vfms.F32 s11, s9, s4 @ s11 = input * b0 + state(0) * b1 + state(1) * b2 - state(2) * a1 vfms.F32 s11, s10, s5 @ s11 = input * b0 + state(0) * b1 + state(1) * b2 - state(2) * a1 - state(3) * a2 = y @ neuen Zustand speichern vmov s8, s11 @ s8 = y, so kann mit einem stm der gesamte neue Zustand gesichert werden vstm r1!, {s6-s9} vmov s0, s8 @ Rückgabewert in s0 bx lr @ Return