         /* CUBLAS(nrm2)(cublasHandle, n, r, 1, &nrm_r); */
         CUBLAS(dot)(cublasHandle, n, r, 1, r, 1, &nrm_r);
         nrm_r = SQRTF(nrm_r);
-		if (nrm_r < tol) {
-			i++;
+        if (nrm_r < tol) {
+            i++;
             break;
-		}
+        }
         beta = (nrm_r * nrm_r) / (nrm_r_old * nrm_r_old);
-		CUBLAS(scal)(cublasHandle, n, &beta, p, 1);
+        CUBLAS(scal)(cublasHandle, n, &beta, p, 1);
         CUBLAS(axpy)(cublasHandle, n, &onef, r, 1, p, 1);
-	}
-    #if EXTRAVERBOSE > 0
-	    scs_printf("tol: %.4e, resid: %.4e, iters: %li\n", tol, nrm_r, (long) i+1);
-    #endif
-    cudaMemcpy(bh, bg, n * sizeof(scs_float), cudaMemcpyDeviceToHost);
+    }
+#if EXTRAVERBOSE > 0
+    scs_printf("tol: %.4e, resid: %.4e, iters: %li\n", tol, nrm_r, (long) i+1);
+#endif
     return i;
 }
 
+#ifdef TEST_GPU_MAT_MUL
 void accumByAtransHost(const AMatrix * A, Priv * p, const scs_float *x, scs_float *y) {
-	_accumByAtrans(A->n, A->x, A->i, A->p, x, y);
+    _accumByAtrans(A->n, A->x, A->i, A->p, x, y);
 }
 
 void accumByAHost(const AMatrix * A, Priv * p, const scs_float *x, scs_float *y) {
-	_accumByA(A->n, A->x, A->i, A->p, x, y);
-}
+    _accumByA(A->n, A->x, A->i, A->p, x, y);
+}
+
+void testGpuMatMul(const AMatrix * A, Priv * p, scs_float * b) {
+    /* test to see if matrix multiplication codes agree */
+    scs_float t[A->n + A->m], u[A->n + A->m], *bg;
+    cudaMalloc((void **)&bg, (A->n + A->m) * sizeof(scs_float));
+
+    cudaMemcpy(bg, b, (A->n + A->m) * sizeof(scs_float), cudaMemcpyHostToDevice);
+    memcpy(t, b, (A->n + A->m) * sizeof(scs_float));
+
+    accumByAtransGpu(p, &(bg[A->n]), bg);
+    accumByAtransHost(A, p, &(t[A->n]), t);
+    cudaMemcpy(u, bg, (A->n + A->m) * sizeof(scs_float), cudaMemcpyDeviceToHost);
+    printf("A trans multiplication err %2.e\n", calcNormDiff(u, t, A->n));
+
+    accumByAGpu(p, bg, &(bg[A->n]));
+    accumByAHost(A, p, t, &(t[A->n]));
+    cudaMemcpy(u, bg, (A->n + A->m) * sizeof(scs_float), cudaMemcpyDeviceToHost);
+    printf("A multiplcation err %2.e\n", calcNormDiff(&(u[A->n]), &(t[A->n]), A->m));
+    cudaFree(bg);
+}
+#endif
 
 scs_int solveLinSys(const AMatrix * A, const Settings * stgs, Priv * p, scs_float * b, const scs_float * s, scs_int iter) {
-	scs_int cgIts;
-	scs_float cgTol = calcNorm(b, A->n) * (iter < 0 ? CG_BEST_TOL : CG_MIN_TOL / POWF((scs_float) iter + 1, stgs->cg_rate));
-
-	tic(&linsysTimer);
-	/* solves Mx = b, for x but stores result in b */
-	/* s contains warm-start (if available) */
+    scs_int cgIts;
+    scs_float * bg = p->bg;
+    scs_float negOnef = -1.0;
+    scs_float cgTol = calcNorm(b, A->n) * (iter < 0 ? CG_BEST_TOL : CG_MIN_TOL / POWF((scs_float) iter + 1, stgs->cg_rate));
+
+    tic(&linsysTimer);
+    /* solves Mx = b, for x but stores result in b */
+    /* s contains warm-start (if available) */
 
 #ifdef TEST_GPU_MAT_MUL
-    /* test to see if matrix multiplication codes agree */
-    scs_float tn[A->n], tm[A->m];
-    memcpy(tn, b, A->n * sizeof(scs_float));
-    memcpy(tm, &(b[A->n]), A->m * sizeof(scs_float));
-#endif
-
-    accumByAtrans(A, p, &(b[A->n]), b);
-
-#ifdef TEST_GPU_MAT_MUL
-    accumByAtransHost(A, p, tm, tn);
-    printf("A trans multiplication err %2.e\n", calcNormDiff(b, tn, A->n));
-#endif
-
+    testGpuMatMul(A, p, b);
+#endif
+
+    /* all on GPU */
+    cudaMemcpy(bg, b, (A->n + A->m) * sizeof(scs_float), cudaMemcpyHostToDevice);
+    accumByAtransGpu(p, &(bg[A->n]), bg);
     /* solves (I+A'A)x = b, s warm start, solution stored in b */
-	cgIts = pcg(A, stgs, p, s, b, A->n, MAX(cgTol, CG_BEST_TOL));
-	scaleArray(&(b[A->n]), -1, A->m);
-
-#ifdef TEST_GPU_MAT_MUL
-    memcpy(tn, b, A->n * sizeof(scs_float));
-    memcpy(tm, &(b[A->n]), A->m * sizeof(scs_float));
-#endif
-
-    accumByA(A, p, b, &(b[A->n]));
-
-#ifdef TEST_GPU_MAT_MUL
-    accumByAHost(A, p, tn, tm);
-    printf("A multiplcation err %2.e\n", calcNormDiff(&(b[A->n]), tm, A->m));
-#endif
+    cgIts = pcg(A, stgs, p, s, bg, A->n, MAX(cgTol, CG_BEST_TOL));
+    CUBLAS(scal)(p->cublasHandle, A->m, &negOnef, &(bg[A->n]), 1);
+    accumByAGpu(p, bg, &(bg[A->n]));
+    cudaMemcpy(b, bg, (A->n + A->m) * sizeof(scs_float), cudaMemcpyDeviceToHost);
 
     if (iter >= 0) {
-		totCgIts += cgIts;
-	}
-
-	totalSolveTime += tocq(&linsysTimer);
+        totCgIts += cgIts;
+    }
+
+    totalSolveTime += tocq(&linsysTimer);
 #if EXTRAVERBOSE > 0
-	scs_printf("linsys solve time: %1.2es\n", tocq(&linsysTimer) / 1e3);
-#endif
-	return 0;
+    scs_printf("linsys solve time: %1.2es\n", tocq(&linsysTimer) / 1e3);
+#endif
+    return 0;
 }
 
 #ifdef __cplusplus

	return p;
}

/* solves (I+A'A)x = b, s warm start, solution stored in b */
static scs_int pcg(const AMatrix * A, const Settings * stgs, Priv * pr, const scs_float * s, scs_float * bh, scs_int max_its,
		scs_float tol) {
	scs_int i, n = A->n;
	scs_float alpha, nrm_r, nrm_r_old, pGp, negAlpha, beta;
    scs_float onef = 1.0, negOnef = -1.0;
	scs_float *p = pr->p; /* cg direction */
	scs_float *Gp = pr->Gp; /* updated CG direction */
	scs_float *r = pr->r; /* cg residual */
    scs_float *bg = pr->bg; /* rhs */
	cublasHandle_t cublasHandle = pr->cublasHandle;

    if (s == SCS_NULL) {
		cudaMemcpy(r, bh, n * sizeof(scs_float), cudaMemcpyHostToDevice);
		cudaMemset(bg, 0, n * sizeof(scs_float));
	} else {
        /* bg (= b) contains s */
        cudaMemcpy(bg, s, n * sizeof(scs_float), cudaMemcpyHostToDevice);
        matVec(A, stgs, pr, bg, r);
        /* p contains bh temporarily */
        cudaMemcpy(p, bh, n * sizeof(scs_float), cudaMemcpyHostToDevice);
        CUBLAS(axpy)(cublasHandle, n, &negOnef, p, 1, r, 1);
        CUBLAS(scal)(cublasHandle, n, &negOnef, r, 1);
    }

    cudaMalloc((void **)&p->p, A->n * sizeof(scs_float));
    cudaMalloc((void **)&p->r, A->n * sizeof(scs_float));
    cudaMalloc((void **)&p->Gp, A->n * sizeof(scs_float));
    cudaMalloc((void **)&p->bg, (A->n + A->m) * sizeof(scs_float));
    cudaMalloc((void **)&p->tmp_m, A->m * sizeof(scs_float)); /* intermediate result */

    cudaMemcpy(Ag->i, A->i, (A->p[A->n]) * sizeof(scs_int), cudaMemcpyHostToDevice);
