Updated CSR matrix routine