home *** CD-ROM | disk | FTP | other *** search
/ QRZ! Ham Radio 6 / QRZ Ham Radio Callsign Database - Volume 6.iso / pc / files / dsp / 56100tar.z / 56100tar / 56100 / source.1 < prev    next >
Encoding:
Text File  |  1992-04-28  |  43.4 KB  |  1,138 lines
  1. ========================================================================
  2.                                               Program        Program
  3.          Benchmark                             Length         Length
  4.                                               in Icyc        in Words
  5.  
  6. B.2.1    Real Multiply                              3           3
  7. B.2.2    N Real Multiplies                         2N          11
  8. B.2.3    Real Update                                4           4
  9. B.2.4    N Real Updates                            3N          14
  10. B.2.5    N Term Real Convolution (FIR)             1N           9
  11. B.2.6    N Term Real*Complex Convolution           2N          15
  12. B.2.7    Complex Multiply                           6           6
  13. B.2.8    N Complex Multiplies                      4N          14
  14. B.2.9    Complex Update                             7           7
  15. B.2.10   N Complex Updates                         6N          18
  16. B.2.11   N Term Complex Convolution (FIR)          4N          14
  17. B.2.12   Nth Order Power Series                    1N          13
  18. B.2.13   2nd Order Real Biquad Filter              12          12
  19. B.2.14   N Cascaded 2nd Order Biquads              5N          23
  20. B.2.15   N Radix 2 FFT Butterflies                10N          13
  21. B.2.16   Adaptive LMS FIR                       2N+19          22
  22. B.2.17   FIr Lattice Filter                      4N+7          10
  23. B.2.18   All Pole Iir Lattice Filter            3N+11          14
  24. B.2.19   General Lattice Filter                 4N+12          15
  25. B.2.20   Normalized Lattice Filter              5N+11          15
  26. B.2.21   [1x3][3x3] Matrix Multiply                21          21
  27. B.2.22   [NxN][NxN] Matrix multiply            N3+7N2          25
  28. B.2.23   3x3 2-D FIR Kernel                        12          44
  29. B.2.24   Signed 16 Bit Result Divide               36          18
  30. B.2.25   Signed Integer Divide                     32
  31. B.2.26   Multiply 32/48-bit Fractions             4+8
  32.  
  33. B.3.1    Wave Generation Double Integration        2N          15
  34. B.3.2    Wave Generation 2nd Order Oscillator      4N          16
  35. B.3.3    Cascaded Transpose BIQUAD Cell            8N          15
  36. B.3.4    IIR nth Order Direct Form II Canonic      2N          11
  37. B.3.5    Find Index Of A Max Value In Array        3N          10
  38. B.3.6    PID Algorithm                              5           5
  39. B.3.7    Reed Solomon Main Loop                   18N          17
  40. B.3.8    N Double Precision Real Multiplies        9N          18
  41. B.3.9    Double Precision Autocorrelation          19
  42. ========================================================================
  43. APPENDIX B
  44. DSP BENCHMARKS
  45. B.1  INTRODUCTION
  46. Appendix B consists of a set of DSP Benchmarks intended to highlight
  47. the DSP56100 performance in various applications, show examples of
  48. programming techniques, and provide code fragments for user application
  49. programs. Additional code will be put on the Dr. Bub Electronic Bulletin
  50. Board System as it becomes available. The following table lists these
  51. Benchmark programs and provides an overview of the program's performance.
  52.  
  53. The assembly language source is organized into 5 columns as shown below.
  54.  
  55. Label   Opcode  Operands    Data Bus                 Comment
  56. FIR     MAC     X1,X0,A     X:(R0)+,X1  X:(R3)+,X0   ;Do each tap
  57.  
  58. The Label column is used for program entry points and end of loop
  59. indication. The Opcode column indicates the Data ALU, Address ALU or
  60. Program Controller operation to be performed. The Operands column
  61. specifies the operands to be used by the opcode. The Data Bus specifies
  62. an optional data transfer over the Data Bus and the addressing mode to
  63. be used. The Comment column is used for documentation purposes and does
  64. not affect the assembled code. The Opcode column must always be included
  65. in the source code. For each benchmark, the number of program words and
  66. instruction cycles are given.
  67.  
  68. The following equates are used in the benchmark programs.
  69.  
  70.             page      132
  71.             opt       cc
  72. ;define  section
  73. AD          EQU       0
  74. BD          EQU       $100
  75. bd          EQU       $100
  76. C           EQU       $200
  77. c           EQU       $200
  78. D           EQU       $300
  79. N           EQU       100
  80. AR          EQU       $300
  81. AI          EQU       $400
  82. OUTPUT      EQU       $500
  83. output      EQU       $FFF1
  84. INPUT       EQU       $501
  85. input       EQU       $FFF1
  86. W           EQU       0
  87. w           EQU       0
  88. H           EQU       0
  89. XM          EQU       0
  90. state       equ       0
  91. ntaps       equ       $10
  92. k           equ       0
  93. n           equ       32
  94. p           equ       10
  95. mask        equ       10
  96. image       equ       $40
  97. dividend    equ       .25
  98. divisor     equ       .5
  99. paddr       equ       0
  100. qaddr       equ       4
  101. w1          equ       0
  102. w2          equ       10
  103. s           equ       0
  104. tablebase   equ       0
  105. lpc         equ       8
  106. frame       equ       0
  107. cor         equ       $100
  108. shift       equ       $80       ;shift constant
  109. table       equ       $180      ;base address of a-law table
  110.             org       p:$40
  111.  
  112. B.2 FIRST SET OF BENCHMARKS
  113. ========================================================================
  114. B.2.1   Real Multiply
  115. ;c = a * b
  116.  
  117.         move            x:(r0)+n0,x1    x:(r3)+n3,x0
  118.         mpyr    x1,x0,a
  119.         move            a,x:(r1)
  120.  
  121. ========================================================================
  122. B.2.2   N Real Multiplies
  123. ;c(I) = a(I) * b(I), I=1,...,N
  124.  
  125.         move    #AD,r0
  126.         move    #BD,r3
  127.         move    #C,r2
  128.         move              x:(r0)+,y0  x:(r3)+,x0
  129.         do      #N,END_DO2
  130.         mpyr    y0,x0,a   x:(r0)+,y0  x:(r3)+,x0
  131.         move    a,x:(r2)+
  132. END_DO2
  133.  
  134. ========================================================================
  135. B.2.3   Real Update
  136. ;d = c + a * b
  137.  
  138.         move              x:(r0)+n0,x1  x:(r3)+n3,x0
  139.         move              x:(r2),a
  140.         macr    x1,x0,a
  141.         move              a,x:(r1)
  142.  
  143. ========================================================================
  144. B.2.4   N Real Updates
  145. ;d(I) = c(I) + a(I) * b(I), I=1,...,N
  146.  
  147.         move    #AD,r0
  148.         move    #BD,r3
  149.         move    #C,r2
  150.         move    #D,r1
  151.         move              x:(r0)+,y0  x:(r3)+,x0
  152.         do      #N,END_DO4
  153.         move              x:(r2)+,a
  154.         macr    y0,x0,a   x:(r0)+,y0  x:(r3)+,x0
  155.         move              a,x:(r1)+
  156. END_DO4
  157.  
  158. ========================================================================
  159. B.2.5   Real Correlation Or Convolution (FIR Filter) 
  160. ;c(n) = SUM(I=0,...,N-1) {a(I) * b(n-I)} 
  161.  
  162.         move    #AD,r0
  163.         move    #BD,r3
  164.         clr     a         x:(r0)+,y0
  165.         move              x:(r3)+,x0
  166.         rep     #N
  167.         mac     y0,x0,a   x:(r0)+,y0  x:(r3)+,x0
  168.         rnd     a
  169.  
  170. ========================================================================
  171. B.2.6   Real * Complex Correlation Or Convolution (FIR Filter)
  172. ;cr(n) + jci(n) = SUM(I=0,...,N-1) {(ar(I) + jai(I)) * b(n-I)} 
  173. ;cr(n) = SUM(I=0,...,N-1) {ar(I) * b(n-I)}
  174. ;ci(n) = SUM(I=0,...,N-1) {ai(I) * b(n-I)}
  175.  
  176.         move    #AR,r0
  177.         move    #AI,r1
  178.         move    #BD,r3
  179.         clr     a         x:(r0)+,x1
  180.         clr     b         x:(r1)+,y1
  181.         move              x:(r3)+,x0
  182.         do      #N,END_DO6
  183.         mac     x0,x1,a   x:(r0)+,x1
  184.         mac     x0,y1,b   x:(r1)+,y1  x:(r3)+,x0
  185. END_DO6
  186.         rnd     a
  187.         rnd     b
  188.  
  189. ========================================================================
  190. B.2.7   Complex Multiply
  191.  
  192.         move              x:(r0)+,y1  x:(r3)+,x1    ;ar br
  193.         mpy     y1,x1,a   x:(r0)+,y0  x:(r3)+,x0    ;ar*br, ai, bi
  194.         macr   -y0,x0,a                             ;ar*br-ai*bi
  195.         mpy     y1,x0,b   a,x:(r2)+                 ;ar*bi
  196.         macr    y1,x0,b                             ;ar*bi+ai*br
  197.         move              b,x:(r2)+
  198.  
  199. ========================================================================
  200. B.2.8   N Complex Multiplies
  201. ;   cr(I) + jci(I) = (ar(I) + jai(I)) * (br(I) + jbi(I)), I=1,...,N
  202. ;   cr(I) = ar(I) * br(I) - ai(I) * bi(I)            y1=ar    x1=br
  203. ;   ci(I) = ar(I) * bi(I) + ai(I) * br(I)            y0=ai    x0=bi
  204.  
  205.         move    #AD,r0
  206.         move    #C-1,r2
  207.         move    #BD,r3
  208.         move              x:(r2),b                  ;dummy move!
  209.         move              x:(r0)+,y1  x:(r3)+,x1    ;ar;br
  210.         do      #N,END_DO8
  211.         mpy     y1,x1,a   x:(r0)+,y0  x:(r3)+,x0    ;ar*br, ai, bi
  212.         macr   -y0,x0,a   b,x:(r2)+                 ;ar*br-ai*bi
  213.         mpy     y0,x1,b   a,x:(r2)+                 ;ai*br
  214.         macr    y1,x0,b   x:(r0)+,y1  x:(r3)+,x1    ;ar*bi+ai*br, ar
  215. END_DO8
  216.         move              b,x:(r2)+
  217.  
  218. ========================================================================
  219. B.2.9   Complex Update
  220.  
  221.         move              x:(r2)+,a                  ;cr
  222.         move              x:(r0)+,y1  x:(r3)+,x1
  223.         mac     y1,x1,a   x:(r0)+,y0  x:(r3)+,x0     ;cr+ar*br,ai,bi
  224.         macr   -y0,x0,a   x:(r2)+,b                  ;cr+ar*br ai*bi
  225.         mac     y1,x0,b   a,x:(r1)+                  ;ci+ar*bi
  226.         macr    y0,x1,b                              ;ci+ar*bi+ai*br
  227.         move              b,x:(r1)+
  228.  
  229. ========================================================================
  230. B.2.10  N Complex Updates
  231.  
  232.         move    #AD,r0
  233.         move    #BD,r3
  234.         move    #D-1,r1
  235.         move    #C,r2
  236.         move              x:(r1),b                  ;dummy in B
  237.         move              x:(r0)+,y1                ;ar
  238.         do      #N,END_DOA
  239.         move              x:(r2)+,a   x:(r3)+,x0    ;cr,br
  240.         mac     y1,x0,a   x:(r0)+,y0  x:(r3)+,x1    ;cr+ar*br, ai, bi
  241.         macr   -y0,x1,a   b,x:(r1)+                 ;cr+ar*br ai*bi
  242.         move              x:(r2)+,b                 ;ci
  243.         mpy     y1,x1,b   a,x:(r1)+                 ;ci+ar*bi, dr
  244.         macr    y0,x0,b   x:(r0)+,y1                ;ci+ar*bi+ai*br
  245. END_DOA
  246.         move              b,x:(r1)+
  247.  
  248. ========================================================================
  249. B.2.11  Complex Correlation Or Convolution (Complex FIR)
  250. ;   cr(n) + jci(n) =
  251. ;       SUM(I=0,...,N-1) {(ar(I) + jai(I)) * (br(n-I) + jbi(n-I))}
  252. ;   cr(n) = SUM(I=0,...,N-1) {ar(I) * br(n-I) - ai(I) * bi(n-I)}
  253. ;   ci(n) = SUM(I=0,...,N-1) {ar(I) * bi(n-I) + ai(I) * br(n-I)}
  254. ;       y1=ar  x1=br
  255. ;       y0=ai  x0=bi
  256.  
  257.         move    #AD,r0
  258.         move    #BD,r3
  259.         clr     a         x:(r0)+,y1                ;ar
  260.         clr     b         x:(r3)+,x1                ;br
  261.         do      #N,END_DOB
  262.         mac     y1,x1,a   x:(r0)+,y0  x:(r3)+,x0    ;ar*br, ai, bi
  263.         mac     y1,x0,b                             ;ar*bi
  264.         mac     y0,x1,b   x:(r0)+,y1  x:(r3)+,x1    ;ar*bi+ai*br, ar
  265.         mac    -y0,x0,a                             ;ar*br-ai*bi
  266. END_DOB
  267.         rnd     a
  268.         rnd     b
  269.  
  270. ========================================================================
  271. B.2.12  Nth Order Power Series (Real)
  272. ;   c = SUM(I=0,...,N) {a(I) * b**I}
  273. ;     = [[[a(n) *b+a(n-1)] *b+a(n-2)]*b+a(n-3)]...
  274.  
  275.         move    #BD,r1
  276.         move    #AD,r0
  277.         move              x:(r1),y0                ;b
  278.         move              y0,x0
  279.         move              x:(r0)+,a                ;a(n)
  280.         move              x:(r0)+,b                ;a(n-1)
  281.         do      #N/2,END_DOC
  282.         mac     a1,y0,b   x:(r0)+,a                ;a(n-2)
  283.         mac     b1,x0,a   x:(r0)+,b                ;a(0)+a(1)*b
  284. END_DOC
  285.         rnd     a
  286.  
  287. ========================================================================
  288. B.2.13  2nd Order Real Biquad IIR Filter
  289. ;   w(n)/2 = x(n)/2 - (a1/2) * w(n-1) - (a2/2) * w(n-2)
  290. ;   y(n)/2 = w(n)/2 + (b1/2) * w(n-1) + (b2/2) * w(n-2)
  291.  
  292. ;   DHigh Memory Order - w(n-2), w(n-1)
  293. ;   DLow Memory Order - (a2/2), (a1/2), (b2/2), (b1/2)
  294.  
  295. ;   this version uses two pointers
  296.  
  297.         move    #-1,n0
  298.         ori     #$08,mr
  299.         rnd     a         x:(r3)+,x1                  ;x1=a2/2
  300.         move              x:(r0)+,x0                  ;x0=wn-2
  301.         mac     y1,x0,a   x:(r0)+n0,y1  x:(r3)+,x1    ;y1=wn-1
  302.         mac     y1,x1,a   x1,x:(r0)+                  ;a=wn
  303.         move              x:(r3)+,x1                  ;x1=b2/2
  304.         mac     x1,x0,a   a,x:(r0)+
  305.         move              x:(r3)+,x1                  ;x1=b1/2
  306.         macr    y1,x1,a
  307.  
  308.         move              a,x:<<output
  309.  
  310. ========================================================================
  311. B.2.14  N Cascaded Real Biquad IIR Filters
  312.  
  313. ;   w(n)/2 = x(n)/2 - (a1/2) * w(n-1) - (a2/2) * w(n-2)
  314. ;   y(n)/2 = w(n)/2 + (b1/2) * w(n-1) + (b2/2) * w(n-2)
  315.  
  316. ;   D High Memory Order - w(n-2)1,w(n-1)1,w(n-2)2,w(n-1)2,...
  317. ;   D Low Memory Order - (a2/2)1,(a1/2)1,(b2/2)1,(b1/2)1,(a2/2)2,...
  318.  
  319. ;   this version uses two pointers
  320.  
  321.         ori     #$08,mr
  322.         move    #W,r0
  323.         move    #C,r3
  324.         move    #-1,n0
  325.         movep             x:<<input,a
  326.         rnd     a         x:(r3)+,x1                  ;x1=a2/2
  327.         move              x:(r0)+,y0                  ;y0=wn-2
  328.         do      #N,END_DOE
  329.         mac     y0,x1,a   x:(r0)+n0,y1  x:(r3)+,x1    ;y1=wn-1
  330.         macr    y1,x1,a   x1,x:(r0)+
  331.         move              x:(r3)+,x1                  ;x1= b2/2
  332.         mac     y0,x1,a   a,x:(r0)+
  333.         move              x:(r3)+,x1                  ;x1=b1/2
  334.         mac     y1,x1,a   x:(r0)+,y0    x:(r3)+,x1
  335. END_DOE
  336.  
  337. ;   this version uses three pointers
  338.  
  339.         ori     #$08,mr
  340.         move    #W,r0
  341.         move    #C,r3
  342.         move    #C+2,r1
  343.         move    #2,n3
  344.         move    #4,n1
  345.         move    #-1,n0
  346.         movep             x:<<input,a                   ;a=x 
  347.         move              x:(r0)+,y0    x:(r3)+,x0      ;y0=w-2
  348.         do      #N,END_DOF
  349.         mac     y0,x0,a   x:(r0)+n0,y1  x:(r3)+n3,x0    ;w-1;a1/2
  350.         macr    y1,x0,a   y1,x:(r0)+                    ;a=w
  351.         move              x:(r1)+n1,x0  x:(r3)+,x1      ;x0=b2/2
  352.         mac     y0,x0,a   a,x:(r0)+                     ;a=w+b2/2w-2
  353.         mac     y1,x1,a   x:(r0)+,y0    x:(r3)+,x0      ;a=y; next w-2
  354. END_DOF
  355.  
  356. ========================================================================
  357. B.2.15  N Radix 2 FFT Butterflies
  358. ;   Decimation in time (DIT), in-place algorithm
  359.  
  360. ;   Twiddle Factor Wk= wr - jwi = cos(2pk/N) -j sin(2pk/N) pointed by r1
  361. ;      which must be saved on each pass. 
  362.  
  363. ;   xr = ar + wr * br - wi * bi
  364. ;   xi = ai + wi * br + wr * bi
  365. ;   yr = ar - wr * br + wi * bi = 2 * ar - xr
  366. ;   yi  = ai - wi * br -  wr * bi = 2 * ai - xi
  367.  
  368.         move              x:(r1)+,y0  x:(r3)+,x1   ;y0=wr; x1=br
  369.         move              x:(r0),b                 ;b=ar
  370.         move              x:(r1)+n1,y1             ;y1=wi
  371.  
  372. ;   save r1, update r1 to point last bi/yi
  373.  
  374.         do      #n,end_bfly
  375.         mac     y0,x1,b   x:(r3)+,x0               ;b=ar+wrbr
  376.         macr    y1,x0,b   a,x:(r1)+                ;b=xr
  377.         move              x:(r0)+,a                ;a=ar
  378.         subl    b,a       b,x:(r2)+                ;a=2ar-xr=yr
  379.         move              x:(r0),b
  380.         move              a,x:(r1)+                ;b=ai
  381.         mac     y1,x1,b   x:(r3)+,x1               ;b=ai+wibr
  382.         macr    y0,x0,b   x:(r0)+,a  x:(r3)+,x0    ;b=xi;a=ai
  383.         subl    b,a       b,x:(r2)+                ;a=2ai-xi=yi
  384.         move              x:(r0),b                 ;b=ar
  385. end_bfly
  386.         move              b,x:(r1)+n1              ;save last yi
  387.  
  388. ;   save r1, update r1 to point last bi/yi
  389.  
  390. ========================================================================
  391. B.2.16  LMS ADAPTIVE FILTER
  392.  
  393. ;Notation and symbols:
  394. ;   x(n)    - Input sample at time n.
  395. ;   d(n)    - Desired signal at time n.
  396. ;   y(n)    - FIR filter output at time n.
  397. ;   H(n)    - Filter coefficient vector at time n.
  398. ;                  H={c0,c1,c2,...,ck,...,c(N-1)}
  399. ;   X(n)    - Filter state variable vector at time N.
  400. ;                  X={x(n),x(n-1),....,x(n-N+1)}
  401. ;   Mu      - Adaptation gain.
  402. ;   N       - Number of coefficient taps in the filter. 
  403. ;   True LMS Algorithm       Delayed LMS Algorithm
  404. ;   Get input sample         Get input sample
  405. ;   Save input sample        Save input sample
  406. ;   Do FIR                   Do FIR
  407. ;   Get d(n), find e(n)      Update coefficients
  408. ;   Update coefficients      Get d(n), find e(n)
  409. ;   Output y(n)              Output y(n)
  410. ;   Shift vector X           Shift vector X
  411.  
  412. ;   System equations:
  413. ;   e(n)=d(n)-H(n)X(n)       e(n)=d(n)-H(n)X(n)         (FIR filter and error)
  414. ;   H(n+1)=H(n)+uX(n)e(n)    H(n+1)=H(n)+uX(n-1)e(n-1)  (Coefficient update)
  415.  
  416. ;References:
  417.  
  418. ;"Adaptive Digital Filters and Signal Analysis", Maurice G. Bellanger,
  419. ; Marcel Deker Inc. New York and Basel
  420.  
  421. ;"The DLMS Algorithm Suitable for the Pipelined Realization of Adaptive
  422. ; Filters", Proc. IEEE ASSP Workshop, Academia Sinica, Beijing, 1986
  423.  
  424. ;Note:
  425. ;The sections of code shown describe how to initialize all registers,
  426. ;filter an input sample and do the coefficient update. Only the instruc-
  427. ;tions relating to the filtering and coefficient update are shown as
  428. ;part of the benchmark. Instructions executed only once (for initial-
  429. ;ization) or instructions that may be user application dependent are not
  430. ;included in the benchmark.
  431.  
  432. ========================================================================
  433. ;Implementation of the true LMS on the DSP56100
  434.  
  435.         move    #XM,r0                         ;start of X
  436.         move    #N-1,m0                        ;mod 4
  437.         move    #-2,n0                         ;adjustment for filtering
  438.         move    m0,m2                          ;mod N
  439.         movep             x:<<input,y0              ;get input sample
  440.         move    #H,r3                               ;coefficients
  441.         clr     a         y0,x:(r0)+                ;save x(n)
  442.         move              x:(r3)+,x1                ;get c0
  443.         rep     #N-1                                ;do fir
  444.         mac     y0,x1,a   x:(r0)+,y0  x:(r3)+,x1
  445.         macr    y0,x1,a                             ;last tap
  446.         movep             a,x:<<output         ;output fir if desired
  447.  
  448. ;Get d(n), subtract fir output, multiply by "u", put the result in x0.
  449. ;This section is application dependent.
  450.  
  451.         move    #H,r3                               ;coefficients
  452.         move    r3,r2                               ;coefficients
  453.         move              x:(r0)+,y0                ;get x(n)
  454.         move              x:(r3)+,a                 ;a=c0
  455.         do      #ntaps,_coefupdate                  ;update coef.
  456.         macr    x0,y0,a   x:(r0)+,y0  x:(r3)+,x1
  457.         tfr     x1,a      a,x:(r2)+                 ;copy c, 
  458. _coefupdate
  459.         move              x:(r0)+n0,y0              ;update r0
  460.         move              x:(r3)-,y0                ;update r3 
  461.  
  462. ========================================================================
  463. ;   Implementation of the delayed LMS on the DSP56100
  464.  
  465. ;   Delayed LMS algorithm with matched coefficient and data vectors
  466. ;   Algorithm runs in 2N (2 coeffs processed in each 4 cycle loop)
  467.  
  468. ;   Register Usage:
  469. ;   Data Sample is stored in Y0 and Y1.
  470. ;   Coefficient is stored in X1
  471. ;   Loop Gain * Error is stored in X0.
  472. ;   FIR operation done in B.
  473. ;   Coeff update operation done in A.
  474.  
  475. ;      FIR sum = a = a +c(k)old*x(n-k)
  476. ;      c(k)new=  b = c(k)old -mu*eold *x(n-k-1)
  477.  
  478.         move    #state,r0
  479.         move    #ntaps,m0
  480.         move    #-2,n0
  481.         move    #1,n1
  482.         move    #c+1,r3
  483.         move    #c,r1
  484.  
  485.         clr     b         x:(r0)+,y0                ;y0 = x(n)
  486.         move              x:(r0)+,y1   x:(r3)+,x1   ;y1=x(n-1)
  487.  
  488.         do      #ntaps/2,end_lms
  489.         mac     y0,x1,b   a,x:(r1)+n1  x1,a
  490.         macr    x0,y1,a   x:(r0)+,y0   x:(r3)+,x1
  491.         mac     x1,y1,b   a,x:(r1)+n1  x1,a
  492.         macr    y0,x0,a   x:(r0)+,y1   x:(r3)+,x1
  493. end_lms
  494.         move              a,x:(r1)+
  495.         move              (r0)+n0
  496.  
  497. ========================================================================
  498. ;   Implementation of the double precision true LMS on the DSP56100
  499. ;   Memory map:
  500.  
  501.         move    #XM,r0                      ;start of X
  502.         move     #N-1,m0                     ;mod 4
  503.         move    #-2,n0                      ;adjustment for filtering
  504.         move    #2,n3
  505.         move    m0,m2                       ;mod N
  506.         movep             x:<<input,y0      ;get input sample
  507.  
  508.         move    #H,r3                                 ;coefficients
  509.         clr     a         y0,x:(r0)+                  ;save x(n)
  510.         move              x:(r3)+n3,x1                ;get c0 
  511.         rep     #N-1                                  ;do fir 
  512.         mac     x1,y0,a   x:(r0)+,y0  x:(r3)+n3,x1    ;mac; next x
  513.         macr    x1,y0,a                               ;last tap
  514.         movep             a,x:<<output                ;output fir if desired
  515.  
  516. ;Get d(n), subtract fir output, multiply by "u", put the result in x0.
  517. ;This section is application dependent.
  518.  
  519.         move    #H,r3                                 ;coefficients
  520.         move    r3,r2                                 ;coefficients
  521.         move              x:(r0)+,y0                  ;get x(n)
  522.         move              x:(r3)+,a                   ;a1=c0h
  523.         move              x:(r3)+,a0                  ;a0=col
  524.         do      #ntaps,_coefupdat                     ;update coef.
  525.         mac     x0,y0,a   x:(r0)+,y0
  526.         move              x:(r3)+,b                   ;u e(n) x(n)+c
  527.         move              x:(r3)+,b0                  ;b0=next c()l
  528.         move              a1,x:(r2)+                  ;save next c()h
  529.         tfr     b,a       a0,x:(r2)+                  ;copy c
  530. _coefupdat
  531.         move              x:(r0)+n0,y0                ;update r0
  532.         move    (r3)-                                 ;update r3
  533.         move    (r3)-
  534.  
  535. ========================================================================
  536. ;Implementation of the double precision delayed LMS on the DSP56100
  537.  
  538. ;   Delayed LMS algorithm with matched coefficient and data vectors
  539. ;   Algorithm runs in 4N (2 coeffs processed in each 8 cycle loop)
  540. ;   Register Usage:
  541. ;   Data Sample is stored in Y0 and Y1.
  542. ;   Coefficient is stored in X1
  543. ;   Loop Gain * Error is stored in X0.
  544. ;   FIR operation done in B.
  545. ;   Coeff update operation done in A.
  546. ;      FIR sum = a = a +c(k)old*x(n-k)
  547. ;      c(k)new = b = c(k)old -mu*eold *x(n-k-1)
  548.  
  549.         move    #state,r0
  550.         move    #ntaps,m0
  551.         move    #-2,n0
  552.         move    #1,n1
  553.         move    #c,r3
  554.         move    #c-2,r1
  555.  
  556.         clr     b         x:(r0)+,y0                ;y0 = x(n)
  557.         move              x:(r0)+,y1  x:(r3)+,x1    ;y1= x(n-1) x1=c0h
  558.  
  559.         do      #ntaps/2,end_lms2
  560.         mac     y0,x1,b   a,x:(r1)+n1
  561.         tfr     x1,a      a0,x:(r1)+n1              ;a1=ckh
  562.         move              x:(r3)+,a0                ;a0=ckl
  563.         macr    x0,y1,a   x:(r0)+,y0  x:(r3)+,x1    ;x1=c(k+1)h
  564.         mac     x1,y1,b   a,x:(r1)+n1
  565.         tfr     x1,a      a0,x:(r1)+n1
  566.         move              x:(r3)+,a0
  567.         macr    y0,x0,a   x:(r0)+,y1  x:(r3)+,x1
  568. end_lms2
  569.         move              a,x:(r1)+
  570.         move              a0,x:(r1)+
  571.         move              (r0)+n0
  572.  
  573. ========================================================================
  574. ;-----------------------------------------------------------------------
  575. ; The complete code for a true LMS that executes in two instruction
  576. ; cycles per tap is shown below. A brief description of how the algorithm
  577. ; is derived precedes the LMS code. Note that the coefficients stored to
  578. ; memory are saturated (should overflow occur), whereas the coefficients
  579. ; used in the FIR filter are not saturated. Therefore, the coefficients
  580. ; stored to memory, and the coefficients used in the FIR filter calcula-
  581. ; tion, are not guaranteed to be the same. This should not be a problem
  582. ; in designs where the echo gain is guaranteed to be less than one.
  583. ;-----------------------------------------------------------------------
  584.  
  585.         opt     cc,cex
  586.         page    132,66
  587.         section FAST_LMS
  588. n_tap   equ     16
  589.         org     x:$0100
  590. ref_buf      dsm   n_tap    ;Ref_buf is a modulo n_tap buffer, containing
  591.                             ;a reference signal.
  592. coeff        ds    n_tap    ;Note: Coefficients are stored in reverse order
  593. ref_ptr      dc    ref_buf  ;data pointer for reference buffer
  594. scaled_error dc    0        ;scaled error sample from last call of echo_input
  595. norm_factor  dc    0.1      ;scale factor for error signal
  596.  
  597.         org     p:0
  598.         jmp     Test_EC
  599.         org     p:$0100
  600.  
  601. ;-----------------------------------------------------------------------
  602. ; The following pseudo code is for the "standard" LMS echo canceller
  603. ; algorithm.
  604. ;       y(n) = estimate of echo at time sample n.
  605. ;       x(n) = reference input signal at time n.
  606. ;       input (n) = input signal (containing echo signal) at time n.
  607. ;       c(n,k) = k'th coefficient at time n.
  608. ;
  609. ;       /* initialize N coefficients at time 0 to 0 */
  610. ;
  611. ;       for (k = 0 to n-1) {
  612. ;           c(0,k) = 0;
  613. ;       }
  614. ;
  615. ;       /* LMS follows, do forever */
  616. ;
  617. ;   n = 0;
  618. ;   do forever {
  619. ;           y(n) = 0;
  620. ;
  621. ;           for (k=0 to N-1) {
  622. ;               y(n) = y(n) + c(n,k)*x(n-k);        /* FIR filter */
  623. ;           }
  624. ;
  625. ;       error(n) = input(n) - y(n);
  626. ;
  627. ;       for (k = 0 to N-1) {
  628. ;           c(n+1,k) = c(n,k) + delta*error(n)*x(n-k) ; /* Coefficient Update */
  629. ;
  630. ;       }
  631. ;
  632. ;       n = n+1;
  633. ;   }
  634. ;-----------------------------------------------------------------------
  635. ;-----------------------------------------------------------------------
  636. ;   The following is equivalent to the above (i.e., given the same input
  637. ;   signals, the error signal and coefficients will follow the exact
  638. ;   same trajectories. Note the calculations are run from the back of
  639. ;   the filter to the front. This saves two registers. Also note that
  640. ;   the calculation order of the coefficient and the FIR filter has been
  641. ;   reversed.
  642. ;
  643. ;   /* initialize N coefficients at time -1 to 0 */
  644. ;
  645. ;   for (k = 0 to N-1) {
  646. ;       c(-1,k) = 0;
  647. ;   }
  648. ;
  649. ;   error (-1) = 0          ;The initial error must be set to zero.
  650. ;
  651. ;   /* LMS follows, do forever */
  652. ;
  653. ;   n = 0;
  654. ;   do forever {
  655. ;
  656. ;       y(n) = 0;
  657. ;
  658. ;       for (k = N-1 to 0) {
  659. ;           c(n,k) = c(n-1,k) + delta*error(n-1)*x(n-1-k); /* Coefficient */
  660. ;       }
  661. ;
  662. ;       for (k= N-1to 0) {
  663. ;           y(n) = y(n) + c(n,k)*x(n-k);    /* FIR filter */
  664. ;       }
  665. ;
  666. ;       error(n) = input(n) - y(n);
  667. ;
  668. ;       n = n+1;
  669. ;
  670. ;   }
  671. ;-----------------------------------------------------------------------
  672. ; Note that the two "for" loops in the do forever loop can now be combined.
  673. ;-----------------------------------------------------------------------
  674. ;
  675. ;   /* initialize N coefficients at time -1 to 0 */
  676. ;
  677. ;   for (k = 0 to N-1) {
  678. ;       c(-1,k) = 0;
  679. ;   }
  680. ;
  681. ;   error(-1) = 0;
  682. ;
  683. ;   /* LMS follows, do forever */
  684. ;
  685. ;   n = 0;
  686. ;   do forever {
  687. ;
  688. ;       y(n) = 0;
  689. ;
  690. ;       for (k = N-1 to 0) {
  691. ;           c(n,k) = c(n-1,k) + delta*error(n-1)*x(n-1-k); /* Coefficient */
  692. ;           y(n) = y(n) + c(n,k)*x(n-k);    /* FIR filter */
  693. ;       }
  694. ;
  695. ;       error(n) = input(n) - y(n);
  696. ;
  697. ;       n = n+1;
  698. ;
  699. ;   }
  700. ;-----------------------------------------------------------------------
  701. ;-----------------------------------------------------------------------
  702. ;
  703. ;   Echo Canceller Routine (Fast LMS)
  704. ;
  705. ;   Upon Entry
  706. ;       x1 should contain newest reference sample
  707. ;       y1 should contain newest input sample
  708. ;
  709. ;   Upon Exit
  710. ;       b will contain echo cancelled output
  711. ;
  712. ;   Note that the coefficients are stored in reverse time order.
  713. ;-----------------------------------------------------------------------
  714.  
  715. FAST_LMS:
  716.  
  717.         move    #+1,n1
  718.  
  719.         move    #n_tap-1,m0
  720.         move    #-1,m1
  721.         move    m1,m3
  722.  
  723.         move    x:ref_ptr,r0    ;r0 is the get reference signal pointer
  724.  
  725.         move    #coeff,r3       ;r3 is the get coefficient pointer
  726.         move    r3,r1           ;r1 is the put coefficient pointer
  727.  
  728.         move    x:(r0),y0       ;y0 contains the oldest reference sample
  729.         move    x1,x:(r0)+      ;store newest reference sample in reference register
  730.  
  731.         clr     b    x:(r3)+,a  ;fetch first coefficient, and clear b for FIR
  732.         move    x:scaled_error,x0    ;x0 is the scaled error sample
  733.  
  734.         do      #n_tap,end_fir_update
  735.  
  736.         macr    x0,y0,a   x:(r0)+,y0   x:(r3)+,x1
  737.         mac     a1,y0,b   a,x:(r1)+n1  x1,a
  738. end_fir_update
  739.  
  740.         neg     b
  741.         move    r0,x:ref_ptr        ;store get reference pointer
  742.         add     y1,b                ;b = EC output = input - echo_estimate
  743.  
  744.         move    x:norm_factor,x0
  745.         move    b,y0
  746.         mpyr    y0,x0,a
  747.         move    a,x:scaled_error
  748.  
  749.         move    b,x:output_port
  750.  
  751.         rts
  752. ;-----------------------------------------------------------------------
  753. ;;   Test shell follows
  754. ;       Remote signal is an impulse train, period greater than echo span
  755. ;       Input is the resulting echo signal
  756. ;-----------------------------------------------------------------------
  757.  
  758.         org    x:$1000
  759.  
  760. output_port    ds   1        ;write output to D/A
  761.  
  762.         org    x:$0400
  763.  
  764. Remote_signal  dc   0.8
  765.                dc   0.0
  766.                dc   0.0
  767.                dc   0.0
  768.                dc   0.0
  769.                dc   0.0
  770.                dc   0.0
  771.                dc   0.0
  772.                dc   0.0
  773.                dc   0.0
  774.                dc   0.0
  775.                dc   0.0
  776.                dc   0.0
  777.                dc   0.0
  778.                dc   0.0
  779.                dc   0.0
  780.                dc   0.0
  781.                dc   0.0
  782.                dc   0.0
  783.                dc   0.0
  784.  
  785.         org    x:$0420
  786.  
  787. echo_input     dc   0.0
  788.                dc   0.0
  789.                dc   0.2
  790.                dc   0.4
  791.                dc   0.7
  792.                dc   0.4
  793.                dc   0.2
  794.                dc   0.1
  795.                dc   0.0
  796.                dc   -0.1
  797.                dc   -0.2
  798.                dc   -0.1
  799.                dc   0.0
  800.                dc   0.1
  801.                dc   0.0
  802.                dc   0.0
  803.                dc   0.0
  804.                dc   0.0
  805.                dc   0.0
  806.                dc   0.0
  807.  
  808. remote_get     dc   remote_signal
  809. input_get      dc   echo_input
  810.  
  811.         org    p:
  812.  
  813. Test_EC
  814.         move    #0 ,x0
  815.         move    #-1,m0
  816.         move    #coeff,r0
  817.         rep     #n_tap
  818.         move    x0,x:(r0)+        ;zero coefficients
  819.  
  820.         move    #$ffff,x0
  821.         do      x0,end_test_loop
  822.  
  823.         move    x:remote_get,r0
  824.         move    #19,m0
  825.         move    x:input_get,r1
  826.         move    #19,m1
  827.         move    x:(r0)+,x1
  828.         move    x:(r1)+,y1
  829.         move    r0,x:remote_get
  830.         move    r1,x:input_get
  831.  
  832.         jsr     FAST_LMS
  833.  
  834. end_test_loop
  835.         nop
  836.         nop
  837.  
  838.         debug
  839.  
  840.         endsec
  841.         end
  842.  
  843. ========================================================================
  844. B.2.17  FIR Lattice Filter
  845. ; Lattice filter benchmarks. N refers to the number of "k" coefficients
  846. ; in the lattice filter. Some filters may have other coefficients other
  847. ; than the "k" coefficients but their number may be determined from k.
  848.  
  849.         move    #state,r0    ;point to state variable storage
  850.         move    #N,m0        ;N=number of k coefficients
  851.         move    #k,r1        ;point to k coefficients
  852.         move    #N-1,m1      ;mod for k's
  853.         move    #0,n0
  854.  
  855.         movep             x:<<input,b        ;get input
  856.  
  857.         move              b,x:(r0)+          ;save 1st state
  858.         move              x:(r1)+,x0         ;get k 
  859.         do      #N,end_elat
  860.         move              x:(r0)+n0,a   b,y0 ;get s;copy t 
  861.         macr    x0,y0,a   x:(r0)+n0,x1       ;t*k+s, copy s
  862.         macr    x1,x0,b   x:(r1)+,x0         ;s*k+t, nxt k
  863.         move              a,x:(r0)+          ;sv st
  864. end_elat
  865.         move              x:(r0)-,y1
  866.         move              x:(r1)-,x0
  867.         movep             b,x:<<output       ;output 
  868.  
  869. ========================================================================
  870. B.2.18  All Pole IIR Lattice Filter
  871.  
  872.         move    #k+N-1,r0                      ;point to k
  873.         move    #N-1,m0                        ;number of k's-1
  874.         move    #-1,n1
  875.         move    n1,n3
  876.         movep             x:<<input,a          ;get input sample
  877.         move    #state,r3                      ;pt to x() 
  878.         move              x:(r0)-,y1           ;y1=k3
  879.         move              x:(r3)+,x1           ;x1=s3
  880.         macr   -x1,y1,a   x:(r0)+n0,y1         ;a=in-k3s3;y1=k2
  881.         move              x:(r3)-,x1           ;x1=s2
  882.         do      #n-1,endlat
  883.         macr   -x1,y1,a   b,x:(r3)+            ;a=a-s2k2=t2;update s3
  884.         move              x:(r3)+,b     a,x1            ;b=s2
  885.         macr    x1,y1,b   x:(r0)+n0,y1  x:(r3)+n3,x1    ;b=s2+t2k2;get s1,k1
  886. endlat
  887.         move              b,x:(r3)+            ;sv 2nd last s
  888.         move              x:(r0)+,y1           ;update r0 
  889.         move              a,x:(r3)+            ;save last s
  890.         movep             a,x:<<output         ;output 
  891.  
  892. ========================================================================
  893. B.2.19  General Lattice Filter
  894.  
  895.         move    #k,r0                          ;point to coefficients
  896.         move    #2*N,m0                        ;mod 2*(# of k's)+1
  897.         move    #-1,n3
  898.         movep             x:<<input,a                 ;get input sample
  899.         move    #state,r3                             ;pto filter states
  900.         move              x:(r0)+,y1                  ;get first k
  901.         move              x:(r3)-,x1                  ;first s 
  902.         do      #N,el                                 ;do filter 
  903.         macr   -y1,x1,a   b,x:(r3)+                   ;t-k*s, save s 
  904.         move              x:(r3)+,b   a,x1            ;get s again 
  905.         macr    x1,y1,b   x:(r0)+,y1  x:(r3)+n3,x1    ;t'*k+s,get k& s
  906. el
  907.         move              b,x:(r3)+                   ;s 2nd to1st st 
  908.         clr     a         a,x:(r3)+                   ;s first state 
  909.         move              x:(r3)+,x1                  ;get last state
  910.         rep     #N                                    ;do fir taps 
  911.         mac     y1,x1,a   x:(r0)+,y1  x:(r3)+,x1
  912.         macr    y1,x1,a   x:(r3)+,x1                  ;finish, adj pter 
  913.         movep              a,x:<<output
  914.  
  915. ========================================================================
  916. B.2.20  Normalized Lattice Filter
  917.  
  918.         move    #c,r0                             ;point to coefficients
  919.         move    #3*N,m0                           ;mod on coefficients
  920.         move    #0,n3
  921.         movep             x:<<input,a                 ;get input sample
  922.         move    #state,r3                             ;pt to state 
  923.         move              x:(r0)+,y1  a,x1            ;get first Q 
  924.         do      #n,endnlat
  925.         mpy     x1,y1,a   x:(r0)+,y0  x:(r3)+n3,x0    ;q*t; get k & s
  926.         macr   -x0,y0,a   b,x:(r3)+                   ;q*t-k*s,save s
  927.         mpy     y0,x1,b   a,x1                        ;k*t, set t' 
  928.         macr    y1,x0,b   x:(r0)+,y1                  ;k*t+q*s, get q
  929. endnlat
  930.         move              b,x:(r3)+                   ;sv scnd lst st
  931.         move              a,x:(r3)+                   ;save state
  932.         clr     a         x:(r3)+,x1                  ;clr acc
  933.         rep     #n                                    ;do fir taps
  934.         mac     x1,y1,a   x:(r0)+,y1  x:(r3)+,x1
  935.         macr    x1,y1,a   x:(r3)+,x1                  ;rnd, adj pter
  936.         movep             a,x:<<output                ;output sample
  937.  
  938. ========================================================================
  939. B.2.21  [1x3][3x3] Matrix Multiply
  940.  
  941.         move    #AD,r3                              ;point to mat a
  942.         move    #bd,r0                              ;point to vec b
  943.         move    #2,m0                               ;addrb mod 3
  944.         move    #c,r2                               ;point to vec c
  945.         move              x:(r0)+,y0  x:(r3)+,x0    ;y0=a11;x0=b1
  946.         mpy     y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;a11*b1
  947.         mac     y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;+a12*b2
  948.         macr    y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;+a13*b3
  949.         move              a,x:(r2)+                 ;store c1
  950.         mpy     y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;a21*b1
  951.         mac     y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;+a22*b2
  952.         macr    y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;+a23*b3
  953.         move              a,x:(r2)+                 ;store c2
  954.         mpy     y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;a31*b1
  955.         mac     y0,x0,a   x:(r0)+,y0  x:(r3)+,x0    ;+a32*b2
  956.         macr    y0,x0,a                             ;+a33*b3->c3
  957.         move              a,x:(r2)+                 ;store c3
  958.  
  959. ========================================================================
  960. B.2.22  [NxN][NxN] Matrix Multiply
  961. ;The matrix multiplications are for square NxN matrices.
  962. ;All the elements;are stored in "row major" format. i.e. for the array A:
  963.  
  964.         move    #AD,r0            ;point to A 
  965.         move    #bd,r3            ;point to B
  966.         move    #c,r2             ;output mat C
  967.         move    #N,b              ;array size
  968.         move    b,n3
  969.  
  970.         do      #N,erows                              ;do rows 
  971.         do      #N,ecols                              ;do columns
  972.         move              x1,r0                       ;copy row A
  973.         move    r1,r3                                 ;copy col B
  974.         clr     a         x:(r0)+,y0
  975.         move              x:(r3)+n3,x0                ;clr sum & pipe
  976.         rep     #N-1                                  ;sum 
  977.         mac     y0,x0,a   x:(r0)+,y0  x:(r3)+n3,x0
  978.         macr    y0,x0,a   x:(r3)+,y1                  ;finish, next col
  979.         move              a,x:(r2)+                   ;save output 
  980. ecols
  981.         add     x1,b                                  ;next row A 
  982.         move              b,x1
  983.         move    #bd,r1                                ;first element B 
  984. erows
  985.  
  986. ========================================================================
  987. B.2.23  N Point 3x3 2-D FIR Convolution
  988. ;The two dimensional FIR uses a 3x3 coefficient mask:
  989. ;
  990. ;The image is an array of 512x512 pixels. To provide boundary conditions
  991. ;for the FIR filtering, the;image is surrounded by a set of zeros such
  992. ;that the image is actually stored as a 514x514 array. i.e
  993. ;
  994. ;The image (with boundary) is stored in row major storage. The first
  995. ;element of the array image is image(1,1) followed by image(1,2). The
  996. ;last element of the first row is image(1,514);followed by the beginning
  997. ;of the next column image(2,1). These are stored sequentially in the
  998. ;array; "im" in d memory.
  999. ;
  1000. ;Image(1,1) maps to index 0, image(1,514) maps to index 513,
  1001. ;Image(2,1) maps to index 514 (row major storage).
  1002. ;
  1003. ;Although many other implementations are possible, this is a realistic
  1004. ;type of image environment;where the actual size of the image may not
  1005. ;be an exact power of 2. Other possibilities include storing a 512x512
  1006. ;image but computing only a 511x511 result, computing a 512x512 result
  1007. ;without boundary conditions but throwing away the pixels on the border,
  1008. ;etc.
  1009.  
  1010. ;   r0 -> image(n,m)         image(n,m+1)         image(n,m+2)
  1011. ;   r1 -> image(n+514,m)     image(n+514,m+1)     image(n+514,m+2)
  1012. ;   r2 -> image(n+2*514,m)   image(n+2*514,m+2)   image(n+2*514,m+3)
  1013. ;   r3 -> FIR coefficients
  1014. ;   b  -> output image
  1015.  
  1016.         move    #mask,r3                ;pt to coef.
  1017.         move    #-8,n3
  1018.         move    #image,r0               ;top boundary
  1019.         move    #image+514,r1           ;left of first pixel
  1020.         move    #image+2*514,r2         ;left of 2nd row
  1021.  
  1022.         move    #512,y1
  1023.         move    #-1,n1                  ;adjust.
  1024.         move    n1,n2
  1025.  
  1026.         move    #output,b               ;output image
  1027.         move              x:(r0)+,y0    ;y0=im(1,1)   
  1028.         move              x:(r3)+,x0    ;x0=c11
  1029.  
  1030.         do      y1,rows
  1031.         do      y1,cols
  1032.         mpy     y0,x0,a   x:(r0)+,y0    x:(r3)+,x0    ;im(1,1)*c11
  1033.         mac     y0,x0,a   x:(r0)+n0,y0  x:(r3)+,x0    ;+im(1,2)*c12
  1034.         mac     y0,x0,a   x:(r1)+,y0    x:(r3)+,x0    ;+im(1,3)*c13
  1035.         mac     y0,x0,a   x:(r1)+,y0    x:(r3)+,x0    ;+im(2,1)*c21
  1036.         mac     y0,x0,a   x:(r1)+n1,y0  x:(r3)+,x0    ;+im(2,2)*c22
  1037.         mac     y0,x0,a   x:(r2)+,y0    x:(r3)+,x0    ;+im(2,3)*c23
  1038.         mac     y0,x0,a   x:(r2)+,y0    x:(r3)+,x0    ;+im(3,1)*c31
  1039.         mac     y0,x0,a   x:(r2)+n2,y0  x:(r3)+n3,x0  ;+im(3,2)*c32
  1040.         macr    y0,x0,a   x:(r0)+,y0    x:(r3)+,x0    ;+im(3,3)*c33
  1041.         move              a,x:(b1)
  1042.         inc24   b
  1043. cols
  1044. ; adjust pointers for frame boundary
  1045.         move    #2,n1
  1046.         move    n1,n2
  1047.         inc     b         x:(r0)+,x1                  ;adj r0
  1048.         inc     b         x:(r1)+n1,x1                ;adj r1
  1049.         move    (r2)+n2                               ;adj r2
  1050.         move              x:(r0)+,x1                  ;preload
  1051.         move    #-1,n1                                ;adjust.
  1052.         move    n1,n2
  1053.  
  1054. rows
  1055.  
  1056. ========================================================================
  1057. B.2.24  Signed 16 Bit Result Divide
  1058. ;This is a routine for a 4 quadrant divide (i.e., a signed divisor and
  1059. ;a signed dividend) which generated a 16-bit signed quotient and a 32-bit
  1060. ;signed remainder. The quotient is stored in the lower 16 bits of accum-
  1061. ;ulator a, a0, and the remainder in the upper 16 bits a1. The true
  1062. ;(restored) remainder is stored in b1. The original dividend must occupy
  1063. ;the low order 32 bits of the destination accumulator, a, and must be a
  1064. ;POSITIVE number. The divisor must be larger than the dividend so that a 
  1065. ;fractional quotient is generated.
  1066.  
  1067.         abs     a    a,b        ;make dividend positive
  1068.         move    b,x:$0          ;save rem. sign in x:$0
  1069.         eor     x0,b            ;quo. sign in N bit of CCR
  1070.         andi    #$fe,ccr        ;clear carry bit C (quotient sign bit)
  1071.         rep     #$10            ;form a 16-bit quotient
  1072.         div     x0,a            ;form quot. in a0, remainder in a1
  1073.         tfr     a,b                ;save remainder and quot. in b1,b0
  1074.         jpl     savequo         ;go to savequo if quot. is positive
  1075.         neg     b               ;complement quotient if N bit is set
  1076. savequo 
  1077.         tfr     x0,b            ;get signed divisor
  1078.         move    b0,x1           ;save quo. in x1
  1079.         abs     b               ;get abs value of signed divisor
  1080.         add     a,b             ;restore remainder in b1
  1081.         bftstl  #$8000,x:$0     ;test if remainder is positive
  1082.         beq     <done1          ;branch if positive
  1083.         move    #$0,b0          ;prevent unwanted carry
  1084.         neg     b               ;complement remainder
  1085. done1
  1086.  
  1087. ========================================================================
  1088. B.2.25  Signed Integer Divide
  1089. ;Registers usex: a,b,x0
  1090. ;Output: Quotient -> a0
  1091.  
  1092.         move    #dividend,a        ;sign ext A2 
  1093.         move    a2,a1              ;and A1
  1094.         move    #dividend,a0       ;move into A
  1095.         asl     a                  ;prep divide
  1096.         move    #divisor,x0        ;divisor into x0
  1097.         abs     a    a,b           ;dividend pos
  1098.         andi    #$fe,ccr           ;clr the carry
  1099.         rep     #$10               ;16bit quotient
  1100.         div     x0,a               ;form quot. a0
  1101.         eor     x0,b               ;save sign in N
  1102.         bpl     <done2
  1103.         neg     a                  ;comp.bit is set
  1104. done2
  1105.  
  1106. ========================================================================
  1107. B.2.26  Multiply 32-bit Fractions
  1108. ;This routine will execute the multiplication of two 32-bit FRACTIONAL
  1109. ;numbers that are already stored in memory as follows:
  1110.  
  1111. ;       r0 ->    X:$Paddr P0
  1112. ;                X:$Paddr P1
  1113. ;       r3 ->    X:$Qaddr Q0
  1114. ;                X:$Qaddr Q1
  1115.  
  1116. ;The initial 32-bit numbers are:
  1117. ;   P = P1:P0 (16:16 bits)
  1118. ;   Q = Q1:Q0 (16:16 bits)
  1119.  
  1120. ;The result, R, is a 64 bit number that is stored in the two
  1121. ;accumulators A and B as follows:
  1122. ;   R = R3:R2:R1:R0
  1123. ;     = A1:A0:B1:B0    (32:32bits)
  1124. ;     = A2:A1:A0:B1:B0 (sign extended)
  1125.  
  1126.         move    #paddr,r0            ;init pter for P
  1127.         move    #qaddr,r3            ;init pter for Q
  1128.         nop
  1129.         move              x:(r0)+,y0  x:(r3)+,x0    ;P0,Q0
  1130.         move              x:(r0)+,y1  x:(r3)+,x1    ;P1,Q1
  1131.         mpyuu   x0,y0,a
  1132.         move    a0,b0                ;b0=P0*Q0=R0
  1133.         dmacsu  x1,y0,a              ;a=P0*Q1+a1
  1134.         macsu   y1,x0,a              ;a=a+ P1*Q0
  1135.         move    a0,b1                ;b1=R1
  1136.         dmacss  x1,y1,a              ;a=P1*Q1+ a1=R3:R2
  1137.  
  1138.