sse-pack.lisp

(declaim (optimize (speed 3) (safety 0) (space 0) (debug 0) (compilation-speed 0)))

(setf sb-ext:*efficiency-note-cost-threshold* 1)
(setf *block-compile-default* t)
(setf sb-ext:*efficiency-note-limit* 8)
(setf sb-ext:*inline-expansion-limit* 1000)
(sb-int:set-floating-point-modes :traps (list :divide-by-zero))

(declaim (sb-ext:muffle-conditions style-warning))
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; Setting up SSE2 functionality                                             ;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(in-package #:sb-vm)

(deftype %d2 () '(simd-pack double-float))
(deftype %d2+ () '(simd-pack (double-float 0.0d0)))
(deftype %s4 () '(simd-pack single-float))
(deftype %s4+  () '(simd-pack (single-float 0.0)))

(eval-when (:compile-toplevel :load-toplevel :execute)
  (defmacro define-double-binary-vop-operation (name sse-operation cost)
     `(eval-when (:compile-toplevel :load-toplevel :execute)
        (defknown (,name) (%d2 %d2) %d2
            (movable flushable always-translatable)
          :overwrite-fndb-silently t)
        (define-vop (,name)
          (:translate ,name)
          (:policy :fast-safe)
          (:args (a :scs (double-sse-reg))
                 (b :scs (double-sse-reg) :to :save))
          (:arg-types simd-pack-double simd-pack-double)
          (:results (dest :scs (double-sse-reg)))
          (:result-types simd-pack-double)
          (:generator ,cost ;(move dest a)
                      (inst ,sse-operation dest a b)))))

  (defmacro define-single-binary-vop-operation (name sse-operation cost)
     `(eval-when (:compile-toplevel :load-toplevel :execute)
        (defknown (,name) (%s4 %s4) %s4
            (movable flushable always-translatable)
          :overwrite-fndb-silently t)
        (define-vop (,name)
          (:translate ,name)
          (:policy :fast-safe)
          (:args (a :scs (single-sse-reg))
                 (b :scs (single-sse-reg) :to :save))
          (:arg-types simd-pack-single simd-pack-single)
          (:results (dest :scs (single-sse-reg)))
          (:result-types simd-pack-single)
          (:generator ,cost ;(move dest a)
                      (inst ,sse-operation dest a b)))))

  (defmacro define-single-unary-vop-operation (name sse-operation cost)
   `(eval-when (:compile-toplevel :load-toplevel :execute)
      (defknown (,name) (%s4) %s4
          (movable flushable always-translatable)
        :overwrite-fndb-silently t)
      (define-vop (,name)
        (:translate ,name)
        (:policy :fast-safe)
        (:args (a :scs (single-sse-reg)))
        (:arg-types simd-pack-single)
        (:results (dest :scs (single-sse-reg)))
        (:result-types simd-pack-single)
        (:generator ,cost (inst ,sse-operation dest a)))))
  
  (defmacro define-double-unary-vop-operation (name sse-operation cost)
    `(eval-when (:compile-toplevel :load-toplevel :execute)
       (defknown (,name) (%d2) %d2
           (movable flushable always-translatable)
         :overwrite-fndb-silently t)
       (define-vop (,name)
         (:translate ,name)
         (:policy :fast-safe)
         (:args (a :scs (double-sse-reg)))
         (:arg-types simd-pack-double)
         (:results (dest :scs (double-sse-reg)))
         (:result-types simd-pack-double)
         (:generator ,cost (inst ,sse-operation dest a)))))

   (defmacro define-d2s-conv-unary-vop-operation (name sse-operation cost)
   `(eval-when (:compile-toplevel :load-toplevel :execute)
      (defknown (,name) (%d2) %s4
          (movable flushable always-translatable)
        :overwrite-fndb-silently t)
      (define-vop (,name)
        (:translate ,name)
        (:policy :fast-safe)
        (:args (a :scs (double-sse-reg)))
        (:arg-types simd-pack-double)
        (:results (dest :scs (single-sse-reg)))
        (:result-types simd-pack-single)
        (:generator ,cost (inst ,sse-operation dest a)))))

   (defmacro define-s2d-conv-unary-vop-operation (name sse-operation cost)
     `(eval-when (:compile-toplevel :load-toplevel :execute)
	(defknown (,name) (%s4) %d2
            (movable flushable always-translatable)
          :overwrite-fndb-silently t)
	(define-vop (,name)
          (:translate ,name)
          (:policy :fast-safe)
          (:args (a :scs (single-sse-reg)))
          (:arg-types simd-pack-single)
          (:results (dest :scs (double-sse-reg)))
          (:result-types simd-pack-double)
          (:generator ,cost (inst ,sse-operation dest a)))))
  
   (define-double-binary-vop-operation %d2+ vaddpd 1)
   (define-double-binary-vop-operation %d2- vsubpd 1)
   (define-double-binary-vop-operation %d2* vmulpd 1)
   (define-double-binary-vop-operation %d2/ vdivpd 1)
   (define-single-binary-vop-operation %s4+ vaddps 1)
   (define-single-binary-vop-operation %s4- vsubps 1)
   (define-single-binary-vop-operation %s4* vmulps 1)
   (define-single-binary-vop-operation %s4/ vdivps 1)
   (define-double-unary-vop-operation  %d2sqrt vsqrtpd 1)
   (define-single-unary-vop-operation %s4rsqrt vrsqrtps 2)
   (define-d2s-conv-unary-vop-operation %d2pd2ps vcvtpd2ps 2)
   (define-s2d-conv-unary-vop-operation %s4ps2pd vcvtps2pd 2)

  (defknown (%d2rsqrt) (%d2 %d2+ %d2+) %d2
      (movable flushable always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%d2rsqrt)
    (:translate %d2rsqrt)
    (:policy :fast-safe)
    (:args (x :scs (double-sse-reg))
	   (half :scs (double-sse-reg))
	   (threehalfs :scs (double-sse-reg)))    
    (:arg-types simd-pack-double simd-pack-double
		simd-pack-double)
    (:temporary (:sc double-sse-reg) %xmm0)
    (:temporary (:sc double-sse-reg) %xmm1)
    (:temporary (:sc double-sse-reg) %xmm2)
    (:temporary (:sc double-sse-reg) %xmm3)
    (:results (dest :scs (double-sse-reg)))
    (:result-types simd-pack-double)
    (:generator 8 ;; what should be the cost?
		(inst vcvtpd2ps %xmm0 x)
		(inst vrsqrtps %xmm0 %xmm0)
		(inst vcvtps2pd %xmm0 %xmm0)
		(inst vmulpd %xmm2 threehalfs %xmm0)
		(inst vmulpd %xmm3 %xmm0 %xmm0)
		(inst vmulpd %xmm1 x %xmm0)
		(inst vmulpd %xmm0 %xmm1 %xmm3)
		(inst vmulpd %xmm0 half %xmm0)
		(inst vsubpd %xmm0 %xmm2 %xmm0)
		(inst vmulpd %xmm2 threehalfs %xmm0)
		(inst vmulpd %xmm3 %xmm0 %xmm0)
		(inst vmulpd %xmm1 x %xmm0)
		(inst vmulpd %xmm0 %xmm1 %xmm3)
		(inst vmulpd %xmm0 half %xmm0)
		(inst vsubpd dest %xmm2 %xmm0)))
  
  (defknown (%d2rec) (%d2 %d2+) %d2
      (movable flushable always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%d2rec)
    (:translate %d2rec)
    (:policy :fast-safe)
    (:args (x :scs (double-sse-reg))
	   (two :scs (double-sse-reg)))    
    (:arg-types simd-pack-double simd-pack-double)
    (:temporary (:sc double-sse-reg) %xmm0)
    (:temporary (:sc double-sse-reg) %xmm1)
    (:temporary (:sc double-sse-reg) %xmm2)
    (:results (dest :scs (double-sse-reg)))
    (:result-types simd-pack-double)
    (:generator 8 ;; what should be the cost?
                (inst vmovapd %xmm0 x)
		(inst vcvtpd2ps %xmm0 %xmm0)
		(inst vrcpps %xmm0 %xmm0)
		(inst vcvtps2pd %xmm0 %xmm0)
		(inst vmulpd %xmm2 two %xmm0)
		(inst vmulpd %xmm0 %xmm0 %xmm0)
		(inst vmulpd %xmm1 x %xmm0)
		(inst vsubpd %xmm0 %xmm2 %xmm1)
		(inst vmulpd %xmm2 two %xmm0)
		(inst vmulpd %xmm0 %xmm0 %xmm0)
		(inst vmulpd %xmm1 x %xmm0)
		(inst vsubpd dest %xmm2 %xmm1)))

  (defknown (%d2hsum) (%d2) %d2
      (movable flushable always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%d2hsum)
    (:translate %d2hsum)
    (:policy :fast-safe)
    (:args (x :scs (double-sse-reg)))
    (:arg-types simd-pack-double)
    (:temporary (:sc double-sse-reg) %xmm0)
    (:temporary (:sc double-sse-reg) %xmm1)
    (:results (dest :scs (double-sse-reg)))
    (:result-types simd-pack-double)
    (:generator 4 ;; what should be the cost?
		(inst vmovapd %xmm0 x)
		(inst vextractf128 %xmm1 x 1)
		(inst vaddpd %xmm0 %xmm0 %xmm1)
		(inst vunpckhpd %xmm1 %xmm0 %xmm0)
		(inst vaddsd dest %xmm0 %xmm1)))

  (defknown (%d2hsum2) (%d2) %d2
      (movable flushable always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%d2hsum2)
    (:translate %d2hsum2)
    (:policy :fast-safe)
    (:args (x :scs (double-sse-reg)))
    (:arg-types simd-pack-double)
    (:temporary (:sc double-sse-reg) %xmm0)
    (:temporary (:sc double-sse-reg) %xmm1)
    (:results (dest :scs (double-sse-reg)))
    (:result-types simd-pack-double)
    (:generator 4 ;; what should be the cost?
		(inst vmovapd %xmm1 x)
		(inst vmovhlps %xmm1 %xmm1 %xmm0)
		(inst vaddsd dest %xmm0 %xmm1)
		(inst vzeroupper)))

  (defknown (%s4hsum) (%s4) %s4
      (movable flushable always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%s4hsum)
    (:translate %s4hsum)
    (:policy :fast-safe)
    (:args (%xmm0 :scs (single-sse-reg) :target dest))
    (:arg-types simd-pack-single)
    (:temporary (:sc single-sse-reg) %xmm1)
    (:results (dest :scs (single-sse-reg)))
    (:result-types simd-pack-single)
    (:generator 4 ;; what should be the cost?
		(inst movshdup %xmm1 %xmm0)
		(inst addps %xmm0 %xmm1)
		(inst movhlps %xmm1 %xmm0)
		(inst addss dest %xmm1)))


  (defknown %s4ref ((simple-array single-float (*))
                    (integer 0 #.most-positive-fixnum)) %s4
      (movable foldable flushable always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%s4ref)
    (:translate %s4ref)
    (:args (v :scs (descriptor-reg))
           (i :scs (any-reg)))
    (:arg-types simple-array-single-float
                tagged-num)
    (:results (dest :scs (single-sse-reg)))
    (:result-types simd-pack-single)
    (:policy :fast-safe)
    (:generator 4 (inst vmovaps dest
			(float-ref-ea v i 0 8
				      :scale (ash 8 (- n-fixnum-tag-bits))))))

  (defknown %d2ref ((simple-array double-float (*))
                    (integer 0 #.most-positive-fixnum)) %d2
      (movable foldable flushable always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%d2ref)
    (:translate %d2ref)
    (:args (v :scs (descriptor-reg))
           (i :scs (any-reg)))
    (:arg-types simple-array-double-float
                tagged-num)
    (:results (dest :scs (double-sse-reg)))
    (:result-types simd-pack-double)
    (:policy :fast-safe)
    (:generator 4 (inst vmovapd dest
			(float-ref-ea v i 0 8
                                      :scale (ash 8 (- n-fixnum-tag-bits))))))

  (defknown %s4set ((simple-array single-float (*))
                    (integer 0 #.most-positive-fixnum)
                    %s4) %s4
      (always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%s4set)
    (:translate %s4set)
    (:args (v :scs (descriptor-reg))
           (i :scs (any-reg))
           (x :scs (single-sse-reg) :target dest))
    (:arg-types simple-array-single-float
                tagged-num
                simd-pack-single)
    (:results (dest :scs (single-sse-reg) :from (:argument 2)))
    (:result-types simd-pack-single)
    (:policy :fast-safe)
    (:generator 4 (inst vmovaps (float-ref-ea v i 0 8
	      	      :scale (ash 8 (- n-fixnum-tag-bits))) x)))

  (defknown %d2set ((simple-array double-float (*))
                    (integer 0 #.most-positive-fixnum)
                    %d2) %d2
      (always-translatable)
    :overwrite-fndb-silently t)
  (define-vop (%d2set)
    (:translate %d2set)
    (:args (v :scs (descriptor-reg))
           (i :scs (any-reg))
           (x :scs (double-sse-reg) :target dest))
    (:arg-types simple-array-double-float
                tagged-num
                simd-pack-double)
    (:results (dest :scs (double-sse-reg) :from (:argument 2)))
    (:result-types simd-pack-double)
    (:policy :fast-safe)
    (:generator 4 (inst vmovapd (float-ref-ea v i 0 8
			:scale (ash 8 (- n-fixnum-tag-bits))) x)))

  (declaim (ftype (function (double-float double-float)
			    (simd-pack double-float)) %make-sse-double))
  (declaim (inline %make-sse-double))
  (defun %make-sse-double (a b)
    (declare (optimize (speed 3) (safety 0) (debug 0))
	     (type double-float a b))
    (sb-vm::%make-simd-pack-double a b))
  
  (declaim (ftype (function (single-float single-float
			     single-float single-float)
			    (simd-pack single-float)) %make-sse-single))
  (declaim (inline %make-sse-single))
  (defun %make-sse-single (a b c d)
    (declare (optimize (speed 3) (safety 0) (debug 0))
	     (type single-float a b c d))
    (sb-vm::%make-simd-pack-single a b c d))
)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(in-package #:cl-user)

(deftype %d2  () '(simd-pack double-float))
(deftype %d2+ () '(simd-pack (double-float 0.0d0)))
(deftype %s4  () '(simd-pack single-float))
(deftype %s4+  () '(simd-pack (single-float 0.0)))

(eval-when (:compile-toplevel :load-toplevel :execute)
  ;; (declaim (inline make-sse-double))
  ;; (defun make-sse-double (a &optional (b a))
  ;;   (declare (optimize (speed 3) (safety 0) (debug 0)))
  ;;   (cond ((typep a 'double-float) (sb-vm::%make-sse-double a b))
  ;; 	  ((typep a 'single-float) (sb-vm::%make-sse-double
  ;; 				    (coerce a 'double-float)
  ;; 				    (coerce b 'double-float)))
  ;; 	  ((typep a 'integer) (sb-vm::%make-sse-double
  ;;                              (coerce a 'double-float)
  ;;                              (coerce b 'double-float)))))

  (declaim (inline make-sse-double))
  (defun make-sse-double (a &optional (b a))
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((typep a 'double-float) (sb-vm::%make-sse-double a b))
	  ((typep a 'single-float) (sb-vm::%make-sse-double
				    (float a 0d0) (float b 0d0)))
	  ((typep a 'integer     ) (sb-vm::%make-sse-double
				    (float a 0d0) (float b 0d0)))))

  (declaim (inline make-sse-single))
  (defun make-sse-single (a &optional (b a) (c a) (d a))
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((typep a 'single-float) (sb-vm::%make-sse-single a b c d))
	  ((typep a 'double-float) (sb-vm::%make-sse-single
				    (float a 0s0) (float b 0s0)
				    (float c 0s0) (float d 0s0)))
	  ((typep a 'integer     ) (sb-vm::%make-sse-single
				    (float a 0s0) (float b 0s0)
				    (float c 0s0) (float d 0s0)))))

  (defparameter %d2-zero% (make-sse-double 0.0d0))
  (defparameter %s4-zero% (make-sse-single 0.0))
  (defparameter %0.0d2    (make-sse-double 0.0d0))
  (defparameter %0.0s4    (make-sse-single 0.0))
  (defparameter %d2-one%  (make-sse-double 1.0d0))
  (defparameter %1.0d2    (make-sse-double 1.0d0))
  (defparameter %1.0s4    (make-sse-single 1.0))
  
  (defmacro define-double-binary-operation (name sse-operation)
    `(eval-when (:compile-toplevel :load-toplevel :execute)
       (declaim (inline ,name))
       (declaim (ftype (function (%d2 %d2) %d2) ,name))
       (defun ,name (x y)
	 (declare (optimize (speed 3) (safety 0) (debug 0))
		  (type %d2 x y))
	 (,sse-operation x y))))
  
  (defmacro define-single-binary-operation (name sse-operation)
    `(eval-when (:compile-toplevel :load-toplevel :execute)
       (declaim (inline ,name))
       (declaim (ftype (function (%s4 %s4) %s4) ,name))
       (defun ,name (x y)
	 (declare (optimize (speed 3) (safety 0) (debug 0))
		  (type %s4 x y))
	 (,sse-operation x y))))

  (declaim (inline s4+))
  (declaim (ftype (function (&rest (simd-pack single-float))
			    (simd-pack single-float)) s4+))
  (defun s4+ (&rest args)
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((null args) %0.0s4)
          ((null (cdr args)) (car args))
          ((null (cddr args)) (sb-vm::%s4+ (car args) (cadr args)))
          (t (sb-vm::%s4+ (car args) (apply #'s4+ (cdr args))))))
  (define-compiler-macro s4+ (&whole whole &rest args &environment env)
  (if (> (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'speed)
         (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'space))
      (let ((args (case (car whole)
		    (apply (nconc (butlast (cdr args)) (car (last (cdr args)))))
                    (funcall args)
                    (t args))))
        (cond ((null args) %0.0s4)
              ((null (cdr args)) (car args))
              ((null (cddr args)) `(sb-vm::%s4+ ,(car args) ,(cadr args)))
              (t `(sb-vm::%s4+ ,(car args)
			       ,(funcall (compiler-macro-function 's4+)
			       `(funcall #'s4+ ,@(cdr args)) env))))) whole))

  (declaim (inline d2+))
  (declaim (ftype (function (&rest (simd-pack double-float))
			    (simd-pack double-float)) d2+))
  (defun d2+ (&rest args)
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((null args) %0.0d2)
          ((null (cdr args)) (car args))
          ((null (cddr args)) (sb-vm::%d2+ (car args) (cadr args)))
          (t (sb-vm::%d2+ (car args) (apply #'d2+ (cdr args))))))
  (define-compiler-macro d2+ (&whole whole &rest args &environment env)
  (if (> (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'speed)
         (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'space))
      (let ((args (case (car whole)
		    (apply (nconc (butlast (cdr args)) (car (last (cdr args)))))
                    (funcall args)
                    (t args))))
        (cond ((null args) %0.0d2)
              ((null (cdr args)) (car args))
              ((null (cddr args)) `(sb-vm::%d2+ ,(car args) ,(cadr args)))
              (t `(sb-vm::%d2+ ,(car args) ,(funcall (compiler-macro-function 'd2+)
		    `(funcall #'d2+ ,@(cdr args)) env))))) whole))

  (declaim (inline s4*))
  (declaim (ftype (function (&rest (simd-pack single-float))
			    (simd-pack single-float)) s4*))
  (defun s4* (&rest args)
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((null args) %1.0s4)
          ((null (cdr args)) (car args))
          ((null (cddr args)) (sb-vm::%s4* (car args) (cadr args)))
          (t (sb-vm::%s4* (car args) (apply 's4* (cdr args))))))
  (define-compiler-macro s4* (&whole whole &rest args &environment env)
    (if (> (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'speed)
           (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'space))
	(let ((args (case (car whole)
                      (apply (nconc (butlast (cdr args))
				    (car (last (cdr args)))))
                      (funcall args)
                      (t args))))
          (cond ((null args) %1.0s4)
		((null (cdr args)) (car args))
		((null (cddr args)) `(sb-vm::%s4* ,(car args) ,(cadr args)))
		(t `(sb-vm::%s4* ,(car args)
				 ,(funcall (compiler-macro-function 's4*)
				 `(funcall #'s4* ,@(cdr args)) env))))) whole))
  
  (declaim (inline d2*))
  (declaim (ftype (function (&rest (simd-pack double-float))
			    (simd-pack double-float)) d2*))
  (defun d2* (&rest args)
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((null args) %1.0d2)
          ((null (cdr args)) (car args))
          ((null (cddr args)) (sb-vm::%d2* (car args) (cadr args)))
          (t (sb-vm::%d2* (car args) (apply 'd2* (cdr args))))))
  (define-compiler-macro d2* (&whole whole &rest args &environment env)
    (if (> (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'speed)
           (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'space))
	(let ((args (case (car whole)
                      (apply (nconc (butlast (cdr args)) (car (last (cdr args)))))
                      (funcall args)
                      (t args))))
          (cond ((null args) %1.0d2)
		((null (cdr args)) (car args))
		((null (cddr args)) `(sb-vm::%d2* ,(car args) ,(cadr args)))
		(t `(sb-vm::%d2* ,(car args) ,(funcall (compiler-macro-function 'd2*)
		      `(funcall #'d2* ,@(cdr args)) env))))) whole))

  (declaim (inline s4/))
  (declaim (ftype (function (&rest (simd-pack single-float))
			    (simd-pack single-float)) s4/))
  (defun s4/ (arg &rest args)
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((null args) (sb-vm::%s4/ %1.0s4 arg))
          ((null (cdr args)) (sb-vm::%s4/ arg (car args)))
          (t (apply #'s4/ (sb-vm::%s4/ arg (car args)) (cdr args)))))
  (define-compiler-macro s4/ (&whole whole arg &rest args &environment env)
    (if (> (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'speed)
           (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'space))
	(cond ((null args) `(sb-vm::%s4/ ,%1.0s4 ,arg))
              ((null (cdr args)) `(sb-vm::%s4/ ,arg ,(car args)))
              (t (funcall (compiler-macro-function 's4/)
		   `(funcall #'s4/ (sb-vm::%s4/ ,arg ,(car args)) ,@(cdr args))
		   env))) whole))
  
  (declaim (inline d2/))
  (declaim (ftype (function (&rest (simd-pack double-float))
			    (simd-pack double-float)) d2/))
  (defun d2/ (arg &rest args)
    (declare (optimize (speed 3) (safety 0) (debug 0)))
    (cond ((null args) (sb-vm::%d2/ %1.0d2 arg))
          ((null (cdr args)) (sb-vm::%d2/ arg (car args)))
          (t (apply #'d2/ (sb-vm::%d2/ arg (car args)) (cdr args)))))
  (define-compiler-macro d2/ (&whole whole arg &rest args &environment env)
    (if (> (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'speed)
           (sb-c::policy-quality (slot-value env 'sb-c::%policy) 'space))
	(cond ((null args) `(sb-vm::%d2/ ,%1.0d2 ,arg))
              ((null (cdr args)) `(sb-vm::%d2/ ,arg ,(car args)))
              (t (funcall (compiler-macro-function 'd2/)
		   `(funcall #'d2/ (sb-vm::%d2/ ,arg ,(car args)) ,@(cdr args))
			  env))) whole))

  (defmacro define-double-unary-operation (name sse-operation)
    `(eval-when (:compile-toplevel :load-toplevel :execute)
       (declaim (inline ,name))
       (declaim (ftype (function (%d2) %d2) ,name))
       (defun ,name (x)
	 (declare (optimize (speed 3) (safety 0) (debug 0))
		  (type %d2 x))
	 (,sse-operation x))))

     (defmacro define-single-unary-operation (name sse-operation)
     `(eval-when (:compile-toplevel :load-toplevel :execute)
	(declaim (inline ,name))
	(declaim (ftype (function (%s4) %s4) ,name))
	(defun ,name (x)
	  (declare (optimize (speed 3) (safety 0) (debug 0))
		   (type %s4 x))
	  (,sse-operation x))))

   (defmacro define-s2d-conv-unary-operation (name sse-operation)
     `(eval-when (:compile-toplevel :load-toplevel :execute)
	(declaim (inline ,name))
	(declaim (ftype (function (%s4) %d2) ,name))
	(defun ,name (x)
	  (declare (optimize (speed 3) (safety 0) (debug 0))
		   (type %s4 x))
	  (,sse-operation x))))

   (defmacro define-d2s-conv-unary-operation (name sse-operation)
     `(eval-when (:compile-toplevel :load-toplevel :execute)
	(declaim (inline ,name))
	(declaim (ftype (function (%d2) %s4) ,name))
	(defun ,name (x)
	  (declare (optimize (speed 3) (safety 0) (debug 0))
		   (type %d2 x))
	  (,sse-operation x))))
   (define-double-unary-operation d2sqrt sb-vm::%d2sqrt)
   (define-single-unary-operation s4rsqrt sb-vm::%s4rsqrt)
   (define-d2s-conv-unary-operation d2pd2ps sb-vm::%d2pd2ps)
   (define-s2d-conv-unary-operation s4ps2pd sb-vm::%s4ps2pd)
)

(declaim (inline d2ref))
(declaim (ftype (function ((simple-array double-float (*))
			   (integer 0 #.most-positive-fixnum))
			  %d2) d2ref)) 
(defun d2ref (v i)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type (simple-array double-float (*)) v)
	   (type (integer 0 #.most-positive-fixnum) i))
  (sb-vm::%d2ref v i))

(declaim (inline s4ref))
(declaim (ftype (function ((simple-array single-float (*))
			   (integer 0 #.most-positive-fixnum))
			  %s4) s4ref)) 
(defun s4ref (v i)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type (simple-array single-float (*)) v)
	   (type (integer 0 #.most-positive-fixnum) i))
  (sb-vm::%s4ref v i))

(declaim (inline (setf d2ref)))
(declaim (ftype (function (%d2 (simple-array double-float (*))
			       (integer 0 #.most-positive-fixnum))
			  %d2) (setf d2ref)))
(defun (setf d2ref) (new-value v i)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type (simple-array double-float (*)) v)
	   (type (integer 0 #.most-positive-fixnum) i)
	   (type %d2 new-value))
  (sb-vm::%d2set v i new-value))

(declaim (inline (setf s4ref)))
(declaim (ftype (function (%s4 (simple-array single-float (*))
			       (integer 0 #.most-positive-fixnum))
			  %s4) (setf s4ref)))
(defun (setf s4ref) (new-value v i)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type (simple-array single-float (*)) v)
	   (type (integer 0 #.most-positive-fixnum) i)
	   (type %s4 new-value))
  (sb-vm::%s4set v i new-value))

(eval-when (:compile-toplevel :load-toplevel :execute)
  (defmacro define-constant (name value &optional doc)
  `(defconstant ,name (if (boundp ',name) (symbol-value ',name) ,value)
     ,@(when doc (list doc))))

(define-constant %0.0d2c (make-sse-double 0.0d0))
(define-constant %0.5d2c (make-sse-double 0.5d0))
(define-constant %1.0d2c (make-sse-double 1.0d0))
(define-constant %1.5d2c (make-sse-double 1.5d0))
(define-constant %2.0d2c (make-sse-double 2.0d0))
(define-constant %0.0s4c (make-sse-single 0.0))
(define-constant %0.5s4c (make-sse-single 0.5))
(define-constant %1.0s4c (make-sse-single 1.0))
(define-constant %1.5s4c (make-sse-single 1.5))
(define-constant %2.0s4c (make-sse-single 2.0)))

(declaim (inline d2rsqrt))
(declaim (ftype (function (%d2+) %d2+) d2rsqrt))
(defun d2rsqrt (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type %d2+ %x))
  (sb-vm::%d2rsqrt %x %0.5d2c %1.5d2c))

(declaim (inline d2rsqrt1))
(declaim (ftype (function (%d2+) %d2+) d2rsqrt1))
(defun d2rsqrt1 (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0)))
  (declare (type %d2+ %x))
  (let* ((%y (s2ps2pd (s2rsqrt (d2pd2ps %x))))
  	 (%0.5x (d2* %x %0.5d2c)))
    (declare (type %d2+ %y %0.5x))
    (setq %y (d2* %y (d2- %1.5d2c (d2* %0.5x (d2* %y %y)))))
    (setq %y (d2* %y (d2- %1.5d2c (d2* %0.5x (d2* %y %y)))))))

(declaim (inline d2rec))
(declaim (ftype (function (%d2) %d2) d2rec))
(defun d2rec (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type %d2 %x))
  (sb-vm::%d2rec %x %2.0d2c))

(declaim (inline sse-doubles))
(defun sse-doubles (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type %d2 %x))
  (sb-vm::%simd-pack-doubles %x))

(declaim (inline %sse-double-low))
(declaim (ftype (function (%d2) double-float) sse-double-low))
(defun sse-double-low (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type %d2 %x))
  (sb-vm::%simd-pack-double-item %x 1))

(declaim (inline sse-double-high))
(declaim (ftype (function (%d2) double-float) sse-double-high))
(defun sse-double-high (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type %d2 %x))
  (sb-vm::%simd-pack-double-item %x 0))

(declaim (inline sse-single-high))
(declaim (ftype (function (%s4) single-float) sse-single-high))
(defun sse-single-high (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type %s4 %x))
  (sb-vm::%simd-pack-single-item %x 0))

(declaim (inline sse-double-zero))
(declaim (ftype (function () %d2) sse-double-zero))
(defun sse-double-zero ()
  (declare (optimize (speed 3) (safety 0) (debug 0)))
  (make-sse-double 0.0d0))

(define-modify-macro d2incf (&optional (num %1.0d2)) d2+)
(define-modify-macro d2decf (&optional (num %1.0d2)) d2-)
(define-modify-macro s4incf (&optional (num %1.0s4)) s4+)
(define-modify-macro s4decf (&optional (num %1.0s4)) s4-)

(declaim (inline d2hsum))
(declaim (ftype (function (%d2) double-float) d2hsum))
(defun d2hsum (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0)))
  (sb-vm::%simd-pack-double-item (sb-vm::%d2hsum %x) 0))

(declaim (inline s4hsum))
(declaim (ftype (function (%s4) single-float) s4hsum))
(defun s4hsum (%x)
  (declare (optimize (speed 3) (safety 0) (debug 0)))
  (sb-vm::%simd-pack-single-item (sb-vm::%s4hsum %x) 0))

(declaim (inline d2dot))
(declaim (ftype (function (%d2 %d2) double-float) d2dot))
(defun d2dot (%x %y)
  (declare (optimize (speed 3) (safety 0) (debug 0))
	   (type %d2 %x %y))
  (d2hsum (d2* %x %y)))

(declaim (inline d2vdot))
(declaim (ftype (function ((simple-array double-float (*))
                           (simple-array double-float (*)))
                          double-float) d2vdot))
(defun d2vdot (u v)
  (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
	   (type (simple-array double-float (*)) u v))
  (let* ((n  (min (length u) (length v)))
         (n0 (- n (mod n 2)))) 
    (declare (type fixnum n n0))
    (loop with %sum1 of-type %d2 = %0.0d2c
          for i of-type fixnum below n0 by 2
	  do (progn (d2incf %sum1 (d2* (d2ref u i) (d2ref v i))))
	  finally (return (+ (d2hsum %sum1)
			     (loop for i of-type fixnum from n0 below n
				   summing (* (aref u i) (aref v i))
				     into sum2 of-type double-float
				   finally (return sum2)))))))

  (declaim (inline vdot))
  (declaim (ftype (function ((simple-array double-float (*))
                             (simple-array double-float (*)))
                            double-float) vdot))
  (defun vdot (u v)
    (declare (optimize (speed 3) (safety 0) (debug 0) (space 0))
	     (type (simple-array double-float (*)) u v))
    (let* ((n  (min (length u) (length v)))
	   (n0 (- n (mod n 3))))
      (declare (type fixnum n n0))
      (loop with sum1 of-type double-float = 0.0d0
	    with sum2 of-type double-float = 0.0d0
	    with sum3 of-type double-float = 0.0d0 ;; Gets slower with sum4!
            for i of-type fixnum below n0 by 3
	    do (progn (incf sum1 (* (aref u i) (aref v i)))
		      (incf sum2 (* (aref u (1+ i)) (aref v (1+ i))))
		      (incf sum3 (* (aref u (+ 2 i)) (aref v (+ 2 i)))))
	    finally (return (+ sum1 sum2 sum3
			       (loop for i of-type fixnum from n0 below n
				     summing (* (aref u i) (aref v i))
				       into sum4 of-type double-float
				     finally (return sum4)))))))

(provide :SSE)