;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; RDOS operating system
; Copyright (C) 1988-2011, Leif Ekblad
;
; This program is free software; you can redistribute it and/or modify
; it under the terms of the GNU General Public License as published by
; the Free Software Foundation; either version 2 of the License, or
; (at your option) any later version. The only exception to this rule
; is for commercial usage in embedded systems. For information on
; usage in commercial embedded systems, contact embedded@rdos.net
;
; This program is distributed in the hope that it will be useful,
; but WITHOUT ANY WARRANTY; without even the implied warranty of
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
; GNU General Public License for more details.
;
; You should have received a copy of the GNU General Public License
; along with this program; if not, write to the Free Software
; Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
;
; The author of this program may be contacted at leif@rdos.net
;
; freqbase.ASM
; 
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

include \rdos\kernel\user.def
include \rdos\kernel\user.inc

.386p

BUF_SHIFT   = 9

BUF_SIZE   = 10000h
BUF_MASK   = 0FFFFh

adc_in_entry   STRUC

aie_amp     DW ?,?
aie_phase   DW ?,?
aie_diff    DW ?
aie_level   DW ?

adc_in_entry   ENDS

adc_in_chan_entry  STRUC

aicd_chan    DW ?
aicd_count   DW ?
aicd_data00 adc_in_entry <>
aicd_data01 adc_in_entry <>
aicd_data02 adc_in_entry <>
aicd_data03 adc_in_entry <>
aicd_data04 adc_in_entry <>
aicd_data05 adc_in_entry <>
aicd_data06 adc_in_entry <>
aicd_data07 adc_in_entry <>
aicd_data08 adc_in_entry <>
aicd_data09 adc_in_entry <>
aicd_data10 adc_in_entry <>
aicd_data11 adc_in_entry <>
aicd_data12 adc_in_entry <>
aicd_data13 adc_in_entry <>
aicd_data14 adc_in_entry <>
aicd_data15 adc_in_entry <>
aicd_data16 adc_in_entry <>
aicd_data17 adc_in_entry <>
aicd_data18 adc_in_entry <>

adc_in_chan_entry  ENDS

adc_in_data    STRUC

aid_time    DD ?,?
aid_level   DW 11 DUP(?)
aid_low     adc_in_chan_entry <>
aid_high    adc_in_chan_entry <>

adc_in_data    ENDS

adc_out_data   STRUC

aod_time        DD ?,?
aod_low_freq    DD ?
aod_high_freq   DD ?
aod_mse         DD ?
aod_low_phase   DW ?,?
aod_high_phase  DW ?,?
aod_amp         DW ?

adc_out_data   ENDS

_DATA   segment use32 word public 'DATA'

buf_read    DD ?
buf_write   DD ?
buf_data    DD ?

periods     DD ?
sd_fact     DT ?
freq_arr    DD 11 DUP(?)

val         DD ?

_DATA   ends

;;;;;;;;; INTERNAL PROCEDURES ;;;;;;;;;;;

_TEXT   segment use32 word public 'CODE'

    assume  cs:_TEXT

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           InitFreq
;
;       DESCRIPTION:    Init freq
;
;       PARAMETERS:     EAX                          Periods
;                       ESI                          Freq array
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

    public InitFreq_

f4   DD 4

InitFreq_    Proc near
    push ecx
    push esi
    push edi
;
    mov periods,eax
    fldpi
    fild word ptr f4
    fdivp
    fild word ptr periods
    fmulp
    fstp sd_fact
;
    mov ecx,11
    mov edi,OFFSET freq_arr
    rep movsd
;
    mov buf_read,0
    mov buf_write,0
;
    mov eax,BUF_SIZE SHL BUF_SHIFT
    UserGateApp allocate_circ_buf_nr
    mov buf_data,edx
;
    pop edi
    pop esi
    pop ecx
    ret
InitFreq_    Endp

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           CalcPhase
;
;       DESCRIPTION:    Calc phase 
;
;       PARAMETERS:     EBX           In chan entry
;
;       RETURNS:        EAX           Phase 0 
;                       EDX           Phase 1
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

CalcPhase    Proc near
    movzx ecx,[ebx].aicd_count
    or ecx,ecx
    jnz cpDo
;
    mov eax,buf_read
    mov edx,buf_write
    int 3
    mov eax,buf_read
    mov edx,buf_write
    jmp CalcPhase

cpDo:
    lea ebp,[ebx].aicd_data00
    xor eax,eax
    xor edx,edx
    push ecx

cpLoop:
    movzx ebx,[ebp].aie_phase
    add eax,ebx
;
    movzx ebx,[ebp].aie_phase+2
    add edx,ebx
;
    add ebp,SIZE adc_in_entry
    loop cpLoop
;
    pop ecx
;
    mov ebp,edx
    xor edx,edx
    div ecx
    xchg eax,ebp
    xor edx,edx
    div ecx
    mov edx,eax
    mov eax,ebp
    ret
CalcPhase   Endp

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           CalcFreq
;
;       DESCRIPTION:    Calc freq
;
;       PARAMETERS:     ESI           In entry
;
;       RETURNS:        ST(0)         Freq
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

CalcFreq    Proc near
    lea ebx,[esi].aid_low
    call CalcPhase
    mov [edi].aod_low_phase,ax
    mov [edi].aod_low_phase+2,dx
;
    movsx eax,ax
    movsx edx,dx
    add eax,edx
    sar eax,1
    movzx ebx,[esi].aid_low.aicd_chan
    mov ecx,[4*ebx].freq_arr
    imul ecx
    shrd eax,edx,15
    cdq
    mov ebx,periods
    idiv ebx
    add eax,ecx
    mov [edi].aod_low_freq,eax
;
    lea ebx,[esi].aid_high
    call CalcPhase
    mov [edi].aod_high_phase,ax
    mov [edi].aod_high_phase+2,dx
;
    movsx eax,ax
    movsx edx,dx
    add eax,edx
    sar eax,1
    movzx ebx,[esi].aid_high.aicd_chan
    mov ecx,[4*ebx].freq_arr
    imul ecx
    shrd eax,edx,15
    cdq
    mov ebx,periods
    idiv ebx
    add eax,ecx
    mov [edi].aod_high_freq,eax
;
    add eax,[edi].aod_low_freq
    shr eax,1
    mov val,eax
    fild val
    ret
CalcFreq    Endp

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           CalcAmp
;
;       DESCRIPTION:    Calc amp
;
;       PARAMETERS:     ST(0)         f0
;                       ESI           In entry
;                       EDI           Out entry
;                       EDX           Meassured freq
;
;       RETURNS:        ST(0)         Amp
;                       ST(1)         f0
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

CalcAmp    Proc near
    movzx eax,[esi].aid_low.aicd_chan
    fild [4*eax].freq_arr
;
    movzx eax,[esi].aid_high.aicd_chan
    fild [4*eax].freq_arr
;
    fld st(2)
    fsub st(0), st(2)
    fld st(1)
    fsub st(0), st(3)
    fdivp
;
    fld st(3)
    fsub st(0), st(3) 
    fdiv st(0), st(3)
    fld sd_fact
    fmulp
    fmul st(0), st(0)
    fchs
;
    fldl2e
    fmulp st(1), st(0)
    fld1
    fld st(1)
    fprem
    f2xm1
    faddp
    fscale
    fstp st(1)
;
    movzx eax,[esi].aid_low.aicd_chan
    fild [esi+2*eax].aid_level
    fdiv st(0), st(1)
    fstp st(1)
    fld1
    fsub st(0), st(2)
    fmul st(0), st(1)
    fstp st(1)
;
    fld st(4)
    fsub st(0), st(3) 
    fdiv st(0), st(3)
    fld sd_fact
    fmulp
    fmul st(0), st(0)
    fchs
;
    fldl2e
    fmulp st(1), st(0)
    fld1
    fld st(1)
    fprem
    f2xm1
    faddp
    fscale
    fstp st(1)
;
    movzx eax,[esi].aid_high.aicd_chan
    fild [esi+2*eax].aid_level
    fdiv st(0), st(1)
    fstp st(1)
    fmul st(0), st(2)
    faddp
    fstp st(1)
    fstp st(1)
    fstp st(1)
    ret
CalcAmp    Endp

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           CalcAmpError
;
;       DESCRIPTION:    Calc amp error
;
;       PARAMETERS:     ST(0)         Amp
;                       ESI           In entry
;
;       RETURNS:        ST(0)         Error
;                       ST(1)         Amp
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

f11   DD 11

CalcAmpError    Proc near
    fldz
;
    mov ecx,11
    xor ebx,ebx

cfeLoop:
    fild [esi+2*ebx].aid_level
    fild [4*ebx].freq_arr
    fld st(0)
    fsub st(0), st(5) 
    fdivrp
    fld sd_fact
    fmulp
    fmul st(0), st(0)
    fchs
;
    fldl2e
    fmulp st(1), st(0)
    fld1
    fld st(1)
    fprem
    f2xm1
    faddp
    fscale
    fstp st(1)
;
    fmul st(0), st(3)
    fsubp
    fmul st(0), st(0)
    faddp
;    
    inc ebx
    loop cfeLoop
;
    fild f11
    fdivp
    fsqrt
;
    ret
CalcAmpError    Endp

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           ReadFreq
;
;       DESCRIPTION:    Read freq
;
;       PARAMETERS:     EDI            Data 
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

    public ReadFreq_

ReadFreq_    Proc near
    pushad

rrRetry:
    mov eax,buf_write
    cmp eax,buf_read
    jne rrData
;
    mov ax,10
    UserGateApp wait_milli_nr
    jmp rrRetry

rrData:
    mov esi,buf_read
    shl esi,BUF_SHIFT
    add esi,buf_data
;
    call CalcFreq
    call CalcAmp
    call CalcAmpError
;
    fistp [edi].aod_mse
    fistp [edi].aod_amp
    fstp st(0)
;
    mov eax,buf_read
    inc eax
    and eax,BUF_MASK
    mov buf_read,eax
;
    popad
    ret
ReadFreq_    Endp

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           GetFreqBuf
;
;       DESCRIPTION:    Get freq buf
;
;       RETURNS:        EAX            Buffer ptr
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

    public GetFreqBuf_

GetFreqBuf_    Proc near
    push ecx
;
    mov ecx,buf_write
    mov eax,ecx
    inc ecx
    and ecx,BUF_MASK
    cmp ecx,buf_read
    jne gbOk
;
    xor eax,eax
    jmp gbDone

gbOk:
    shl eax,BUF_SHIFT
    add eax,buf_data
	
gbDone:
    pop ecx
    ret
GetFreqBuf_    Endp

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;       
;
;       NAME:           PostFreq
;
;       DESCRIPTION:    Post freq
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

    public PostFreq_

PostFreq_    Proc near
    push eax
;
    mov eax,buf_write
    inc eax
    and eax,BUF_MASK
    mov buf_write,eax
;
    pop eax
    ret
PostFreq_    Endp


_TEXT   ends

    END