round16.s

; Round top 16 bits of 32-bit fixed-point number in-place with with the 17th bit down
; Note: does not shift! The top bits remain in the top two bytes.
;
; If 0 bit: 5 + 12 = 5 cycles
; If 1 bit: 5 + 5 + 21 + 3? = 28 cycles for positive, 31 for negative
;
; Average given even distribution of data:
; 5 / 2 + 28 / 4 + 31 / 4 = 2.5 + 7 + 7.75 = 17.25 cycles
;
; Note a 16-bit copy is:
; 3 + 3 + 3 + 3 = 12 cycles

round_32_to_16:
  ; in: 32 bits fixed-point in arg..arg+3
  ; out: 16 bits fixed-point in arg+2..arg+3
  lda arg + 1    ; (3 cycles) load byte including 17th-from-top bit
  bpl end        ; (2 cycles)

round_path:
  lda arg + 3   ; (3 cycles) check sign bit
  bpl add_path  ; (2 cycles) determines add or subtract path

sub_path:
  clc           ; (2 cycles) clear carry bit for subtracting 1
  lda arg + 2   ; (3 cycles) load low byte
  sbc #0        ; (2 cycles) subtract
  sta arg + 2   ; (3 cycles) save low byte
  lda arg + 3   ; (3 cycles) load high byte
  sbc #0        ; (2 cycles) subtract
  sta arg + 3   ; (3 cycles) save high byte
  jmp end       ; (3 cycles)

add_path:
  sec           ; (2 cycles) set carry bit for adding 1
  lda arg + 2   ; (3 cycles) load low byte
  adc #0        ; (2 cycles) add
  sta arg + 2   ; (3 cycles) save low byte
  lda arg + 3   ; (3 cycles) load high byte
  adc #0        ; (2 cycles) add
  sta arg + 3   ; (3 cycles) save high byte

end: