list    p=18f452,t=ON,c=132,n=80
        radix   dec


include "p18f452.inc"

  cblock  0
        N_hi,N_lo
        mask
        x,y
        s_hi,s_lo

        min,max,t1,root

  endc


        ORG     0               ;Reset Vector


Main

   ;
   ; Initialize tmr0 to count instructions cycles
   ;  -- this is used to profile the code

        MOVLW   (1<<TMR0ON) | (1<<T08BIT) | (1<<PSA)
        MOVWF   T0CON,0

        clrf    max
        clrf    min
        setf    min
        CLRF    STATUS
        CLRF    x
        CLRF    s_hi
        CLRF    s_lo
        INCF    x,f



  bra regression2     ; this one compares the looped and unrolled versions

;------------------------------------------------------------------------
;
; Here's some test code that goes through all 2^16 cases
;
; psuedo code:
;
;  int  s=0;
;  int  x=0;
;
;  do {
;
;    if(x*x == s)
;      x++;
;
;    n = s;
;    w = sqrt(n);
;    if(w+1 != x)
;      failed();
;
;    s++;
;
;  } while (x<256);
;
; Also, this code will profile the algorithm.
;


xxx
        MOVF    x,W             ;W = x*x
        MULWF   x

        movf    PRODH,w,0       ;if x*x is equal to the current
        cpfseq  s_hi            ;test case, s, then we need to advance
         bra    L1              ;x. Note that x will always be 1 greater
        movf    PRODL,w,0       ;than the sqrt(s)
        cpfseq  s_lo
         bra    L1

        infsnz  x,f
here     bra    here

L1:
  ; N = s
        movf    s_hi,w
        movwf   N_hi
        movf    s_lo,w
        movwf   N_lo


  ; find the sqrt of N (N_hi:N_lo)

        movf    TMR0L,w,0       ;Record the starting time
        movwf   t1

        RCALL   sqrt_unrolled   ;Perform the sqrt

        movwf   root            ;Save the root so that we can check it later

        movf    TMR0L,w,0       ;Record the ending time
        subwf   t1,w            ;Calculate how long the code took by
        negf    WREG,0          ;taking the difference between the start
        addlw   -5              ;and end and subtracting the overhead

        cpfslt  min             ;Save the minimum time
         movwf  min

        cpfsgt  max             ;Save the maximum
         movwf  max

        infsnz  s_lo,f          ;s++ advance for the next test case
         incf   s_hi,f

        incf    root,w          ;if w (sqrt(N)) is one less than x
        cpfseq  x               ; then we've passed
         bra    failed


        bra     xxx

failed:
        bra     xxx             ;Set a break point here to catch failures


;---------------------------------------------
;
;Compare the rolled and unrolled versions:
; 
; This snippet calls the original sqrt routine and the newer
; unrolled routine for all 16 bit integers. The roots for each
; routine are compared.

regression2:
        CLRWDT
  ; N = s
        movf    s_hi,w
        movwf   N_hi
        movf    s_lo,w
        movwf   N_lo


        RCALL   sqrt_unrolled
        MOVWF   root


  ; N = s
        movf    s_hi,w
        movwf   N_hi
        movf    s_lo,w
        movwf   N_lo

        RCALL   sqrt

        XORWF   root,W            ; compare the results.

        BNZ     failed

        infsnz  s_lo,F            ;Increment the 16-bit test case
         incfsz s_hi,F            ;
          bra   regression2

        bra     here              ;wrapped around -- we're done.


;----------------------------------------------------------
;sqrt 
;
; The purpose of this routine is take the square root of a 16 bit
;unsigned integer.
;Inputs:  N_hi - High byte of the 16 bit number to be square rooted
;         N_lo - Low byte     "                  "             "
;Outputs: W register returned with the 8 bit result
;
;Memory used:
;  mask
;
;23 instructions
;Minimum 71 cycles
;Maximum 101 cycles

sqrt
        MOVLW   0xc0            ;Initialize value for mask
        MOVWF   mask
        MOVLW   0x40            ;Initial value for the root

sq1     CPFSLT   N_hi           ;If the root so far is less than the shifted N
         BRA     sq6            ;then we need to subtract the root so far from
                                ;N and set the next bit in the result

sq3     RLCF    N_lo,F          ;Shift N left one position
        RLCF    N_hi,F
        BC      sq4             ;Unconditionally subtract

        RRCF    mask,F          ;mov the 2-bit mask down 1
        XORWF   mask,W          ;clear the last and set the next bit

        BNC     sq1

   ;We are almost done. In the last iteration, we have 7 bits of the root. When
   ;"01" is appended to it, we will have a 9-bit number that must be subtracted
   ;from N. The 9-bits of N from which we must subtract are N_hi and the msb 
   ;of N_lo. The subwfb will allow us to do the subtract in one step

        RLCF    N_lo,F          ;Copy the MSB of N_lo into the borrow (carry)
        SUBWFB  N_hi,F          ;do a `9-bit' subtraction
        SKPNC                   ;If the result is positive
         XORLW  1               ;then toggle the lsb of the result
        RETURN

sq4     BTFSC   mask,0          ;Need to unconditionally set the current bit of the root.
          RETURN                ;However, if we're through iterating, then leave. 

        RRNCF   mask,F
        XORWF   mask,W          ;Append "01" to the root developed so far.
sq6     SUBWF   N_hi,F          ;Subtract the root developed so far 
        IORWF   mask,W          ;Set the current bit
        bra     sq3             ;Go unconditionally set the current bit.

;------------------------------------------------------------------------
;  Unrolled square root
;
; The purpose of this routine is take the square root of a 16 bit
;unsigned integer.
;Inputs:  N_hi - High byte of the 16 bit number to be square rooted
;         N_lo - Low byte     "                  "             "
;Outputs: W register returned with the 8 bit result
;
;Memory used:
;  none
;
;68 instructions
;Minimum 55 cycles
;Maximum 69 cycles
;
;  How it works:
;
; --- binary square roots using the non-restoring method.
;
; Ivan Flores. The Logic of Computer Arithmetic. Prentice-Hall 
; International Series in Electrical Engineering. Prentice-Hall, 
; Englewood Cliffs, New Jersey, 1963.
;
; Here's an example Flores gives that takes the square root of 01011111
;
;      1  0  0  1 .
;   -----------------
;   ) 01 01 11 11 . 00
;     -1
;     ---
;     00 01  <--- positive: first bit is a 1
;     -1 01  <--- root so far with 01 appended
;     -----
;     11 00  <--- negative: 2nd bit is a 0
;     +1 01  <--- restore the wrong guess
;    ------
;     00 01 11
;       -10 01  <--- root so far with 01 appended
;    ---------
;     11 11 10  <--- negative: 3rd bit is a zero
;       +10 01  <--- restore the wrong guess
;    ---------
;        01 11 11
;        -1 00 01  <--- root so far with 01 appended
;       ---------
;         0 11 10  <--- positive: 4th bit is a one
;
; The algorithm is essentially this:
;
; Let N be the number for which we wish to find the square root.
;
; Pair the bits of N together.If the most significant pair is
; greater than 0, then the most significant bit of the root is 1.
; Subtract 1 if this is the case. Append the next pair of bits
; of N to the (possibly decrement) first pair and call this N'.
; Append the binary digits '01' to the root formed so far and 
; subtract this from N'. If the result is greater than or equal
; to 0 then get rid of the '01' that was appended to the root and 
; just append a '1'. If the result of the subtraction was negative
; then get rid of the '01' and append a '0'.
;
; The process repeats: append the next two bits from N to
; N'. Append '01' to the root so far and subtract from N'. 
; If the result is negative, the next bit is 0. If the result
; is positive the next bit is 1 and N' = N' - (root:01).
;
;

sqrt_unrolled


        MOVLW   0x40            ;Initial guess for the root

        CPFSLT  N_hi            ;Compare the guess to N
         bra    sq7_1           ;the upper two bits of N are non zero
        bra     sq3_1           ;the upper two bits of N are zero

sq7_1   subwf   N_hi,F          ;Subtract the root so far
        iorlw   0xc0            ;set the MSB of the root
                                ;An xor below clears the next msb

sq3_1   rlcf    N_lo,f          ;Grab the next two bits in N by
        rlcf    N_hi,f          ;shifting it left and at the same
        xorlw   0x60            ;time 'shifting' the root right
                                ;This is kind of tricky. (actually
                                ;all of the code here is kind of 
                                ;tricky). After this operation
                                ;The upper three bits in N and the
                                ;upper three bits of the root (in W)
                                ;are the ones we'll look at next.
                                ;For W, xor'ing a 0x60 clears the
                                ;bit that was set when we loaded W with
                                ;0x40 (note that the upper two bits of
                                ;0x40 are 01). At the same time, the xor
                                ;sets the next bit. When done, the msb
                                ;of W will be the msb of our root and
                                ;the next two bits will be '01'. These
                                ;three bits are the 'root so far with
                                ;01 appended', and is what we need to
                                ;test against the upper 3 bits of N.
                                ;
                                ;
        bc      sq7_2           ;oops, N must have been greater than
                                ;0xc000. So we must unconditionally 
                                ;subtract the "root-so-far" and set 
                                ;the next bit. (the Carry got set when
                                ;when shifted N left)
                                ;
        CPFSLT  N_hi            ;Compare the upper three bits
         bra    sq7_2           ;
        bra     sq3_2           ;

sq7_2
        subwf   N_hi,f          ;Subtract the root so far
        iorlw   0x60            ;Set the next-to-msb

sq3_2
        rlcf    N_lo,f          ;Align N and the root
        rlcf    N_hi,f          ;
        xorlw   0x30            ;Clear bit 5 of the root and set bit 4

        bc      sq7_3           ;

        CPFSLT  N_hi            ;Compare the upper 4 bits
         bra    sq7_3
        bra     sq3_3

sq7_3
        subwf   N_hi,f
        iorlw   0x30

sq3_3   rlcf    N_lo,f
        rlcf    N_hi,f

        xorlw   0x18

        bc      sq7_4

        CPFSLT  N_hi            ;Compare the upper 5 bits
         bra    sq7_4
        bra     sq3_4


sq7_4
        subwf   N_hi,f
        iorlw   0x18

sq3_4   rlcf    N_lo,f
        rlcf    N_hi,f

        xorlw   0x0c

        bc      sq7_5
        CPFSLT  N_hi
         bra    sq7_5
        bra     sq3_5

sq7_5
        subwf   N_hi,f
        iorlw   0x0c

sq3_5   rlcf    N_lo,f
        rlcf    N_hi,f
        xorlw   0x06

        bc      sq7_6           ;Compare the upper 6 bits
        CPFSLT  N_hi
         bra    sq7_6
        bra     sq3_6

sq7_6
        subwf   N_hi,f
        iorlw   0x06

sq3_6   rlcf    N_lo,f
        rlcf    N_hi,f

        xorlw   0x03

        bc      sq7_7
        CPFSLT  N_hi            ;Compare the upper 7 bits
         bra    sq7_7
        bra     sq3_7


sq7_7
        subwf   N_hi,f
        iorlw   0x03

sq3_7   rlcf    N_lo,f
        rlcf    N_hi,f
        bc      done

        xorlw   1


   ;We are almost done. In the last iteration, we have 7 bits of the root. When
   ;"01" is appended to it, we will have a 9-bit number that must be subtracted
   ;from N. The 9-bits of N from which we must subtract are N_hi and the msb 
   ;of N_lo. The subwfb will allow us to do the subtract in one step


        RLCF    N_lo,F          ;Copy the MSB of N_lo into the borrow (carry)
        SUBWFB  N_hi,F          ;do a `9-bit' subtraction
        SKPNC                   ;If the result is positive
         XORLW  1               ;then toggle the lsb of the result
done      RETURN

        END