PIC Microcontoller Basic Math Method

32-bit signed integer stack based math routines. add, subtract, multiply, divide, by Alan Cashin

I am writing a program for a PIC16F628, needed to do arithmetic. I adapted PETER HEMSLEY SIGNED 32-BIT INTEGER MATHS to run a stack for things like (A-B)/(C-D). I wondered if there would be interest in this and where I'd post the routines. An example straight from my code (with extraneous bits removed): {Ed: quite useful for FORTH implementations)

    MOVLW   Vfast       ; push Vfast
    CALL    Push4
    MOVLW   Vslow       ; push Vslow
    CALL    Push4
    CALL    subtract    ; Vfast - Vslow
    MOVLW   Cfast       ; push Cfast
    CALL    Push3
    MOVLW   Cslow       ; push Cslow
    CALL    Push3
    CALL    subtract    ; Cfast - Cslow
    CALL    divide      ; get final result (slope)
    CALL    round
    MOVLW   Slope   ; pop result
    MOVWF   FSR
    MOVLW   4       ; 4 byte value returned
    CALL    Mpop

I'm not very familiar with the MPLAB X environment, so I'm not sure if there are conventions I didn't follow. I think there's enough comments in the code for people to cannibalise it for their own use.

There's a few of Peter Hemsley's routines not included as I was only interested in +-*/ - probably not too hard to add if someone needs them.

Because all pops and pushes work through the FSR/INDF method, the variables can be in either bank0 or bank1. I toyed with the idea of putting the stack in bank2, but it was extra complexity I didn't need, as there was enough space in 0,1 for my needs.

Also the usual caveat, tested for most things but not guaranteed to be bulletproof, and probably not very efficient.

Source code:

#INCLUDE P16F628A.inc
;
; Maths testing
;
    __CONFIG    _WDT_OFF & _PWRTE_ON & _LVP_OFF
;
; WDT (Watch Dog Timer) disabled
; PWRT (Power on Timer) enabled
; Low Voltage Programming disabled :-
; RB4/PGM pin has digital I/O function, HV on MCLR must be used for programming
;
; Multibyte numbers are stored little endian
;


;Variable declarations

dbank0  udata 0x20

Lit666		RES	2
Lit334		RES	2
Lit400		RES	2
WORKB		RES	3
WORKC		RES	3

dbank1	udata	0xA0
;
; Maths work area all in Bank 1
;
; Note that for the push and pop routines, the limit conditions expect the
; stack to be at address 0xA0 and be 32 bytes long. If a different address
; or size is desired, the limit conditions need changing
;
StackAddr	EQU	0xA0	; the EQUs to define REGA/B/D would
		; not accept STACK, so explicitly define the address again
				; so it was defined again. Not neat but works.
StackSiz	EQU	8	; Stack Size in 32bit numbers (see above)

STACK		RES	4*StackSiz
;
; The stack can be considered as an array of StackSiz columns of 4 rows.
; numbers are stored in columns - the bytes are StackSiz apart, not adjacent.
; The following EQUs are used to make the code more readable

; REGB is the 'top' of the stack, REGA is the 'first down'. Arithmetic
; is performed on REGA and REGB producing a result in REGA. Usually, each function
; pops the stack so the result overwrites REGB (top of stack).
; The exception is division, where REGA has the dividend and REGB
; the remainder. The caller can either pop both results or call the round
; function. The round function will round REGA then pop to get the rounded
; result at top af stack.
; 

REGA0		EQU	StackAddr+1		; lsb
REGA1		EQU	StackAddr+(StackSiz + 1)
REGA2		EQU	StackAddr+(StackSiz * 2 + 1)
REGA3		EQU	StackAddr+(StackSiz * 3 + 1)

REGB0		EQU	StackAddr		; lsb
REGB1		EQU	StackAddr+StackSiz
REGB2		EQU	StackAddr+StackSiz * 2
REGB3		EQU	StackAddr+StackSiz * 3

; REGD is the destination BEFORE push, ends up in REGB after push
REGD0		EQU	StackAddr+(StackSiz * 4 - 1)
REGD1		EQU	StackAddr+(StackSiz - 1)
REGD2		EQU	StackAddr+(StackSiz * 2 - 1)
REGD3		EQU	StackAddr+(StackSiz * 3 - 1)

REGC0		RES	1	; lsb
REGC1		RES	1	; REGC used in multiply and divide. It
REGC2		RES	1	; holds the divisor between divide and round
REGC3		RES	1

MTEMP		RES	1	; work area
MCOUNT		RES	1	;  "    "
LitVal		RES	2	; a space for +ve constants used in arithmetic

	ORG	0x000	; a reset redirects program to this point

	GOTO	MAIN

;
	ORG	0x004	; an interrupt redirects the program to here

;###########################################################

MAIN:
; compute (666+334)/(400-199)
; set up some values
	MOVLW   0x02 ; = 666
	MOVWF   Lit666+1
	MOVLW   0x9A
	MOVWF   Lit666
	MOVLW   0x01 ; = 334
	MOVWF   Lit334+1
	MOVLW   0x4E
	MOVWF   Lit334
	MOVLW   0x01 ; = 400
	MOVWF   Lit400+1
	MOVLW   0x90 ; a value for workA
	MOVWF   Lit400
;
	BSF	STATUS,RP0	; bank 1 for Arithmetic
;
	MOVLW   Lit666   ; push 666
	call	Push2
;
	MOVLW   Lit334   ; push 334
	call	Push2
;
	call	add
;
	MOVLW   Lit400   ; push 400
	call	Push2
;
	MOVLW   d'199'  ; push 199
	call	PushLit
;
	call	subtract
;
	call	divide
;
	MOVLW	WORKB   ; pop remainder in WORKB
	call	Pop3
	MOVLW	WORKB   ; and push it back
	call	Push3
;
	call	round
;
	MOVLW   WORKC	; pop result into workC
	call	Pop3
	BCF	STATUS,RP0	; bank 0 after arithmetic
;
; spin here
;
	GOTO	$

;##################### start math routines ##################

	errorlevel -302 ; Turn off banking message
; known tested (good) code
;
; Push entry points for various size variables.
; Push 1 to 4 expect signed variables
;
PushLit:
	MOVWF	LitVal	; entry to push 1 byte unsigned constants
	CLRF	LitVal+1; have to make them 2 byte otherwise
	MOVLW	LitVal	; values over 127 would be interpreted
	MOVWF	FSR	; as -ve
	MOVLW	2
	GOTO	Mpush

Push4:
	MOVWF	FSR ; entry to push a 4 byte variable
	MOVLW	4
	GOTO	Mpush

Push3:
	MOVWF	FSR ; entry to push a 3 byte variable
	MOVLW	3
	GOTO	Mpush

Push2:
	MOVWF	FSR ; entry to push a 2 byte variable
	MOVLW	2
	GOTO	Mpush

Push1:
	MOVWF	FSR ; entry to push a 1 byte variable
	MOVLW	1

Mpush:
;
; Transfers a new value to the stack with sign extension if required.
; The stack is barrel rolled 1 byte so the new value appears at REGB.
; The location of the value to push is held in FSR, the length specified
; by the least significant 4 bits of W. The remaining bits of W are
; unused but may have a future use so they are ANDed off.
;
; first, store passed info in the stack where the data is about
; to be 'pushed out'. It will be pushed to the location of REGB.
;
	ANDLW	0x0F		; bytes to move
	MOVWF	MCOUNT
	DECF	FSR,F		; move the pointer from the first byte
	ADDWF	FSR,F		; to the msb (the byte with the sign)
	MOVF	INDF,W  ; get msb
	ANDLW	0x80		; look at sign bit
	BTFSS	STATUS,Z; skip if +ve (W=0)
	MOVLW	0xFF
	MOVWF	REGD0		; fill the destination with sign bits
	MOVWF	REGD1		; except the MSB which will be always be
	MOVWF	REGD2		; overwritten
MUnext:
	MOVF	REGD2,W  	; shift destination register up one byte
	MOVWF	REGD3
	MOVF	REGD1,W
	MOVWF	REGD2
	MOVF	REGD0,W
	MOVWF	REGD1
	MOVF	INDF,W  	; get a byte of the incoming number
	MOVWF	REGD0 	; into LSB
	DECF	FSR,F		; repeat until all bytes moved
	DECFSZ  MCOUNT,F
	GOTO	MUnext
;
; roll the stack
;
	MOVLW	STACK+1
	MOVWF	FSR
	MOVF	STACK,W ; get the first byte of stack
MPu2:
	XORWF	INDF,W 	; 3 XORs swaps W and F
	XORWF	INDF,F	; Each byte in STACK is moved up 1 byte
	XORWF	INDF,W	; in memory, effectively moving each 4 byte
	INCF	FSR,F	; value to the next column
	BTFSC	FSR,5	; FSR ok from A1 to BF, leave when C0
	GOTO	MPu2
	MOVWF	STACK	; save what was the last byte of stack in first byte
; all done
	RETURN
;
; Pop entry points for 3 or 4 byte variables
; 3 byte values are truncated with no testing
;
Pop4:
	MOVWF	FSR
	MOVLW	4	; 4 byte value returned
	GOTO	Mpop
Pop3:
	MOVWF	FSR
	MOVLW	3	; 3 byte value returned

Mpop:
;
; transfers the value in REGB (top of stack) to a destination.
; The destination of the value to pop is held in FSR,
; the length specified by the least significant 4 bits of W.
; if length is less than 4, the most significant byte(s) is
; truncated. The stack is then popped (moved down 1 byte).
;
	ANDLW	0x0F		; bytes to move
	MOVWF	MCOUNT
MOnext:
	MOVF	STACK,W	; the next byte to move
	MOVWF	INDF	; into a destination byte
	MOVF	REGB1,W	; Shift REGB down one byte
	MOVWF	REGB0
	MOVF	REGB2,W
	MOVWF	REGB1
	MOVF	REGB3,W
	MOVWF	REGB2
	INCF	FSR,F	; pointer to next destination
	DECFSZ  MCOUNT,F; all moved?
	GOTO	MOnext

; value moved - pop the stack down

PopStk:

; this entry point also used by the arithmetic functions
; to move the result to the top of stack

	MOVLW	STACK+(StackSiz*4-2)
	MOVWF	FSR
	MOVF	STACK+(StackSiz*4-1),W ; last byte of stack
MPd2:
	XORWF	INDF,W	; The value of of each byte is moved
	XORWF	INDF,F	; down 1 byte in memory, effectively
	XORWF	INDF,W	; moving each value to the previous
	DECF	FSR,F	; column
	BTFSC	FSR,5	; FSR ok from BE to A0, leave when 9F
	GOTO	MPd2
; all done
	RETURN
;
; The following functions are based on:
;
;*** SIGNED 32-BIT INTEGER MATHS ROUTINES FOR PIC16 SERIES BY PETER HEMSLEY ***
;
;Functions:
;	add
;	subtract
;	multiply
;	divide
;	round
;
; These were NOT implemented: sqrt, bin2dec, dec2bin
;
; The original routines mostly used lower case for instructions. Additions or
; changes are mostly in upper case. Almost all the changes are to the division
; routine where the role of REGB and REGC are reversed so the reminder is
; left in REGB (if changing the register name was the only modification the
; instruction is still in lower case). The round routine was mostly rewritten
; to save duplicating existing code. Apart from divide, return is via
; PopStk. PopStk does not affect the state of the C flag.
;
; IMPORTANT: these routines assume RP0/1 are set to Bank 1 by the caller
;
;*** 32 BIT SIGNED SUBTRACT ***
;REGA - REGB -> REGA
;Return carry set if overflow

subtract
	call	negateb		;Negate REGB
	skpnc
	GOTO	PopStk		;Overflow

;*** 32 BIT SIGNED ADD ***
;REGA + REGB -> REGA
;Return carry set if overflow

add
	movf	REGA3,w		;Compare signs
	xorwf	REGB3,w
	movwf	MTEMP

	call	addba		;Add REGB to REGA

	clrc			;Check signs
	movf	REGB3,w		;If signs are same
	xorwf	REGA3,w		;so must result sign
	btfss	MTEMP,7		;else overflow
	addlw	0x80
	GOTO	PopStk

;*** 32 BIT SIGNED MULTIPLY ***
;REGA * REGB -> REGA
;Return carry set if overflow

multiply
	clrf	MTEMP		;Reset sign flag
	call	absa		;Make REGA positive
	skpc
	call	absb		;Make REGB positive
	skpnc
	GOTO	PopStk	;Overflow

;Move REGA to REGC
;Used by multiply

	movf	REGA0,w	; code variation: this was in a subroutine,
	movwf	REGC0		; but was moved inline
	movf	REGA1,w
	movwf	REGC1
	movf	REGA2,w
	movwf	REGC2
	movf	REGA3,w
	movwf	REGC3

;Clear REGA
;Used by multiply

	clrf	REGA0		;Clear product
	clrf	REGA1		; code variation: this was in a subroutine,
	clrf	REGA2		; but was moved inline
	clrf	REGA3
	movlw	D'31'		;Loop counter
	movwf	MCOUNT

muloop
	call	sla		;Shift left product and multiplicand
	rlf	REGC0,f	; code variation: this was in a subroutine,
	rlf	REGC1,f	; but was moved inline
	rlf	REGC2,f
	rlf	REGC3,f


	rlf	REGC3,w		;Test MSB of multiplicand
	skpnc			;If multiplicand bit is a 1 then
	call	addba		;add multiplier to product

	skpc			;Check for overflow
	rlf	REGA3,w
	skpnc
	GOTO	PopStk

	decfsz	MCOUNT,f	;Next
	goto	muloop

	btfsc	MTEMP,0		;Check result sign
	call	negatea		;Negative
	GOTO	PopStk


;*** 32 BIT SIGNED DIVIDE ***
;REGA / REGB -> REGA
;Remainder in REGB
;Return carry set if overflow or division by zero

divide
	clrf	MTEMP		;Reset sign flag
	call	absb		;Make divisor (REGB) positive
	skpnc
	return  			;Overflow
;
; modification - so the remainder ends up on the stack, REGB is moved to
; REGC. The use of REGB and REGC is the opposite of the original code but
; the logic remains the same
;
	MOVF	REGB0,w		; Move REGB (divisor) to REGC at the
	MOVWF	REGC0	; same time test for zero divisor
	MOVF	REGB1,w
	MOVWF	REGC1
	IORWF	REGB0,f
	MOVF	REGB2,w
	MOVWF	REGC2
	IORWF	REGB0,f
	MOVF	REGB3,w
	MOVWF	REGC3
	IORWF	REGB0,w
;
	sublw	0	; if all zero, will set C
	skpc
	call	absa		;Make dividend (REGA) positive
	skpnc
	return			;Overflow
; clear REGB to take the remainder
	clrf	REGB0		;Clear remainder
	clrf	REGB1
	clrf	REGB2
	clrf	REGB3
	call	sla		;Purge sign bit

	movlw	D'31'		;Loop counter
	movwf	MCOUNT

dvloop
	call	sla		;Shift dividend (REGA) msb into remainder (REGB)
	CALL	SlbTst		; shifts and tests remainder > divisor
	skpc			;Carry set if remainder >= divisor
	goto	dremlt

	movf	REGC0,w		;Subtract divisor (REGC) from remainder (REGB)
	subwf	REGB0,f
	movf	REGC1,w
	skpc
	incfsz	REGC1,w
	subwf	REGB1,f
	movf	REGC2,w
	skpc
	incfsz	REGC2,w
	subwf	REGB2,f
	movf	REGC3,w
	skpc
	incfsz	REGC3,w
	subwf	REGB3,f
	clrc
	bsf	REGA0,0		;Set quotient bit

dremlt
	decfsz	MCOUNT,f	;Next
	goto	dvloop

	btfsc	MTEMP,0		;Check result sign
	call	negatea		;Negative
	return

;*** ROUND RESULT OF DIVISION TO NEAREST INTEGER ***

round
; modified from original. Some code duplication was noticed so some was
; put in subroutine SlbTst and the IncA entry to negatea was added.
; No error testing, should not be capable of creating an error
	clrf	MTEMP		;Reset sign flag
	call	absa		;Make positive
	clrc
	CALL	SlbTst  	; shifts and tests remainder > divisor
	CLRW			; prevent IncA from returning an error
	BTFSC	STATUS,C	; Carry set if remainder >= divisor
	CALL	IncA		; Increment REGA
	btfsc	MTEMP,0		;Restore sign
	call	negatea
	GOTO	PopStk

;UTILITY ROUTINES


;Add REGB to REGA (Unsigned)
;Used by add, multiply,

addba	movf	REGB0,w		;Add lo byte
	addwf	REGA0,f

	movf	REGB1,w		;Add mid-lo byte
	skpnc			;No carry_in, so just add
	incfsz	REGB1,w		;Add carry_in to REGB
	addwf	REGA1,f		;Add and propagate carry_out

	movf	REGB2,w		;Add mid-hi byte
	skpnc
	incfsz	REGB2,w
	addwf	REGA2,f

	movf	REGB3,w		;Add hi byte
	skpnc
	incfsz	REGB3,w
	addwf	REGA3,f
	return


;Check sign of REGA and convert negative to positive
;Used by multiply, divide, round

absa	rlf	REGA3,w
	skpc
	return			;Positive

;Negate REGA
;Used by absa, multiply, divide, round

negatea	movf	REGA3,w		;Save sign in w
	andlw	0x80

	comf	REGA0,f		;2's complement
	comf	REGA1,f
	comf	REGA2,f
	comf	REGA3,f
	incf	MTEMP,f		;flip sign flag
IncA ; new entry point from round routine
	incfsz	REGA0,f
	goto	nega1
	incfsz	REGA1,f
	goto	nega1
	incfsz	REGA2,f
	goto	nega1
	incf	REGA3,f
nega1
	addwf	REGA3,w		;Return carry set if -2147483648
	return


;Check sign of REGB and convert negative to positive
;Used by multiply, divide

absb	rlf	REGB3,w
	skpc
	return			;Positive

;Negate REGB
;Used by absb, subtract, multiply, divide

negateb	movf	REGB3,w		;Save sign in w
	andlw	0x80

	comf	REGB0,f		;2's complement
	comf	REGB1,f
	comf	REGB2,f
	comf	REGB3,f
	incfsz	REGB0,f
	goto	negb1
	incfsz	REGB1,f
	goto	negb1
	incfsz	REGB2,f
	goto	negb1
	incf	REGB3,f
negb1
	incf	MTEMP,f		;flip sign flag
	addwf	REGB3,w		;Return carry set if -2147483648
	return

SlbTst:
;
; code modification: moved from divide, used by divide, round
; shifts remainder - when dividing, shifts in a bit from REGA;
; if rounding, a zero bit . Then tests remainder => divisor
;
	rlf	REGB0,f ; shift
	rlf	REGB1,f
	rlf	REGB2,f
	rlf	REGB3,f
	movf	REGC3,w	; Test
	subwf	REGB3,w
	skpz
	RETURN
	movf	REGC2,w
	subwf	REGB2,w
	skpz
	RETURN
	movf	REGC1,w
	subwf	REGB1,w
	skpz
	RETURN
	movf	REGC0,w
	subwf	REGB0,w
	RETURN


;Shift left REGA
;Used by multiply, divide, round

sla	rlf	REGA0,f
	rlf	REGA1,f
	rlf	REGA2,f
	rlf	REGA3,f
	return

	errorlevel +302 ; Enable banking message
; untested code
;##################### end math routines ##################

    END