fixed address decode bug

fixed address decode bug

was using the wrong extension word in some situations

;emulator for the MSP430, written to help solve the final level in the Matasano/Square CTF at https://microcorruption.com/

;data structure plans:
;memory to be represented as a large array of bytes
;registers as a much smaller array of words

;some utilities, that would be in a library normally

(ql:quickload "cl-utilities")

(use-package :cl-utilities) ;purely for split-sequence

(defun hex-char-to-int (chr)
  (let ((hex-index "0123456789abcdef")
        (int-index #(0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15)))
    (elt int-index (position chr hex-index))))

(defun hex-string-to-int (str)
  (if (equal str "")
      0
      (let ((down-str (string-downcase str)))
	(loop for i from 0 to (- (length str) 1)
	   for res = (hex-char-to-int (elt down-str i)) then (+ (* res 16) (hex-char-to-int (elt down-str i)))
	   finally (return res)))))

(defun 4bits-to-hex-char (bits)
  (let ((hex-index "0123456789abcdef")
        (int-index #(0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15)))
    (elt hex-index (position bits int-index))))

(defun list-to-string (l)
  (with-output-to-string (s)
    (dolist (c l)
      (princ c s))))

(defun int-to-hex-string (int)
  (list-to-string
   (loop for i = int then (/ (- i (mod i 16)) 16)
       while (>= i 1)
       with res = '()
       do (push (4bits-to-hex-char (mod i 16)) res)
       finally (return res))))

;global variables

(defvar *log-stream* *standard-output*)

(defvar *reg* (make-array 16 :element-type '(UNSIGNED-BYTE 16)))

(defvar *mem* (make-array 65536 :element-type '(UNSIGNED-BYTE 8)))

(defvar *input*)

(defvar *console* "console:")

(defvar *err*)

(defvar *dep* nil)

(defvar *dep-table* 0)

(defvar *unlocked* nil)

(defun log-line (arg)
  (if (integerp arg)
      (print (format nil "~4,'0x" arg) *log-stream*)
      (print arg *log-stream*))
  t)

;some utility functions specific to the emulator
(defun mem (idx)
  (elt *mem* idx))

(defun reg (idx)
  (elt *reg* idx))

(defun flag (idx)
  (ldb (byte 1 idx) (reg 2)))

(defun get-c ()
  (ldb (byte 1 0) (reg 2)))

(defun set-c (int)
  (setf (elt *reg* 2) (dpb int (byte 1 0) (reg 2))))

(defun get-z ()
  (ldb (byte 1 1) (reg 2)))

(defun set-z (int)
  (setf (elt *reg* 2) (dpb int (byte 1 1) (reg 2))))

(defun get-n ()
  (ldb (byte 1 2) (reg 2)))

(defun set-n (int)
  (setf (elt *reg* 2) (dpb int (byte 1 2) (reg 2))))

(defun get-v ()
  (ldb (byte 1 8) (reg 2)))

(defun set-v (int)
  (setf (elt *reg* 2) (dpb int (byte 1 8) (reg 2))))

(defun bytes-to-word (low high)
  (+ (* 256 high) low))

(defun mod+ (&rest args)
  (mod (reduce #'+ args) 65536))

(defun mod* (&rest args)
  (mod (reduce #'* args) 65536))

(defun get-mem (idx)
;  (log-line "get-mem")
 ; (log-line idx)
  ;(log-line (bytes-to-word (mem idx) (mem (+ idx 1))))
  (bytes-to-word (mem idx) (mem (+ idx 1))))

(defun set-mem (idx int b-w)
  (log-line "set-mem")
 (log-line (get-mem idx))
  (log-line "idx")
 (log-line idx)
  (let* ((low (mod int 256))
	 (high (/ (- int low) 256)))
    (if (= b-w 0)
	(setf (elt *mem* idx) low)
	(setf (elt *mem* idx) low
	      (elt *mem* (+ idx 1)) high)))
  (log-line "res")
  (log-line (get-mem idx))
  t)

(defun mem-fun (idx)
  (function (lambda (int b-w) (set-mem idx int b-w))))

(defun set-reg (idx int)
  (setf (elt *reg* idx) int))
;  (let ((low (ldb (byte 8 0) int))
;	(high (ldb (byte 8 8) int)))
;    (setf (elt *reg* idx) (+ (dpb low (byte 8 8) 0)
;			     (dpb high (byte 8 0) 0)))))

(defun reg-fun (idx)
  (function (lambda (int b-w)
    (set-reg idx (trim-bw int b-w)))))

(defun get-reg (idx)
  (elt *reg* idx))
;  (let ((val (reg idx)))
;    (let ((low (ldb (byte 8 0) val))
;	  (high (ldb (byte 8 8) val)))
;      (+ (dpb low (byte 8 8) 0)
;	 (dpb high (byte 8 0) 0)))))

(defun inc-reg (idx)
  (incf (elt *reg* idx) 2))
;  (set-reg idx (+ 2 (get-reg idx))))

(defun dec-reg (idx)
  (incf (elt *reg* idx) -2))
;  (set-reg idx (+ -2 (get-reg idx))))

(defun dep-set (page val)
  (setf *dep-table* (dpb val (byte 1 page) *dep-table*)))

(defun dep-check (page)
  (ldb (byte 1 page) *dep-table*))

(defun nullp (val b-w)
  (if (= (trim-bw val b-w) 0)
      1
      0))

(defun negp (val b-w)
  (ldb (byte 1 (+ b-w 7)) val))

(defun trim-bw (val b-w)
  (ldb (byte (+ b-w 8) 0) val))

;start of actual emulator
(defun main ()
  (setf *reg* (make-array 16 :element-type '(unsigned-byte 16)))
  (set-reg 0 #x4400)
  (loop repeat 1000000
     while (emu-step)
       do (if *unlocked*
	      (return-from main "victory"))))

(defun emu-step ()
  (let* ((pc (get-reg 0))
	 (bytes (subseq *mem* pc (+ pc 6))))
    (log-line bytes)
    (log-line "PC")
    (log-line pc)
    (log-line "sp")
    (log-line (get-reg 1))
    (log-line "sr")
    (log-line (get-reg 2))
    (log-line "registers")
    (log-line *reg*)
    (cond ((oddp pc) (progn (setf *err* "PC unalligned")
			    (return-from emu-step nil)))
	  ((= (ldb (byte 1 4) (get-reg 2)) 1) (progn (setf *err* "cpu-off set")
						 (return-from emu-step nil)))
	  ((= pc 16) (int-handler (ldb (byte 7 8) (get-reg 2))))
	  ((= (bytes-to-word (elt bytes 0) (elt bytes 1)) 0)
	   (progn (setf *err* "zero instruction")
			    (return-from emu-step nil)))
	  (t (instruction-decode bytes)))
    ))

(defun int-handler (type)
  (log-line "interrupt")
  (log-line (subseq *mem* (get-reg 1) (+ 16 (get-reg 1))))
  (let ((arg1 (get-mem (+ 8 (get-reg 1))))
	(arg2 (get-mem (+ 10 (get-reg 1)))))
    (cond ((= type 0) (setf *console* (concatenate 'string *console*
						   (list (code-char (ldb (byte 8 0) arg1))))))
	  ((= type 2) (getsn arg1 arg2))
	  ((= type 3) (dep-set arg1 arg2))
	  ((= type 4) (set-reg 15 (random 65536 (make-random-state t))))
	  ((= type #x7d) (set-mem arg2 0 0)) ;assume password is always false
	  ((= type #x7e) t) ;assume passsword is always false
	  ((= type #x7f) (setf *unlocked* t)))
    (set-reg 0 (get-mem (get-reg 1)))
    (inc-reg 1)))

(defun getsn (start num)
  (log-line "getsn")
  (log-line start)
  (log-line num)
  (log-line (car *input*))
  (let ((cur-input (car *input*)))
    (loop for i from 0 to (- (/ (length cur-input) 2) 1)
       while (<= i num)
       for char = (hex-string-to-int (subseq cur-input (* i 2) (* (+ i 1) 2)))
       do (set-mem (+ start i) char 0)))
  (setf *input* (cdr *input*)))

(defun instruction-decode (bytes)
  ;bytes to be an arry of 6 bytes, to allow for the longest possible instruction
  ;this is the entry point into the instruction decoding, will eventually call the
  ;appropriate function
  (let ((instn (bytes-to-word (elt bytes 0) (elt bytes 1)))
	(data1 (bytes-to-word (elt bytes 2) (elt bytes 3)))
	(data2 (bytes-to-word (elt bytes 4) (elt bytes 5))))
    (cond ((= (ldb (byte 4 12) instn) 1) (single-decode instn data1))
	  ((= (ldb (byte 3 13) instn) 1) (jump-decode instn))
	  ((>= (ldb (byte 4 12) instn) 4) (double-decode instn data1 data2)))))

(defun address-decode (ad dest data b-w)
  (log-line ad)
  (log-line dest)
  (log-line data)
  ;decodes an address for a single operand instruction
  ;returns an integer representing the current value and a function that sets the new value
  (if (or (= dest 2) (= dest 3))
      (if (= dest 2)
	  (cond ((= ad 0) (values (get-reg dest)
				  (reg-fun dest)
				  0)) ;register mode
		((= ad 1) (progn (inc-reg 0)
				 (values (get-mem data)
					 (mem-fun data)
					 1)));absolute mode
		((= ad 2) (values 4 (reg-fun dest) 0)) ;represents a constant, can't set things
		((= ad 3) (values 8 (reg-fun dest) 0)) ;as above
		(t 'nil))
	  (cond ((= ad 0) (values 0 (reg-fun dest) 0)) ;dest = 3
		((= ad 1) (values 1 (reg-fun dest) 0))
		((= ad 2) (values 2 (reg-fun dest) 0))
		((= ad 3) (values 65535 (reg-fun dest) 0))
		(t 'nil)))
      (cond ((= ad 0) (values (get-reg dest)
			      (reg-fun dest)
			      0)) ;register mode
	    ((= ad 1) (let ((idx (mod+ (get-reg dest) data)))
			(inc-reg 0)
			(values (get-mem idx)
				(mem-fun idx)
				1))) ;index mode
	    ((= ad 2) (values (get-mem (get-reg dest))
			      (mem-fun (get-reg dest))
			      0)) ;indirect mode
	    ((= ad 3) (progn (addr-increment dest b-w)
			     (values (get-mem (- (get-reg dest) (if (= dest 0)
								    2
								    (+ 1 (/ b-w 8)))))
				     (mem-fun (- (get-reg dest) (if (= dest 0)
								    2
								    (+ 1 (/ b-w 8)))))
				     0))) ;indirect mode with increment
	    (t 'nil))))

(defun addr-increment (idx b-w)
  (if (= 0 idx)
      (incf (elt *reg* idx) 2)
      (incf (elt *reg* idx) (+ 1 (/ b-w 8)))))

(defun single-decode (instn data)
  (log-line "single-op")
  (inc-reg 0)
  ;decodes and calls a single operand instruction
  (let ((opcode (ldb (byte 3 7) instn))
	(ad (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (if (= 1 (ldb (byte 1 6) instn))
		 0
		 8)))
    (multiple-value-bind (value set-fun)
	(address-decode ad dest data b-w)
      ;(log-line value)
      ;(log-line set-fun)
      (cond ((= opcode 0) (rrc value set-fun b-w))
	    ((= opcode 1) (swpb value set-fun))
	    ((= opcode 2) (rra value set-fun b-w))
	    ((= opcode 3)  (sxt value set-fun))
	    ((= opcode 4) (psh value set-fun b-w))
	    ((= opcode 5) (call value set-fun))
	    (t 'nil)))))
	      ;RETI isn't implemented because it's not in the CTF

(defun rrc (value set-fun b-w)
  (log-line "rrc")
  (let ((carry (ldb (byte 1 0) value))
	(tmp (ash (trim-bw value b-w) -1)))
    (let ((res (dpb (get-c) (byte 1 (+ b-w 7)) tmp)))
      (funcall set-fun res b-w)
      (set-v 0)
      (set-c carry)
      (if (= (negp res b-w) 1)
	  (set-n 1))
      (if (= (nullp res b-w) 0)
	  (set-z 0)))))

(defun swpb (value set-fun)
  (log-line "swpb")
  (let ((low (ldb (byte 8 8) value))
	(high (ldb (byte 8 0) value)))
    (funcall set-fun (+ (* 256 high) low) 8)))

(defun rra (value set-fun b-w)
  (log-line "rra")
  (let ((res (ash (trim-bw value b-w) -1)))
    (funcall set-fun res b-w)
    (if (= (negp res b-w) 1)
	(set-n 1))
    (set-v 0)
    (set-z 0)))

(defun sxt (value set-fun)
  (log-line "sxt")
  (let ((res (dpb (* 255 (ldb (byte 1 7) value)) (byte 8 8) value)))
    (funcall set-fun res 8)
    (set-c (if (= res 0)
	       0
	       1))
    (set-v 0)
    (set-n (negp res 8))
    (set-z (nullp res 8))))

(defun psh (value set-fun b-w)
  (log-line "psh")
  (log-line value)
  (dec-reg 1)
  (set-mem (get-reg 1) value b-w))

(defun call (value set-fun)
  (log-line "call")
  (log-line value)
  (psh (get-reg 0) nil 8)
  (set-reg 0 value))

(defun jump-decode (instn)
  (log-line "jump")
  (log-line instn)
  (log-line (get-reg 2))
  (let ((type (ldb (byte 3 10) instn))
	(offset (ldb (byte 10 0) instn))
	(z (flag 1))
	(c (flag 0))
	(n (flag 2))
	(v (flag 8)))
    (cond ((= type 0) (if (= z 0)
			  (jmp offset)
			  (inc-reg 0)))
	  ((= type 1) (if (= z 1)
			  (jmp offset)
			  (inc-reg 0)))
	  ((= type 2) (if (= c 0)
			  (jmp offset)
			  (inc-reg 0)))
	  ((= type 3) (if (= c 1) (jmp offset)
			  (inc-reg 0)))
	  ((= type 4) (if (= n 1) (jmp offset)
			  (inc-reg 0)))
	  ((= type 5) (if (= n v) (jmp offset)
			  (inc-reg 0)))
	  ((= type 6) (if (/= n v) (jmp offset)
			  (inc-reg 0)))
	  ((= type 7) (jmp offset)))))

(defun jmp (offset)
  (log-line offset)
  (log-line (ldb (byte 9 0) offset))
  (log-line (* (ldb (byte 1 9) offset) 1024))
  (log-line (- (* 2 (- (ldb (byte 9 0) offset)
		       (* (ldb (byte 1 9) offset) 512)))))
  (set-reg 0 (+ (get-reg 0) 2
		(* 2 (- (ldb (byte 9 0) offset)
			(* (ldb (byte 1 9) offset) 512)))))
  t)

(defun double-decode (instn data1 data2)
  (log-line "double-op")
  (inc-reg 0)
  (let ((opcode (ldb (byte 4 12) instn))
	(source (ldb (byte 4 8) instn))
	(ad (ldb (byte 1 7) instn))
	(as (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (if (= 1 (ldb (byte 1 6) instn))
		 0
		 8)))
    (multiple-value-bind (source-val source-fun data-offset)
	(address-decode as source data1 b-w)
      (multiple-value-bind (dest-val dest-fun junk-val)
	  (address-decode ad dest (if (= data-offset 1)
				      data2    ;use the second extension word if
				      data1)
			  b-w)  ;the first was used in the source
;      (let* ((source-lst (multiple-value-list (address-decode as source data1)))
;	     (source-val (car source-lst)))
	;(log-line source-lst)
	(log-line "source")
	(log-line source-val)
	(log-line "dest")
	(log-line dest-val)
	(cond ((= opcode 4) (mov source-val dest-fun b-w))
	      ((= opcode 5) (add source-val dest-val dest-fun b-w))
	      ((= opcode 6) (addc source-val dest-val dest-fun b-w))
	      ((= opcode 7) (subc source-val dest-val dest-fun b-w))
	      ((= opcode 8) (sub source-val dest-val dest-fun b-w))
	      ((= opcode 9) (cmp source-val dest-val b-w))
	      ((= opcode 10) (dadd source-val dest-val dest-fun b-w))
	      ((= opcode 11) (emu-bit source-val dest-val b-w))
	      ((= opcode 12) (bic source-val dest-val dest-fun b-w))
	      ((= opcode 13) (bis source-val dest-val dest-fun b-w))
	      ((= opcode 14) (xor source-val dest-val dest-fun b-w))
	      ((= opcode 15) (emu-and source-val dest-val dest-fun b-w))
	      (t 'nil))))))

(defun mov (val dest-fun b-w)
  (log-line "mov")
 (log-line  val)
  (log-line dest-fun)
  (log-line b-w)
  (funcall dest-fun val b-w))

(defun add-flags (res b-w)
  (set-c (ldb (byte 1 (+ b-w 8)) res))
  (set-v 0)
  (set-n (negp res b-w))
  (set-z (nullp res b-w)))

(defun add (source-val dest-val dest-fun b-w)
  (log-line "add")
  (log-line "source")
  (log-line source-val)
  (log-line "dest")
  (log-line dest-val)
  (log-line b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun addc (source-val dest-val dest-fun b-w)
  (log-line "addc")
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w) (get-c))))
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun subc (source-val dest-val dest-fun b-w)
  (log-line "subc")
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) (get-c) 1))) ;random +1 for bug compat
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun sub (source-val dest-val dest-fun b-w)
  (log-line "sub")
  (let ((res (+ (trim-bw dest-val b-w) (trim-bw (lognot source-val) b-w) 1)))
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun cmp (source-val dest-val b-w)
  (log-line "cmp")
  (log-line  source-val)
  (log-line  dest-val)
  (log-line b-w)
  (let ((res (+ (trim-bw dest-val b-w) (trim-bw (lognot source-val) b-w) 1)))
    (add-flags res b-w))
  (log-line (get-reg 2)))

(defun dadd (source-val dest-val dest-fun b-w)
  (log-line "dadd")
  (let ((trim-s (trim-bw source-val b-w))
	(trim-d (trim-bw dest-val b-w)))
    (let ((res (loop for i in (list 0 4 8 12)
		  for c = 0 then (if (>= tmp 10)
				     1
				     0)
		  for tmp = (+ (ldb (byte 4 i) trim-s)
			       (ldb (byte 4 i) trim-d)
			       c)
		  for tmp2 = (if (>= tmp 10)
				 (- tmp 10)
				 tmp)
		  for val = tmp2 then (dpb tmp2 (byte 4 i) val)
		  finally (progn (set-c c)
				 (if (ldb (byte 1 3) tmp)
				     (set-n 1))
				 (return val)))))
      (funcall dest-fun res b-w))))

(defun emu-bit (source-val dest-val b-w)
  (log-line "emu-bit")
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res b-w))
    (set-c (nullp res b-w))))

(defun bic (source-val dest-val dest-fun b-w)
  (log-line "bic")
  (let ((res (logand (lognot (trim-bw source-val b-w)) (trim-bw dest-val b-w))))
    (funcall dest-fun res b-w)))

(defun bis (source-val dest-val dest-fun b-w)
  (log-line "bis")
  (funcall dest-fun (logior (trim-bw source-val b-w) (trim-bw dest-val b-w)) b-w))

(defun xor (source-val dest-val dest-fun b-w)
  (log-line "xor")
  (let ((res (logxor (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun res b-w)
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res b-w))
    (set-c (nullp res b-w))))

(defun emu-and (source-val dest-val dest-fun b-w)
  (log-line "emu-and")
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun res b-w)
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res b-w))
    (set-c (nullp res b-w))))

;start of file importing code

(defun import-file (file)
  (setf *mem* (make-array 65536 :element-type '(unsigned-byte 8)))
  (with-open-file (in file)
    (loop for line = (read-line in nil)
       while line
       do (if (not (equal #\* (elt line 8)))
	      (let* ((split-line (split-sequence #\: line))
		     (offset (hex-string-to-int (car split-line)))
		     (blocks (subseq (split-sequence #\space (cadr split-line)) 3 11)))
		(loop for i from 0 to 7
		   for block = (elt blocks i)
		   do (set-mem (+ offset (* i 2)) (block-val block) 8)))))))

(defun block-val (str)
  (hex-string-to-int
   (concatenate 'string (subseq str 2 4) (subseq str 0 2))))

(defun reset ()
  (setf *reg* (make-array 16 :element-type '(UNSIGNED-BYTE 16)))
  (setf *mem* (make-array 65536 :element-type '(UNSIGNED-BYTE 8)))
  (setf *err* "null")
  (setf *console* "console:")
  (setf *unlocked* nil))

(defun run-level (file-name input)
  (reset)
  (setf *log-stream* (open "C:\\ben\\log.txt" :direction :output :if-exists :supersede :sharing :external))
  ;(setf *log-stream* *standard-output*)
  (print "start of run")
  (import-file file-name)
  (setf *input* input)
  (main)
  (print file-name)
  (print *console*)
  (print "error:")
  (print *err*)
  (print "unlocked?")
  (print *unlocked*))

;emulator for the MSP430, written to help solve the final level in the Matasano/Square CTF at https://microcorruption.com/

;data structure plans:
;memory to be represented as a large array of bytes
;registers as a much smaller array of words

;some utilities, that would be in a library normally

(ql:quickload "cl-utilities")

(use-package :cl-utilities) ;purely for split-sequence

(defun hex-char-to-int (chr)
  (let ((hex-index "0123456789abcdef")
        (int-index #(0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15)))
    (elt int-index (position chr hex-index))))

(defun hex-string-to-int (str)
  (if (equal str "")
      0
      (let ((down-str (string-downcase str)))
	(loop for i from 0 to (- (length str) 1)
	   for res = (hex-char-to-int (elt down-str i)) then (+ (* res 16) (hex-char-to-int (elt down-str i)))
	   finally (return res)))))

(defun 4bits-to-hex-char (bits)
  (let ((hex-index "0123456789abcdef")
        (int-index #(0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15)))
    (elt hex-index (position bits int-index))))

(defun list-to-string (l)
  (with-output-to-string (s)
    (dolist (c l)
      (princ c s))))

(defun int-to-hex-string (int)
  (list-to-string
   (loop for i = int then (/ (- i (mod i 16)) 16)
       while (>= i 1)
       with res = '()
       do (push (4bits-to-hex-char (mod i 16)) res)
       finally (return res))))

;global variables

(defvar *reg* (make-array 16 :element-type '(UNSIGNED-BYTE 16)))

(defvar *mem* (make-array 65536 :element-type '(UNSIGNED-BYTE 8)))

(defvar *input*)

(defvar *console* "console:")

(defvar *err*)

(defvar *dep* nil)

(defvar *dep-table* 0)

(defvar *unlocked* nil)

(defmacro log-line (&body body)
  (let ((type "fl"))
    (if (equalp type "pr") `(if (integerp ,(car body))
				  (print (int-to-hex-string ,(car body)))
				  (print ,(car body))))
    (if (equalp type "fl") (progn
			     (let ((file (open "C:\\ben\\log.txt" :direction :output :if-exists :supersede)))
			       `(if (integerp ,(car body))
				    (write-line (int-to-hex-string ,(car body)) file)
				    (write-line ,(car body) file)))))
    (if (equalp type "nil") '1)))

;some utility functions specific to the emulator
(defun mem (idx)
  (elt *mem* idx))

(defun reg (idx)
  (elt *reg* idx))

(defun flag (idx)
  (ldb (byte 1 idx) (reg 2)))

(defun get-c ()
  (ldb (byte 1 0) (reg 2)))

(defun set-c (int)
  (setf (elt *reg* 2) (dpb int (byte 1 0) (reg 2))))

(defun get-z ()
  (ldb (byte 1 1) (reg 2)))

(defun set-z (int)
  (setf (elt *reg* 2) (dpb int (byte 1 1) (reg 2))))

(defun get-n ()
  (ldb (byte 1 2) (reg 2)))

(defun set-n (int)
  (setf (elt *reg* 2) (dpb int (byte 1 2) (reg 2))))

(defun get-v ()
  (ldb (byte 1 8) (reg 2)))

(defun set-v (int)
  (setf (elt *reg* 2) (dpb int (byte 1 8) (reg 2))))

(defun bytes-to-word (low high)
  (+ (* 256 high) low))

(defun mod+ (&rest args)
  (mod (reduce #'+ args) 65536))

(defun mod* (&rest args)
  (mod (reduce #'* args) 65536))

(defun get-mem (idx)
  (log-line "get-mem")
  (log-line idx)
  (log-line (bytes-to-word (mem idx) (mem (+ idx 1))))
  (bytes-to-word (mem idx) (mem (+ idx 1))))

(defun set-mem (idx int b-w)
  (log-line "set-mem")
 (log-line (get-mem idx))
  (log-line "idx")
 (log-line idx)
  (let* ((low (mod int 256))
	 (high (/ (- int low) 256)))
    (if (= b-w 0)
	(setf (elt *mem* idx) low)
	(setf (elt *mem* idx) low
	      (elt *mem* (+ idx 1)) high)))
  (log-line "res")
  (log-line (get-mem idx))
  t)

(defun mem-fun (idx)
  (function (lambda (int b-w) (set-mem idx int b-w))))

(defun set-reg (idx int)
  (setf (elt *reg* idx) int))
;  (let ((low (ldb (byte 8 0) int))
;	(high (ldb (byte 8 8) int)))
;    (setf (elt *reg* idx) (+ (dpb low (byte 8 8) 0)
;			     (dpb high (byte 8 0) 0)))))

(defun reg-fun (idx)
  (function (lambda (int b-w)
    (set-reg idx (trim-bw int b-w)))))

(defun get-reg (idx)
  (elt *reg* idx))
;  (let ((val (reg idx)))
;    (let ((low (ldb (byte 8 0) val))
;	  (high (ldb (byte 8 8) val)))
;      (+ (dpb low (byte 8 8) 0)
;	 (dpb high (byte 8 0) 0)))))

(defun inc-reg (idx)
  (incf (elt *reg* idx) 2))
;  (set-reg idx (+ 2 (get-reg idx))))

(defun dec-reg (idx)
  (incf (elt *reg* idx) -2))
;  (set-reg idx (+ -2 (get-reg idx))))

(defun dep-set (page val)
  (setf *dep-table* (dpb val (byte 1 page) *dep-table*)))

(defun dep-check (page)
  (ldb (byte 1 page) *dep-table*))

(defun nullp (val b-w)
  (if (= (trim-bw val b-w) 0)
      1
      0))

(defun negp (val b-w)
  (ldb (byte 1 (+ b-w 7)) val))

(defun trim-bw (val b-w)
  (ldb (byte (+ b-w 8) 0) val))

;start of actual emulator
(defun main ()
  (setf *reg* (make-array 16 :element-type '(unsigned-byte 16)))
  (set-reg 0 #x4400)
  (loop repeat 1000000
     while (emu-step)
       do (if *unlocked*
	      (return-from main "victory"))))

(defun emu-step ()
  (let* ((pc (get-reg 0))
	 (bytes (subseq *mem* pc (+ pc 6))))
    (log-line bytes)
    (log-line "PC")
   (log-line pc)
    (log-line "sp")
   (log-line (get-reg 1))
    (log-line "sr")
   (log-line (get-reg 2))
    (cond ((oddp pc) (progn (setf *err* "PC unalligned")
			    (return-from emu-step nil)))
	  ((= (ldb (byte 1 4) (get-reg 2)) 1) (progn (setf *err* "cpu-off set")
						 (return-from emu-step nil)))
	  ((= pc 16) (int-handler (ldb (byte 7 8) (get-reg 2))))
	  ((= (bytes-to-word (elt bytes 0) (elt bytes 1)) 0)
	   (progn (setf *err* "zero instruction")
			    (return-from emu-step nil)))
	  (t (instruction-decode bytes)))
    ))

(defun int-handler (type)
  (log-line "interrupt")
  (log-line (subseq *mem* (get-reg 1) (+ 16 (get-reg 1))))
  (let ((arg1 (get-mem (+ 8 (get-reg 1))))
	(arg2 (get-mem (+ 10 (get-reg 1)))))
    (cond ((= type 0) (setf *console* (concatenate 'string *console*
						   (list (code-char (ldb (byte 8 0) arg1))))))
	  ((= type 2) (getsn arg1 arg2))
	  ((= type 3) (dep-set arg1 arg2))
	  ((= type 4) (set-reg 15 (random 65536 (make-random-state t))))
	  ((= type #x7d) (set-mem arg2 0 0)) ;assume password is always false
	  ((= type #x7e) t) ;assume passsword is always false
	  ((= type #x7f) (setf *unlocked* t)))
    (set-reg 0 (get-mem (get-reg 1)))
    (inc-reg 1)))

(defun getsn (start num)
  (log-line "getsn")
  (log-line start)
  (log-line num)
  (log-line (car *input*))
  (let ((cur-input (car *input*)))
    (loop for i from 0 to (- (/ (length cur-input) 2) 1)
       while (<= i num)
       for char = (hex-string-to-int (subseq cur-input (* i 2) (* (+ i 1) 2)))
       do (set-mem (+ start i) char 0)))
  (setf *input* (cdr *input*)))

(defun instruction-decode (bytes)
  ;bytes to be an arry of 6 bytes, to allow for the longest possible instruction
  ;this is the entry point into the instruction decoding, will eventually call the
  ;appropriate function
  (let ((instn (bytes-to-word (elt bytes 0) (elt bytes 1)))
	(data1 (bytes-to-word (elt bytes 2) (elt bytes 3)))
	(data2 (bytes-to-word (elt bytes 4) (elt bytes 5))))
    (cond ((= (ldb (byte 4 12) instn) 1) (single-decode instn data1))
	  ((= (ldb (byte 3 13) instn) 1) (jump-decode instn))
	  ((>= (ldb (byte 4 12) instn) 4) (double-decode instn data1 data2)))))

(defun address-decode (ad dest data b-w)
  (log-line ad)
  (log-line dest)
 (log-line  data)
  ;decodes an address for a single operand instruction
  ;returns an integer representing the current value and a function that sets the new value
  (if (or (= dest 2) (= dest 3))
      (if (= dest 2)
	  (cond ((= ad 0) (values (get-reg dest)
				  (reg-fun dest))) ;register mode
		((= ad 1) (progn (inc-reg 0)
				 (values (get-mem data)
					 (mem-fun data))));absolute mode
		((= ad 2) (values 4 4)) ;represents a constant, can't set things
		((= ad 3) (values 8 8)) ;as above
		(t 'nil))
	  (cond ((= ad 0) (values 0 0)) ;dest = 3
		((= ad 1) (values 1 1))
		((= ad 2) (values 2 2))
		((= ad 3) (values 65535 65535))
		(t 'nil)))
      (cond ((= ad 0) (values (get-reg dest)
			      (reg-fun dest))) ;register mode
	    ((= ad 1) (let ((idx (mod+ (get-reg dest) data)))
			(inc-reg 0)
			(values (get-mem idx)
				(mem-fun idx)))) ;index mode
	    ((= ad 2) (values (get-mem (get-reg dest))
			      (mem-fun (get-reg dest)))) ;indirect mode
	    ((= ad 3) (progn (addr-increment dest b-w)
			     (values (get-mem (- (get-reg dest) (if (= dest 0)
								    2
								    (+ 1 (/ b-w 8)))))
				     (mem-fun (- (get-reg dest) (if (= dest 0)
								    2
								    (+ 1 (/ b-w 8)))))))) ;indirect mode with increment
	    (t 'nil))))

(defun addr-increment (idx b-w)
  (if (= 0 idx)
      (incf (elt *reg* idx) 2)
      (incf (elt *reg* idx) (+ 1 (/ b-w 8)))))

(defun single-decode (instn data)
  (log-line "single-op")
  (inc-reg 0)
  ;decodes and calls a single operand instruction
  (let ((opcode (ldb (byte 3 7) instn))
	(ad (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (if (= 1 (ldb (byte 1 6) instn))
		 0
		 8)))
    (multiple-value-bind (value set-fun)
	(address-decode ad dest data b-w)
      ;(log-line value)
      ;(log-line set-fun)
      (cond ((= opcode 0) (rrc value set-fun b-w))
	    ((= opcode 1) (swpb value set-fun))
	    ((= opcode 2) (rra value set-fun b-w))
	    ((= opcode 3)  (sxt value set-fun))
	    ((= opcode 4) (psh value set-fun b-w))
	    ((= opcode 5) (call value set-fun))
	    (t 'nil)))))
	      ;RETI isn't implemented because it's not in the CTF

(defun rrc (value set-fun b-w)
  (log-line "rrc")
  (let ((carry (ldb (byte 1 0) value))
	(tmp (ash (trim-bw value b-w) -1)))
    (let ((res (dpb (get-c) (byte 1 (+ b-w 7)) tmp)))
      (funcall set-fun res b-w)
      (set-v 0)
      (set-c carry)
      (if (= (negp res b-w) 1)
	  (set-n 1))
      (if (= (nullp res b-w) 0)
	  (set-z 0)))))

(defun swpb (value set-fun)
  (log-line "swpb")
  (let ((low (ldb (byte 8 8) value))
	(high (ldb (byte 8 0) value)))
    (funcall set-fun (+ (* 256 high) low) 8)))

(defun rra (value set-fun b-w)
  (log-line "rra")
  (let ((res (ash (trim-bw value b-w) -1)))
    (funcall set-fun res b-w)
    (if (= (negp res b-w) 1)
	(set-n 1))
    (set-v 0)
    (set-z 0)))

(defun sxt (value set-fun)
  (log-line "sxt")
  (let ((res (dpb (* 255 (ldb (byte 1 7) value)) (byte 8 8) value)))
    (funcall set-fun res 8)
    (set-c (if (= res 0)
	       0
	       1))
    (set-v 0)
    (set-n (negp res 8))
    (set-z (nullp res 8))))

(defun psh (value set-fun b-w)
  (log-line "psh")
  (log-line value)
  (dec-reg 1)
  (set-mem (get-reg 1) value b-w))

(defun call (value set-fun)
  (log-line "call")
  (log-line value)
  (psh (get-reg 0) nil 8)
  (set-reg 0 value))

(defun jump-decode (instn)
  (log-line "jump")
  (log-line instn)
  (log-line (get-reg 2))
  (let ((type (ldb (byte 3 10) instn))
	(offset (ldb (byte 10 0) instn))
	(z (flag 1))
	(c (flag 0))
	(n (flag 2))
	(v (flag 8)))
    (cond ((= type 0) (if (= z 0)
			  (jmp offset)
			  (inc-reg 0)))
	  ((= type 1) (if (= z 1)
			  (jmp offset)
			  (inc-reg 0)))
	  ((= type 2) (if (= c 0)
			  (jmp offset)
			  (inc-reg 0)))
	  ((= type 3) (if (= c 1) (jmp offset)
			  (inc-reg 0)))
	  ((= type 4) (if (= n 1) (jmp offset)
			  (inc-reg 0)))
	  ((= type 5) (if (= n v) (jmp offset)
			  (inc-reg 0)))
	  ((= type 6) (if (/= n v) (jmp offset)
			  (inc-reg 0)))
	  ((= type 7) (jmp offset)))))

(defun jmp (offset)
  (log-line offset)
  (log-line (ldb (byte 9 0) offset))
  (log-line (* (ldb (byte 1 9) offset) 1024))
  (log-line (- (* 2 (- (ldb (byte 9 0) offset)
		       (* (ldb (byte 1 9) offset) 512)))))
  (set-reg 0 (+ (get-reg 0) 2
		(* 2 (- (ldb (byte 9 0) offset)
			(* (ldb (byte 1 9) offset) 512)))))
  t)

(defun double-decode (instn data1 data2)
  (log-line "double-op")
  (inc-reg 0)
  (let ((opcode (ldb (byte 4 12) instn))
	(source (ldb (byte 4 8) instn))
	(ad (ldb (byte 1 7) instn))
	(as (ldb (byte 2 4) instn))
	(dest (ldb (byte 4 0) instn))
	(b-w (if (= 1 (ldb (byte 1 6) instn))
		 0
		 8)))
    (multiple-value-bind (source-val source-fun)
	(address-decode as source data1 b-w)
      (multiple-value-bind (dest-val dest-fun)
	  (address-decode ad dest (if (= (ldb (byte 1 0) as) 1)
				      data2    ;use the second extension word if
				      data1)
			  b-w)  ;the first was used in the source
;      (let* ((source-lst (multiple-value-list (address-decode as source data1)))
;	     (source-val (car source-lst)))
	;(log-line source-lst)
	(log-line "source")
	(log-line source-val)
	(log-line "dest")
	(log-line dest-val)
	(cond ((= opcode 4) (mov source-val dest-fun b-w))
	      ((= opcode 5) (add source-val dest-val dest-fun b-w))
	      ((= opcode 6) (addc source-val dest-val dest-fun b-w))
	      ((= opcode 7) (subc source-val dest-val dest-fun b-w))
	      ((= opcode 8) (sub source-val dest-val dest-fun b-w))
	      ((= opcode 9) (cmp source-val dest-val b-w))
	      ((= opcode 10) (dadd source-val dest-val dest-fun b-w))
	      ((= opcode 11) (emu-bit source-val dest-val b-w))
	      ((= opcode 12) (bic source-val dest-val dest-fun b-w))
	      ((= opcode 13) (bis source-val dest-val dest-fun b-w))
	      ((= opcode 14) (xor source-val dest-val dest-fun b-w))
	      ((= opcode 15) (emu-and source-val dest-val dest-fun b-w))
	      (t 'nil))))))

(defun mov (val dest-fun b-w)
  (log-line "mov")
 (log-line  val)
  (log-line dest-fun)
  (log-line b-w)
  (funcall dest-fun val b-w))

(defun add-flags (res b-w)
  (set-c (ldb (byte 1 (+ b-w 8)) res))
  (set-v 0)
  (set-n (negp res b-w))
  (set-z (nullp res b-w)))

(defun add (source-val dest-val dest-fun b-w)
  (log-line "add")
  (log-line "source")
  (log-line source-val)
  (log-line "dest")
  (log-line dest-val)
  (log-line b-w)
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun addc (source-val dest-val dest-fun b-w)
  (log-line "addc")
  (let ((res (+ (trim-bw source-val b-w) (trim-bw dest-val b-w) (get-c))))
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun subc (source-val dest-val dest-fun b-w)
  (log-line "subc")
  (let ((res (+ (trim-bw dest-val b-w) (lognot (trim-bw source-val b-w)) (get-c) 1))) ;random +1 for bug compat
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun sub (source-val dest-val dest-fun b-w)
  (log-line "sub")
  (let ((res (+ (trim-bw dest-val b-w) (trim-bw (lognot source-val) b-w) 1)))
    (add-flags res b-w)
    (funcall dest-fun res b-w)))

(defun cmp (source-val dest-val b-w)
  (log-line "cmp")
  (format t "~4,'0x"  source-val)
  (format t "~4,'0x"  dest-val)
  (log-line b-w)
  (let ((res (+ (trim-bw dest-val b-w) (trim-bw (lognot source-val) b-w) 1)))
    (add-flags res b-w))
  (log-line (get-reg 2)))

(defun dadd (source-val dest-val dest-fun b-w)
  (log-line "dadd")
  (let ((trim-s (trim-bw source-val b-w))
	(trim-d (trim-bw dest-val b-w)))
    (let ((res (loop for i in (list 0 4 8 12)
		  for c = 0 then (if (>= tmp 10)
				     1
				     0)
		  for tmp = (+ (ldb (byte 4 i) trim-s)
			       (ldb (byte 4 i) trim-d)
			       c)
		  for tmp2 = (if (>= tmp 10)
				 (- tmp 10)
				 tmp)
		  for val = tmp2 then (dpb tmp2 (byte 4 i) val)
		  finally (progn (set-c c)
				 (if (ldb (byte 1 3) tmp)
				     (set-n 1))
				 (return val)))))
      (funcall dest-fun res b-w))))

(defun emu-bit (source-val dest-val b-w)
  (log-line "emu-bit")
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res b-w))
    (set-c (nullp res b-w))))

(defun bic (source-val dest-val dest-fun b-w)
  (log-line "bic")
  (let ((res (logand (lognot (trim-bw source-val b-w)) (trim-bw dest-val b-w))))
    (funcall dest-fun res b-w)))

(defun bis (source-val dest-val dest-fun b-w)
  (log-line "bis")
  (funcall dest-fun (logior (trim-bw source-val b-w) (trim-bw dest-val b-w)) b-w))

(defun xor (source-val dest-val dest-fun b-w)
  (log-line "xor")
  (let ((res (logxor (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun res b-w)
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res b-w))
    (set-c (nullp res b-w))))

(defun emu-and (source-val dest-val dest-fun b-w)
  (log-line "emu-and")
  (let ((res (logand (trim-bw source-val b-w) (trim-bw dest-val b-w))))
    (funcall dest-fun res b-w)
    (set-v 0)
    (set-n (ldb (byte 1 (+ b-w 7)) res))
    (set-z (nullp res b-w))
    (set-c (nullp res b-w))))

;start of file importing code

(defun import-file (file)
  (setf *mem* (make-array 65536 :element-type '(unsigned-byte 8)))
  (with-open-file (in file)
    (loop for line = (read-line in nil)
       while line
       do (if (not (equal #\* (elt line 8)))
	      (let* ((split-line (split-sequence #\: line))
		     (offset (hex-string-to-int (car split-line)))
		     (blocks (subseq (split-sequence #\space (cadr split-line)) 3 11)))
		(loop for i from 0 to 7
		   for block = (elt blocks i)
		   do (set-mem (+ offset (* i 2)) (block-val block) 8)))))))

(defun block-val (str)
  (hex-string-to-int
   (concatenate 'string (subseq str 2 4) (subseq str 0 2))))

(defun reset ()
  (setf *reg* (make-array 16 :element-type '(UNSIGNED-BYTE 16)))
  (setf *mem* (make-array 65536 :element-type '(UNSIGNED-BYTE 8)))
  (setf *err* "null")
  (setf *console* "console:")
  (setf *unlocked* nil))

(defun run-level (file-name input)
  (reset)
  (print "start of run")
  (import-file file-name)
  (setf *input* input)
  (main)
  (print file-name)
  (print *console*)
  (print "error:")
  (print *err*)
  (print "unlocked?")
  (print *unlocked*))


add param name/type format options

add param name/type format options

(defpackage :com.orbbec.sdk
  (:use :common-lisp))

(defstruct param type name)
(defstruct code funcname params returntype)

(defun partial (func &rest args1)
   (lambda (&rest args2) (apply func (append args1 args2)))
)

(defun newline ()
	(write-string (format nil "~%"))
)

(defun replace-all (string part replacement &key (test #'char=))
"Returns a new string in which all the occurences of the part
is replaced with replacement."
    (with-output-to-string (out)
      (loop with part-length = (length part)
            for old-pos = 0 then (+ pos part-length)
            for pos = (search part string
                              :start2 old-pos
                              :test test)
            do (write-string string out
                             :start old-pos
                             :end (or pos (length string)))
            when pos do (write-string replacement out)
            while pos)))

(defun formatmethod2 (lineformat funcdata)
	(format nil lineformat
		(code-returntype funcdata)
		(code-funcname funcdata)
		(code-params funcdata)
	)
)

(defun format-paramitem-type (p)
	(format nil "~A" (param-type p))
)

(defun format-paramlist-type (params)
	(format nil "~{~A~^, ~}" (mapcar #'format-paramitem-type params))
)

(defun format-paramitem-name (p)
	(format nil "~A" (param-name p))
)

(defun format-paramlist-name (params)
	(format nil "~{~A~^, ~}" (mapcar #'format-paramitem-name params))
)

(defun format-paramitem-full (p)
	(format nil "~A ~A" (param-type p) (param-name p))
)

(defun format-paramlist-full (params)
	(format nil "~{~A~^, ~}" (mapcar #'format-paramitem-full params))
)

(defun formatmethod (lineformat funcdata)
	(replace-all
		(replace-all
			(replace-all
				(replace-all
					(replace-all lineformat
						"^RETURN^" (code-returntype funcdata)
					)
					"^FUNC^" (code-funcname funcdata)
				)
				"^PARAMS^" (format-paramlist-full (code-params funcdata))
			)
			"^PARAM-NAMES^" (format-paramlist-name (code-params funcdata))
		)
		"^PARAM-TYPES^" (format-paramlist-type (code-params funcdata))
	)
)

(defun formatmethods (lineformat funclist)
	(map 'list
		(partial #'formatmethod lineformat) funclist
	)
)

(setq c1 (make-code :returntype "sk_status"
					:funcname "open_frame"
					:params (list (make-param :type "sk_stream*" :name "stream")
								  (make-param :type "sk_frame**" :name "frame")
							)
		)
)

(setq c2 (make-code :returntype "sk_status"
					:funcname "close_frame"
					:params (list (make-param :type "sk_frame**" :name "frame" ))
		)
)

(setq f1 "^RETURN^ ^FUNC^(^PARAMS^);
")
(setq f2 "^RETURN^ sensekit_^FUNC^(^PARAMS^) {
	return g_Context->^FUNC^(^PARAM-NAMES^);
}
")
(setq f3 "^RETURN^ (*^FUNC^)(void*, ^PARAM-TYPES^);
")

(setq funcs (list c1 c2))

(defun outputfuncs (f)
	(map nil #'write-string f)
)

(defun _ () (load "lpp.cl" :verbose nil))

(defun t ()
	;(write-string (formatmethod f1 c1))
	;(write-string (formatmethod f2 c1))
	(outputfuncs (formatmethods f1 funcs))
	(newline)
	(outputfuncs (formatmethods f2 funcs))
	(newline)
	(outputfuncs (formatmethods f3 funcs))
	(newline)
	nil
)

(t)

(defpackage :com.orbbec.sdk
  (:use :common-lisp))

(defstruct code funcname params returntype)

(defun partial (func &rest args1)
   (lambda (&rest args2) (apply func (append args1 args2)))
)

(defun newline ()
	(write-string (format nil "~%"))
)

(defun replace-all (string part replacement &key (test #'char=))
"Returns a new string in which all the occurences of the part
is replaced with replacement."
    (with-output-to-string (out)
      (loop with part-length = (length part)
            for old-pos = 0 then (+ pos part-length)
            for pos = (search part string
                              :start2 old-pos
                              :test test)
            do (write-string string out
                             :start old-pos
                             :end (or pos (length string)))
            when pos do (write-string replacement out)
            while pos)))

(defun formatmethod2 (lineformat funcdata)
	(format nil lineformat
		(code-returntype funcdata)
		(code-funcname funcdata)
		(code-params funcdata)
	)
)

(defun formatmethod (lineformat funcdata)
	(replace-all
		(replace-all
			(replace-all lineformat
				"^RETURN^" (code-returntype funcdata)
			)
			"^FUNC^" (code-funcname funcdata)
		)
		"^PARAMS^" (code-params funcdata)
	)
)

(defun formatmethods (lineformat funclist)
	(map 'list
		(partial #'formatmethod lineformat) funclist
	)
)

(setq c1 (make-code :returntype "sk_status"
					:funcname "open_frame"
					:params "sk_stream* stream, sk_frame** frame"
		)
)

(setq c2 (make-code :returntype "sk_status"
					:funcname "close_frame"
					:params "sk_frame** frame"
		)
)

(setq f1 "^RETURN^ ^FUNC^(^PARAMS^);
")
(setq f2 "^RETURN^ sensekit_^FUNC^(^PARAMS^) {
	return g_Context->^FUNC^(^PARAMS^);
}
")
(setq f3 "^RETURN^ (*^FUNC^)(void*, ^PARAMS^);
")

(setq funcs (list c1 c2))

(defun outputfuncs (f)
	(map nil #'write-string f)
)

(defun _ () (load "lpp.cl" :verbose nil))

(defun t ()
	;(write-string (formatmethod f1 c1))
	;(write-string (formatmethod f2 c1))
	(outputfuncs (formatmethods f1 funcs))
	(newline)
	(outputfuncs (formatmethods f2 funcs))
	(newline)
	(outputfuncs (formatmethods f3 funcs))
	(newline)
	nil
)

(t)


added routine to detect MSB for all pixels in code block

/*  Copyright 2014 Aaron Boxer

    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 3 of the License, or
    (at your option) any later version.

    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, see <http://www.gnu.org/licenses/>. */

#include "ocl_platform.cl"

CONSTANT sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE  | CLK_FILTER_NEAREST;

void KERNEL run(read_only image2d_t idata, const unsigned int  width, const unsigned int height, const unsigned int  codeblockX, const unsigned int codeblockY, const unsigned int precision) {

    //find max bit plane number
	LOCAL char msb;
	msb = 0;
	int2 posIn = (int2)(getLocalId(0) + getGlobalId(0)*codeblockX, getLocalId(1) + getGlobalId(1)*codeblockY);
	int4 val = read_imagei(idata, sampler, posIn);
	int4 pixelMsb = 32 - clz(val);  // between one and 32
	int currentBit = 32;            // between one and 32

	// wait until all work items have unset msb
	localMemoryFence();

	while(!msb && currentBit) {
	     if (pixelMsb.x == currentBit){
		    msb = currentBit;
		}
		// wait to see if any work item has set msb in this iteration
		localMemoryFence();

        currentBit--;
	}
	//now we know the msb for the x channel of this code block

}

/*  Copyright 2014 Aaron Boxer

    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 3 of the License, or
    (at your option) any later version.

    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, see <http://www.gnu.org/licenses/>. */

#include "ocl_platform.cl"

// simulate OpenCL 2.0 _any functionality
/*
void KERNEL test(void) {
  LOCAL char ControlBit;
  localMemoryFence();
   if(true)
      ControlBit |= 0x1;
   /// ...
   localMemoryFence();
   /// ...
   if(ControlBit) {

	} else {

	}
}
*/

CONSTANT sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE  | CLK_FILTER_NEAREST;

void KERNEL run(read_only image2d_t idata, const unsigned int  width, const unsigned int height, const unsigned int  codeblockX, const unsigned int codeblockY, const unsigned int precision) {

    //find max bit plane number
	LOCAL char foundBitPlane;
	foundBitPlane = 0;

}


[OpenGL] Fix test on PPC after r352540

[OpenGL] Fix test on PPC after r352540

Summary:
Specify -triple like test/SemaOpenCL/logical-ops.cl. Otherwise, this test fails
on PPC.

Reviewers: bkramer

Subscribers: Anastasia, cfe-commits

Differential Revision: https://reviews.llvm.org/D57442

git-svn-id: ffe668792ed300d6c2daa1f6eba2e0aa28d7ec6c@352618 91177308-0d34-0410-b5e6-96231b3b80d8

// RUN: cp %s %t
// RUN: %clang_cc1 -cl-std=CL1.2 -pedantic -Wall -fixit %t -triple x86_64-unknown-linux-gnu
// RUN: %clang_cc1 -cl-std=CL1.2 -fsyntax-only -pedantic -Wall -Werror %t -triple x86_64-unknown-linux-gnu
// RUN: %clang_cc1 -cl-std=CL1.2 -E -o - %t -triple x86_64-unknown-linux-gnu | FileCheck %s

#pragma OPENCL EXTENSION cl_khr_fp64 : enable

typedef __attribute__((ext_vector_type(4))) char char4;
typedef __attribute__((ext_vector_type(4))) short short4;
typedef __attribute__((ext_vector_type(4))) int int4;
typedef __attribute__((ext_vector_type(4))) unsigned int uint4;
typedef __attribute__((ext_vector_type(8))) int int8;
typedef __attribute__((ext_vector_type(4))) long long4;
typedef __attribute__((ext_vector_type(4))) float float4;
typedef __attribute__((ext_vector_type(4))) double double4;

int printf(__constant const char* st, ...) __attribute__((format(printf, 1, 2)));

void vector_fixits() {
  printf("%v4f", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v8d", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v4d", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4f", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4ld", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4hlf", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v8hld", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v4hld", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4hlf", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4hd", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v4hld", (short4) 123);
  // CHECK: printf("%v4hd", (short4) 123);

  printf("%v4ld", (short4) 123);
  // CHECK: printf("%v4hd", (short4) 123);

  printf("%v4hld", (long4) 123);
  // CHECK: printf("%v4ld", (long4) 123);

  printf("%v8f", (float4) 2.0f);
  // CHECK: printf("%v4hlf", (float4) 2.0f);

  printf("%v4f", (float4) 2.0f);
  // CHECK: printf("%v4hlf", (float4) 2.0f);

  printf("%v4lf", (double4) 2.0);
  // CHECK: printf("%v4lf", (double4) 2.0);

  /// FIXME: This should be fixed
  printf("%v4hhd", (int4) 123);
  // CHECK: printf("%v4hhd", (int4) 123);

  /// FIXME: This should be fixed
  printf("%v4hhd", (int8) 123);
  // CHECK: printf("%v4hhd", (int8) 123);
}

// RUN: cp %s %t
// RUN: %clang_cc1 -cl-std=CL1.2 -pedantic -Wall -fixit %t
// RUN: %clang_cc1 -cl-std=CL1.2 -fsyntax-only -pedantic -Wall -Werror %t
// RUN: %clang_cc1 -cl-std=CL1.2 -E -o - %t | FileCheck %s

#pragma OPENCL EXTENSION cl_khr_fp64 : enable

typedef __attribute__((ext_vector_type(4))) char char4;
typedef __attribute__((ext_vector_type(4))) short short4;
typedef __attribute__((ext_vector_type(4))) int int4;
typedef __attribute__((ext_vector_type(4))) unsigned int uint4;
typedef __attribute__((ext_vector_type(8))) int int8;
typedef __attribute__((ext_vector_type(4))) long long4;
typedef __attribute__((ext_vector_type(4))) float float4;
typedef __attribute__((ext_vector_type(4))) double double4;

int printf(__constant const char* st, ...) __attribute__((format(printf, 1, 2)));

void vector_fixits() {
  printf("%v4f", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v8d", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v4d", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4f", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4ld", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4hlf", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v8hld", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v4hld", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4hlf", (int8) 123);
  // CHECK: printf("%v8hld", (int8) 123);

  printf("%v4hd", (int4) 123);
  // CHECK: printf("%v4hld", (int4) 123);

  printf("%v4hld", (short4) 123);
  // CHECK: printf("%v4hd", (short4) 123);

  printf("%v4ld", (short4) 123);
  // CHECK: printf("%v4hd", (short4) 123);

  printf("%v4hld", (long4) 123);
  // CHECK: printf("%v4ld", (long4) 123);

  printf("%v8f", (float4) 2.0f);
  // CHECK: printf("%v4hlf", (float4) 2.0f);

  printf("%v4f", (float4) 2.0f);
  // CHECK: printf("%v4hlf", (float4) 2.0f);

  printf("%v4lf", (double4) 2.0);
  // CHECK: printf("%v4lf", (double4) 2.0);

  /// FIXME: This should be fixed
  printf("%v4hhd", (int4) 123);
  // CHECK: printf("%v4hhd", (int4) 123);

  /// FIXME: This should be fixed
  printf("%v4hhd", (int8) 123);
  // CHECK: printf("%v4hhd", (int8) 123);
}


camerad: fix OX not having vignetting comp (#26329)


use size as a float2 and use fmod instead

__constant sampler_t nearest = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP_TO_EDGE
	| CLK_FILTER_NEAREST;

__constant sampler_t linear = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP_TO_EDGE
	| CLK_FILTER_LINEAR;

//For this simple test we'll just take a vector as the velocity
__kernel void simpleAdvect(float dt, __constant float2 *vel, read_only image2d_t inImg,
	write_only image2d_t outImg)
{
	int2 coord = (int2)(get_global_id(0), get_global_id(1));
	float2 x_n = (float2)(coord.x, coord.y);

	//TODO: Some issue exists with the math that causes +velocity to go much
	//faster than -velocity
	//Perform RK2 to traceback the "start" position of this pixel
	float2 x_nHalf = x_n - 0.5f * dt * (*vel);
	float2 x_nS = x_n - dt * (*vel);
	//Wrap x_nS appropriately. Is there no built in function for this?
	//Or a faster way to do this?
	//get the width and height so we can wrap the coord around if needed
	float2 size = convert_float2(get_image_dim(inImg));
	//Wrap x, note that we wrap when at size and up because the image is 0 indexed
	//so 256 is an out of bounds value that should wrap back to 0
	if (x_nS.x > size.x - 1)
		x_nS.x = fmod(x_nS.x, size.x);
	else if (x_nS.x < 0)
		x_nS.x += size.x;
	//Wrap y
	if (x_nS.y > size.y - 1)
		x_nS.y = fmod(x_nS.y, size.y);
	else if (x_nS.y < 0)
		x_nS.y += size.y;

	//Now sample the value where we "started" and set the next value at x_n to that one
	//We use the linear sampler to get linear interpolation at the traceposition for free
	//This is ok on Nvidia
	float4 tVal = read_imagef(inImg, nearest, x_nS);
	//But why can't I do this on Nvidia? (is it b/c Nvidia has CL1.1?)
	//float4 tVal = (float4)(1.0f, 0.0f, 0.0f, 1.0f);
	write_imagef(outImg, coord, tVal);
}

__constant sampler_t nearest = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP_TO_EDGE
	| CLK_FILTER_NEAREST;

__constant sampler_t linear = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP_TO_EDGE
	| CLK_FILTER_LINEAR;

//For this simple test we'll just take a vector as the velocity
__kernel void simpleAdvect(float dt, __constant float2 *vel, read_only image2d_t inImg,
	write_only image2d_t outImg)
{
	int2 coord = (int2)(get_global_id(0), get_global_id(1));
	float2 x_n = (float2)(coord.x, coord.y);

	//TODO: Some issue exists with the math that causes +velocity to go much
	//faster than -velocity
	//Perform RK2 to traceback the "start" position of this pixel
	float2 x_nHalf = x_n - 0.5f * dt * (*vel);
	float2 x_nS = x_n - dt * (*vel);
	//Wrap x_nS appropriately. Is there no built in function for this?
	//Or a faster way to do this?
	//get the width and height so we can wrap the coord around if needed
	int2 size = get_image_dim(inImg);
	//Wrap x, note that we wrap when at size and up because the image is 0 indexed
	//so 256 is an out of bounds value that should wrap back to 0
	if (x_nS.x > size.x - 1)
		x_nS.x = convert_int(x_nS.x) % size.x;
	else if (x_nS.x < 0)
		x_nS.x += size.x;
	//Wrap y
	if (x_nS.y > size.y - 1)
		x_nS.y = convert_int(x_nS.y) % size.y;
	else if (x_nS.y < 0)
		x_nS.y += size.y;

	//Now sample the value where we "started" and set the next value at x_n to that one
	//We use the linear sampler to get linear interpolation at the traceposition for free
	//This is ok on Nvidia
	float4 tVal = read_imagef(inImg, nearest, x_nS);
	//But why can't I do this on Nvidia? (is it b/c Nvidia has CL1.1?)
	//float4 tVal = (float4)(1.0f, 0.0f, 0.0f, 1.0f);
	write_imagef(outImg, coord, tVal);
}


templatize compareop

templatize compareop

// from lib/THC/THCSortUtils.cuh:

// This needs the following template variables:
//   K              key type
//   V              value type
//   COMPARE_OP     a comparison operator, like <   or >

/*__device__*/ inline void swapVars_K({{K}} *p_t1, {{K}}*p_t2) {
  {{K}} tmp = *p_t1;
  *p_t1 = *p_t2;
  *p_t2 = tmp;
}

/*__device__*/ inline void swapVars_V({{V}} *p_t1, {{V}}*p_t2) {
  {{V}} tmp = *p_t1;
  *p_t1 = *p_t2;
  *p_t2 = tmp;
}

/*__device__*/ inline void swapVars_bool(bool *p_t1, bool *p_t2) {
  bool tmp = *p_t1;
  *p_t1 = *p_t2;
  *p_t2 = tmp;
}

/*__device__*/ inline void bitonicSwap({{K}}* p_kA, V*p_vA, bool*p_validA,
                                   {{K}}* p_kB, V*p_vB, bool*p_validB,
                                   bool dir) {
  // Invalid entries always sort to the end
  bool swap = (kA {{COMPARE_OP}} kB) && validA) || !validB;
  if (swap == dir) {
    swapVars_K(p_kA, p_kB);
    swapVars_V(p_vA, p_vB);
    swapVars_bool(p_validA, p_validB);
  }
};

template <int Power2SortSize>
/*__device__*/ inline void bitonicSort({{K}} keys[Power2SortSize],
                                   {{V}} values[Power2SortSize],
                                   bool valid[Power2SortSize]) {
#pragma unroll
  for (unsigned int size = 2; size < Power2SortSize; size *= 2) {
    bool flag = ((get_local_id(0) & (size / 2)) != 0);

#pragma unroll
    for (unsigned int stride = size / 2; stride > 0; stride /= 2) {

      // Single warp per slice is completely synchronous
      if (Power2SortSize > 64) {
        barrier(CLK_LOCAL_MEM_FENCE);
      }

      unsigned int pos = 2 * get_local_id(0) - (get_local_id(0) & (stride - 1));
      bitonicSwap(
        &keys[pos], &values[pos], &valid[pos],
        &keys[pos + stride], &values[pos + stride], &valid[pos + stride],
        flag);
    }
  }

#pragma unroll
  for (unsigned int stride = Power2SortSize / 2; stride > 0; stride /= 2) {
    // Single warp per slice is completely synchronous
    if (Power2SortSize > 64) {
      barrier(CLK_LOCAL_MEM_FENCE);
    }

    unsigned int pos = 2 * get_local_id(0) - (get_local_id(0) & (stride - 1));
    bitonicSwap(
      &keys[pos], &values[pos], &valid[pos],
      &keys[pos + stride], &values[pos + stride], &valid[pos + stride],
      false);
  }

  // Single warp per slice is completely synchronous
  if (Power2SortSize > 64) {
    barrier(CLK_LOCAL_MEM_FENCE);
  }
}

// Sorts (key, value) pairs (in different tensors) in-place; i.e.,
// modifies the input `keys` and `values`
template <int KeyDims, int ValueDims,
          int Power2SortSize>
kernel void
bitonicSortKVInPlace(TensorInfo<{{IndexType}}> keys,
                     {{IndexType}} keySlices,
                     {{IndexType}} keySliceSize,
                     {{IndexType}} keySliceStride,
                     TensorInfo<{{IndexType}}> values,
                     {{IndexType}} valueSliceStride) {
  // Find the slice of the tensor that we are sorting
  const {{IndexType}} linearIndex = getLinearBlockId<{{IndexType}}>();
  // Tiling the slices could have us be out of bounds, if there are a
  // lot of slices to sort
  if (linearIndex >= keySlices) {
    return;
  }

  local {{K}} sharedKeys[Power2SortSize];
  local {{V}} sharedValues[Power2SortSize];
  local bool sharedValid[Power2SortSize];

  const {{IndexType}} keyStartOffset =
    IndexToOffset<{{IndexType}}, KeyDims>::get(linearIndex, keys);
  const {{IndexType}} valueStartOffset =
    IndexToOffset<{{IndexType}}, ValueDims>::get(linearIndex, values);

  // If the sort size is 1, the data is already sorted
  if (Power2SortSize == 1) {
    return;
  } else {
    // Otherwise, each thread is responsible for loading and storing 2
    // elements. The sort size is guaranteed to be >= 2
    const int elem1 = get_local_id(0);
    const int elem2 = get_local_id(0) + (Power2SortSize / 2);

    bool valid1 = (elem1 < keySliceSize);
    {{K}} k1 = valid1 ?
      keys.data[keyStartOffset + elem1 * keySliceStride] : ({{K}}) 0;
    {{V}} v1 = valid1 ?
      values.data[valueStartOffset + elem1 * valueSliceStride] : ({{V}}) 0;

    sharedKeys[elem1] = k1;
    sharedValues[elem1] = v1;
    sharedValid[elem1] = valid1;

    bool valid2 = (elem2 < keySliceSize);
    {{K}} k2 = valid2 ?
      keys.data[keyStartOffset + elem2 * keySliceStride] : ({{K}}) 0;
    {{V}} v2 = valid2 ?
      values.data[valueStartOffset + elem2 * valueSliceStride] : ({{V}}) 0;

    sharedKeys[elem2] = k2;
    sharedValues[elem2] = v2;
    sharedValid[elem2] = valid2;

    // Sort!
    bitonicSort<Power2SortSize>(
      sharedKeys, sharedValues, sharedValid);

    // elem1 values are always valid, since otherwise we would have
    // chosen the next smallest power-of-2 for sorting
    keys.data[keyStartOffset + elem1 * keySliceStride] =
      sharedKeys[elem1];
    values.data[valueStartOffset + elem1 * valueSliceStride] =
      sharedValues[elem1];

    if (valid2) {
      // elem2 values might be out-of-range, if the data size we are
      // sorting is not a power-of-2
      keys.data[keyStartOffset + elem2 * keySliceStride] =
        sharedKeys[elem2];
      values.data[valueStartOffset + elem2 * valueSliceStride] =
        sharedValues[elem2];
    }
  }
}

// from lib/THC/THCSortUtils.cuh:

/*__device__*/ inline void swapVars_K({{K}} *p_t1, {{K}}*p_t2) {
  {{K}} tmp = *p_t1;
  *p_t1 = *p_t2;
  *p_t2 = tmp;
}

/*__device__*/ inline void swapVars_V({{V}} *p_t1, {{V}}*p_t2) {
  {{V}} tmp = *p_t1;
  *p_t1 = *p_t2;
  *p_t2 = tmp;
}

/*__device__*/ inline void swapVars_bool(bool *p_t1, bool *p_t2) {
  bool tmp = *p_t1;
  *p_t1 = *p_t2;
  *p_t2 = tmp;
}

template <typename Comparator>
/*__device__*/ inline void bitonicSwap({{K}}* p_kA, V*p_vA, bool*p_validA,
                                   {{K}}* p_kB, V*p_vB, bool*p_validB,
                                   bool dir,
                                   const Comparator& comp) {
  // Invalid entries always sort to the end
  bool swap = (comp(kA, kB) && validA) || !validB;
  if (swap == dir) {
    swapVars_K(p_kA, p_kB);
    swapVars_V(p_vA, p_vB);
    swapVars_bool(p_validA, p_validB);
  }
};

template <typename Comparator,
          typename {{IndexType}}, int Power2SortSize>
/*__device__*/ inline void bitonicSort({{K}} keys[Power2SortSize],
                                   {{V}} values[Power2SortSize],
                                   bool valid[Power2SortSize],
                                   const Comparator& comp) {
#pragma unroll
  for (unsigned int size = 2; size < Power2SortSize; size *= 2) {
    bool flag = ((get_local_id(0) & (size / 2)) != 0);

#pragma unroll
    for (unsigned int stride = size / 2; stride > 0; stride /= 2) {

      // Single warp per slice is completely synchronous
      if (Power2SortSize > 64) {
        barrier(CLK_LOCAL_MEM_FENCE);
      }

      unsigned int pos = 2 * get_local_id(0) - (get_local_id(0) & (stride - 1));
      bitonicSwap<Comparator>(
        &keys[pos], &values[pos], &valid[pos],
        &keys[pos + stride], &values[pos + stride], &valid[pos + stride],
        flag, comp);
    }
  }

#pragma unroll
  for (unsigned int stride = Power2SortSize / 2; stride > 0; stride /= 2) {
    // Single warp per slice is completely synchronous
    if (Power2SortSize > 64) {
      barrier(CLK_LOCAL_MEM_FENCE);
    }

    unsigned int pos = 2 * get_local_id(0) - (get_local_id(0) & (stride - 1));
    bitonicSwap<Comparator>(
      &keys[pos], &values[pos], &valid[pos],
      &keys[pos + stride], &values[pos + stride], &valid[pos + stride],
      false, comp);
  }

  // Single warp per slice is completely synchronous
  if (Power2SortSize > 64) {
    barrier(CLK_LOCAL_MEM_FENCE);
  }
}

// Sorts (key, value) pairs (in different tensors) in-place; i.e.,
// modifies the input `keys` and `values`
template <int KeyDims, int ValueDims,
          typename Comparator, typename {{IndexType}}, int Power2SortSize>
kernel void
bitonicSortKVInPlace(TensorInfo<{{IndexType}}> keys,
                     {{IndexType}} keySlices,
                     {{IndexType}} keySliceSize,
                     {{IndexType}} keySliceStride,
                     TensorInfo<{{IndexType}}> values,
                     {{IndexType}} valueSliceStride,
                     const Comparator& comp) {
  // Find the slice of the tensor that we are sorting
  const {{IndexType}} linearIndex = getLinearBlockId<{{IndexType}}>();
  // Tiling the slices could have us be out of bounds, if there are a
  // lot of slices to sort
  if (linearIndex >= keySlices) {
    return;
  }

  local {{K}} sharedKeys[Power2SortSize];
  local {{V}} sharedValues[Power2SortSize];
  local bool sharedValid[Power2SortSize];

  const {{IndexType}} keyStartOffset =
    IndexToOffset<{{IndexType}}, KeyDims>::get(linearIndex, keys);
  const {{IndexType}} valueStartOffset =
    IndexToOffset<{{IndexType}}, ValueDims>::get(linearIndex, values);

  // If the sort size is 1, the data is already sorted
  if (Power2SortSize == 1) {
    return;
  } else {
    // Otherwise, each thread is responsible for loading and storing 2
    // elements. The sort size is guaranteed to be >= 2
    const int elem1 = get_local_id(0);
    const int elem2 = get_local_id(0) + (Power2SortSize / 2);

    bool valid1 = (elem1 < keySliceSize);
    {{K}} k1 = valid1 ?
      keys.data[keyStartOffset + elem1 * keySliceStride] : ({{K}}) 0;
    {{V}} v1 = valid1 ?
      values.data[valueStartOffset + elem1 * valueSliceStride] : ({{V}}) 0;

    sharedKeys[elem1] = k1;
    sharedValues[elem1] = v1;
    sharedValid[elem1] = valid1;

    bool valid2 = (elem2 < keySliceSize);
    {{K}} k2 = valid2 ?
      keys.data[keyStartOffset + elem2 * keySliceStride] : ({{K}}) 0;
    {{V}} v2 = valid2 ?
      values.data[valueStartOffset + elem2 * valueSliceStride] : ({{V}}) 0;

    sharedKeys[elem2] = k2;
    sharedValues[elem2] = v2;
    sharedValid[elem2] = valid2;

    // Sort!
    bitonicSort<Comparator, K, V, {{IndexType}}, Power2SortSize>(
      sharedKeys, sharedValues, sharedValid, comp);

    // elem1 values are always valid, since otherwise we would have
    // chosen the next smallest power-of-2 for sorting
    keys.data[keyStartOffset + elem1 * keySliceStride] =
      sharedKeys[elem1];
    values.data[valueStartOffset + elem1 * valueSliceStride] =
      sharedValues[elem1];

    if (valid2) {
      // elem2 values might be out-of-range, if the data size we are
      // sorting is not a power-of-2
      keys.data[keyStartOffset + elem2 * keySliceStride] =
        sharedKeys[elem2];
      values.data[valueStartOffset + elem2 * valueSliceStride] =
        sharedValues[elem2];
    }
  }
}


use native_exp function in opencl kernel for bilateral

/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

int lIdx(int x, int y,
        int stride1, int stride0)
{
    return (y*stride1 + x*stride0);
}

void load2LocalMem(__local outType *  shrd,
        __global const inType *      in,
        int lx, int ly, int shrdStride,
        int dim0, int dim1,
        int gx, int gy,
        int inStride1, int inStride0)
{
    int gx_  = clamp(gx, 0, dim0-1);
    int gy_  = clamp(gy, 0, dim1-1);
    shrd[ lIdx(lx, ly, shrdStride, 1) ] = (outType)in[ lIdx(gx_, gy_, inStride1, inStride0) ];
}

float gaussian1d(float x, float variance)
{
    return native_exp((x * x) / (-2.f * variance));
}

__kernel
void bilateral(__global outType *        d_dst,
               KParam                    oInfo,
               __global const inType *   d_src,
               KParam                    iInfo,
               __local outType *         localMem,
               __local outType *         gauss2d,
               float sigma_space, float sigma_color,
               int gaussOff, int nBBS0, int nBBS1)
{
    const int radius      = max((int)(sigma_space * 1.5f), 1);
    const int padding     = 2 * radius;
    const int window_size = padding + 1;
    const int shrdLen     = get_local_size(0) + padding;
    const float variance_range = -2.f * sigma_color * sigma_color;
    const float variance_space = -2.f * sigma_space * sigma_space;

    // gfor batch offsets
    unsigned b2 = get_group_id(0) / nBBS0;
    unsigned b3 = get_group_id(1) / nBBS1;
    __global const inType* in = d_src + (b2 * iInfo.strides[2] + b3 * iInfo.strides[3] + iInfo.offset);
    __global outType* out     = d_dst + (b2 * oInfo.strides[2] + b3 * oInfo.strides[3]);

    int lx = get_local_id(0);
    int ly = get_local_id(1);

    const int gx = get_local_size(0) * (get_group_id(0)-b2*nBBS0) + lx;
    const int gy = get_local_size(1) * (get_group_id(1)-b3*nBBS1) + ly;

    // generate gauss2d spatial variance values for block
    if (lx<window_size && ly<window_size) {
        int x = lx - radius;
        int y = ly - radius;
        gauss2d[ly*window_size+lx] = native_exp( ((x*x) + (y*y)) / variance_space);
    }

    int s0 = iInfo.strides[0];
    int s1 = iInfo.strides[1];
    int d0 = iInfo.dims[0];
    int d1 = iInfo.dims[1];
    // pull image to local memory
    for (int b=ly, gy2=gy; b<shrdLen; b+=get_local_size(1), gy2+=get_local_size(1)) {
        // move row_set get_local_size(1) along coloumns
        for (int a=lx, gx2=gx; a<shrdLen; a+=get_local_size(0), gx2+=get_local_size(0)) {
            load2LocalMem(localMem, in, a, b, shrdLen, d0, d1, gx2-radius, gy2-radius, s1, s0);
        }
    }

    barrier(CLK_LOCAL_MEM_FENCE);

    if (gx<iInfo.dims[0] && gy<iInfo.dims[1]) {
        lx += radius;
        ly += radius;
        outType center_color = localMem[ly*shrdLen+lx];
        outType res  = 0;
        outType norm = 0;
#pragma unroll
        for(int wj=0; wj<window_size; ++wj) {
            int joff = (ly+wj-radius)*shrdLen + (lx-radius);
            int goff = wj*window_size;
#pragma unroll
            for(int wi=0; wi<window_size; ++wi) {
                outType tmp_color   = localMem[joff+wi];
                outType gauss_range = gaussian1d(center_color - tmp_color, variance_range);
                outType weight      = gauss2d[goff+wi] * gauss_range;
                norm += weight;
                res  += tmp_color * weight;
            }
        }
        out[gy*oInfo.strides[1] + gx] = res / norm;
    }
}

/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

int lIdx(int x, int y,
        int stride1, int stride0)
{
    return (y*stride1 + x*stride0);
}

void load2LocalMem(__local outType *  shrd,
        __global const inType *      in,
        int lx, int ly, int shrdStride,
        int dim0, int dim1,
        int gx, int gy,
        int inStride1, int inStride0)
{
    int gx_  = clamp(gx, 0, dim0-1);
    int gy_  = clamp(gy, 0, dim1-1);
    shrd[ lIdx(lx, ly, shrdStride, 1) ] = (outType)in[ lIdx(gx_, gy_, inStride1, inStride0) ];
}

float gaussian1d(float x, float variance)
{
    return exp((x * x) / (-2.f * variance));
}

__kernel
void bilateral(__global outType *        d_dst,
               KParam                    oInfo,
               __global const inType *   d_src,
               KParam                    iInfo,
               __local outType *         localMem,
               __local outType *         gauss2d,
               float sigma_space, float sigma_color,
               int gaussOff, int nBBS0, int nBBS1)
{
    const int radius      = max((int)(sigma_space * 1.5f), 1);
    const int padding     = 2 * radius;
    const int window_size = padding + 1;
    const int shrdLen     = get_local_size(0) + padding;
    const float variance_range = -2.f * sigma_color * sigma_color;
    const float variance_space = -2.f * sigma_space * sigma_space;

    // gfor batch offsets
    unsigned b2 = get_group_id(0) / nBBS0;
    unsigned b3 = get_group_id(1) / nBBS1;
    __global const inType* in = d_src + (b2 * iInfo.strides[2] + b3 * iInfo.strides[3] + iInfo.offset);
    __global outType* out     = d_dst + (b2 * oInfo.strides[2] + b3 * oInfo.strides[3]);

    int lx = get_local_id(0);
    int ly = get_local_id(1);

    const int gx = get_local_size(0) * (get_group_id(0)-b2*nBBS0) + lx;
    const int gy = get_local_size(1) * (get_group_id(1)-b3*nBBS1) + ly;

    // generate gauss2d spatial variance values for block
    if (lx<window_size && ly<window_size) {
        int x = lx - radius;
        int y = ly - radius;
        gauss2d[ly*window_size+lx] = exp( ((x*x) + (y*y)) / variance_space);
    }

    int s0 = iInfo.strides[0];
    int s1 = iInfo.strides[1];
    int d0 = iInfo.dims[0];
    int d1 = iInfo.dims[1];
    // pull image to local memory
    for (int b=ly, gy2=gy; b<shrdLen; b+=get_local_size(1), gy2+=get_local_size(1)) {
        // move row_set get_local_size(1) along coloumns
        for (int a=lx, gx2=gx; a<shrdLen; a+=get_local_size(0), gx2+=get_local_size(0)) {
            load2LocalMem(localMem, in, a, b, shrdLen, d0, d1, gx2-radius, gy2-radius, s1, s0);
        }
    }

    barrier(CLK_LOCAL_MEM_FENCE);

    if (gx<iInfo.dims[0] && gy<iInfo.dims[1]) {
        lx += radius;
        ly += radius;
        outType center_color = localMem[ly*shrdLen+lx];
        outType res  = 0;
        outType norm = 0;
#pragma unroll
        for(int wj=0; wj<window_size; ++wj) {
            int joff = (ly+wj-radius)*shrdLen + (lx-radius);
            int goff = wj*window_size;
#pragma unroll
            for(int wi=0; wi<window_size; ++wi) {
                outType tmp_color   = localMem[joff+wi];
                outType gauss_range = gaussian1d(center_color - tmp_color, variance_range);
                outType weight      = gauss2d[goff+wi] * gauss_range;
                norm += weight;
                res  += tmp_color * weight;
            }
        }
        out[gy*oInfo.strides[1] + gx] = res / norm;
    }
}