msm: kgsl: Implement fast preemption for 5XX

Allow 5XX targets to preempt quickly from an atomic context. In
particular this allows quicker transition from a high priority
ringbuffer to a lower one without having to wait for the worker
to schedule.

CRs-Fixed: 1009124
Change-Id: Ic0dedbad01a31a5da2954b097cb6fa937d45ef5c
Signed-off-by: Jordan Crouse <jcrouse@codeaurora.org>

22 files changed, 1735 insertions, 1771 deletions

diff --git a/drivers/gpu/msm/Makefile b/drivers/gpu/msm/Makefile
index db5a9ca28408..90aee3cad5ad 100644
--- a/drivers/gpu/msm/Makefile
+++ b/drivers/gpu/msm/Makefile
@@ -33,6 +33,8 @@ msm_adreno-y += \
 	adreno_a3xx_snapshot.o \
 	adreno_a4xx_snapshot.o \
 	adreno_a5xx_snapshot.o \
+	adreno_a4xx_preempt.o \
+	adreno_a5xx_preempt.o \
 	adreno_sysfs.o \
 	adreno.o \
 	adreno_cp_parser.o \
diff --git a/drivers/gpu/msm/adreno.c b/drivers/gpu/msm/adreno.c
index 28980ec1e07d..d2361790dedc 100644
--- a/drivers/gpu/msm/adreno.c
+++ b/drivers/gpu/msm/adreno.c
@@ -1557,6 +1557,22 @@ static int adreno_vbif_clear_pending_transactions(struct kgsl_device *device)
 	return ret;
 }
 
+static void adreno_set_active_ctxs_null(struct adreno_device *adreno_dev)
+{
+	int i;
+	struct adreno_ringbuffer *rb;
+
+	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
+		if (rb->drawctxt_active)
+			kgsl_context_put(&(rb->drawctxt_active->base));
+		rb->drawctxt_active = NULL;
+
+		kgsl_sharedmem_writel(KGSL_DEVICE(adreno_dev),
+			&rb->pagetable_desc, PT_INFO_OFFSET(current_rb_ptname),
+			0);
+	}
+}
+
 static int adreno_stop(struct kgsl_device *device)
 {
 	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
@@ -1671,10 +1687,7 @@ int adreno_reset(struct kgsl_device *device, int fault)
 
 	/* Set the page table back to the default page table */
 	kgsl_mmu_set_pt(&device->mmu, device->mmu.defaultpagetable);
-	kgsl_sharedmem_writel(device,
-		&adreno_dev->ringbuffers[0].pagetable_desc,
-		offsetof(struct adreno_ringbuffer_pagetable_info,
-			current_global_ptname), 0);
+	adreno_ringbuffer_set_global(adreno_dev, 0);
 
 	return ret;
 }
@@ -2298,10 +2311,8 @@ static int adreno_suspend_context(struct kgsl_device *device)
 		return status;
 	/* set the device to default pagetable */
 	kgsl_mmu_set_pt(&device->mmu, device->mmu.defaultpagetable);
-	kgsl_sharedmem_writel(device,
-		&adreno_dev->ringbuffers[0].pagetable_desc,
-		offsetof(struct adreno_ringbuffer_pagetable_info,
-			current_global_ptname), 0);
+	adreno_ringbuffer_set_global(adreno_dev, 0);
+
 	/* set ringbuffers to NULL ctxt */
 	adreno_set_active_ctxs_null(adreno_dev);
 
diff --git a/drivers/gpu/msm/adreno.h b/drivers/gpu/msm/adreno.h
index 31be91bf4c39..9f462bca26ce 100644
--- a/drivers/gpu/msm/adreno.h
+++ b/drivers/gpu/msm/adreno.h
@@ -193,6 +193,47 @@ enum adreno_gpurev {
 
 struct adreno_gpudev;
 
+/* Time to allow preemption to complete (in ms) */
+#define ADRENO_PREEMPT_TIMEOUT 10000
+
+/**
+ * enum adreno_preempt_states
+ * ADRENO_PREEMPT_NONE: No preemption is scheduled
+ * ADRENO_PREEMPT_START: The S/W has started
+ * ADRENO_PREEMPT_TRIGGERED: A preeempt has been triggered in the HW
+ * ADRENO_PREEMPT_FAULTED: The preempt timer has fired
+ * ADRENO_PREEMPT_PENDING: The H/W has signaled preemption complete
+ * ADRENO_PREEMPT_COMPLETE: Preemption could not be finished in the IRQ handler,
+ * worker has been scheduled
+ */
+enum adreno_preempt_states {
+	ADRENO_PREEMPT_NONE = 0,
+	ADRENO_PREEMPT_START,
+	ADRENO_PREEMPT_TRIGGERED,
+	ADRENO_PREEMPT_FAULTED,
+	ADRENO_PREEMPT_PENDING,
+	ADRENO_PREEMPT_COMPLETE,
+};
+
+/**
+ * struct adreno_preemption
+ * @state: The current state of preemption
+ * @counters: Memory descriptor for the memory where the GPU writes the
+ * preemption counters on switch
+ * @timer: A timer to make sure preemption doesn't stall
+ * @work: A work struct for the preemption worker (for 5XX)
+ * @token_submit: Indicates if a preempt token has been submitted in
+ * current ringbuffer (for 4XX)
+ */
+struct adreno_preemption {
+	atomic_t state;
+	struct kgsl_memdesc counters;
+	struct timer_list timer;
+	struct work_struct work;
+	bool token_submit;
+};
+
+
 struct adreno_busy_data {
 	unsigned int gpu_busy;
 	unsigned int vbif_ram_cycles;
@@ -368,7 +409,7 @@ struct adreno_device {
 	const struct firmware *lm_fw;
 	uint32_t *lm_sequence;
 	uint32_t lm_size;
-	struct kgsl_memdesc preemption_counters;
+	struct adreno_preemption preempt;
 	struct work_struct gpmu_work;
 	uint32_t lm_leakage;
 	uint32_t lm_limit;
@@ -1257,16 +1298,32 @@ static inline int adreno_bootstrap_ucode(struct adreno_device *adreno_dev)
 }
 
 /**
- * adreno_preempt_state() - Check if preemption state is equal to given state
+ * adreno_in_preempt_state() - Check if preemption state is equal to given state
  * @adreno_dev: Device whose preemption state is checked
  * @state: State to compare against
  */
-static inline unsigned int adreno_preempt_state(
-			struct adreno_device *adreno_dev,
-			enum adreno_dispatcher_preempt_states state)
+static inline bool adreno_in_preempt_state(struct adreno_device *adreno_dev,
+			enum adreno_preempt_states state)
 {
-	return atomic_read(&adreno_dev->dispatcher.preemption_state) ==
-		state;
+	return atomic_read(&adreno_dev->preempt.state) == state;
+}
+/**
+ * adreno_set_preempt_state() - Set the specified preemption state
+ * @adreno_dev: Device to change preemption state
+ * @state: State to set
+ */
+static inline void adreno_set_preempt_state(struct adreno_device *adreno_dev,
+		enum adreno_preempt_states state)
+{
+	/*
+	 * atomic_set doesn't use barriers, so we need to do it ourselves.  One
+	 * before...
+	 */
+	smp_wmb();
+	atomic_set(&adreno_dev->preempt.state, state);
+
+	/* ... and one after */
+	smp_wmb();
 }
 
 static inline bool adreno_is_preemption_enabled(
@@ -1274,7 +1331,6 @@ static inline bool adreno_is_preemption_enabled(
 {
 	return test_bit(ADRENO_DEVICE_PREEMPTION, &adreno_dev->priv);
 }
-
 /**
  * adreno_ctx_get_rb() - Return the ringbuffer that a context should
  * use based on priority
@@ -1311,25 +1367,6 @@ static inline struct adreno_ringbuffer *adreno_ctx_get_rb(
 		return &(adreno_dev->ringbuffers[
 				adreno_dev->num_ringbuffers - 1]);
 }
-/*
- * adreno_set_active_ctxs_null() - Put back reference to any active context
- * and set the active context to NULL
- * @adreno_dev: The adreno device
- */
-static inline void adreno_set_active_ctxs_null(struct adreno_device *adreno_dev)
-{
-	int i;
-	struct adreno_ringbuffer *rb;
-	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
-		if (rb->drawctxt_active)
-			kgsl_context_put(&(rb->drawctxt_active->base));
-		rb->drawctxt_active = NULL;
-		kgsl_sharedmem_writel(KGSL_DEVICE(adreno_dev),
-			&rb->pagetable_desc,
-			offsetof(struct adreno_ringbuffer_pagetable_info,
-				current_rb_ptname), 0);
-	}
-}
 
 /*
  * adreno_compare_prio_level() - Compares 2 priority levels based on enum values
@@ -1386,4 +1423,36 @@ static inline bool adreno_support_64bit(struct adreno_device *adreno_dev)
 }
 #endif /*BITS_PER_LONG*/
 
+static inline void adreno_ringbuffer_set_global(
+		struct adreno_device *adreno_dev, int name)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+
+	kgsl_sharedmem_writel(device,
+		&adreno_dev->ringbuffers[0].pagetable_desc,
+		PT_INFO_OFFSET(current_global_ptname), name);
+}
+
+static inline void adreno_ringbuffer_set_pagetable(struct adreno_ringbuffer *rb,
+		struct kgsl_pagetable *pt)
+{
+	struct adreno_device *adreno_dev = ADRENO_RB_DEVICE(rb);
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&rb->preempt_lock, flags);
+
+	kgsl_sharedmem_writel(device, &rb->pagetable_desc,
+		PT_INFO_OFFSET(current_rb_ptname), pt->name);
+
+	kgsl_sharedmem_writeq(device, &rb->pagetable_desc,
+		PT_INFO_OFFSET(ttbr0), kgsl_mmu_pagetable_get_ttbr0(pt));
+
+	kgsl_sharedmem_writel(device, &rb->pagetable_desc,
+		PT_INFO_OFFSET(contextidr),
+		kgsl_mmu_pagetable_get_contextidr(pt));
+
+	spin_unlock_irqrestore(&rb->preempt_lock, flags);
+}
+
 #endif /*__ADRENO_H */
diff --git a/drivers/gpu/msm/adreno_a3xx.c b/drivers/gpu/msm/adreno_a3xx.c
index ea8b75f4c83b..2accbe5c5764 100644
--- a/drivers/gpu/msm/adreno_a3xx.c
+++ b/drivers/gpu/msm/adreno_a3xx.c
@@ -1756,9 +1756,9 @@ static int _ringbuffer_bootstrap_ucode(struct adreno_device *adreno_dev,
 		*cmds++ = cp_type3_packet(CP_INTERRUPT, 1);
 		*cmds++ = 0;
 
-		rb->wptr = rb->wptr - 2;
+		rb->_wptr = rb->_wptr - 2;
 		adreno_ringbuffer_submit(rb, NULL);
-		rb->wptr = rb->wptr + 2;
+		rb->_wptr = rb->_wptr + 2;
 	} else {
 		for (i = pfp_idx; i < adreno_dev->pfp_fw_size; i++)
 			*cmds++ = adreno_dev->pfp_fw[i];
diff --git a/drivers/gpu/msm/adreno_a4xx.c b/drivers/gpu/msm/adreno_a4xx.c
index deadb483d60b..b15d23cfbe0a 100644
--- a/drivers/gpu/msm/adreno_a4xx.c
+++ b/drivers/gpu/msm/adreno_a4xx.c
@@ -178,101 +178,6 @@ static const struct adreno_vbif_platform a4xx_vbif_platforms[] = {
 	{ adreno_is_a418, a430_vbif },
 };
 
-/* a4xx_preemption_start() - Setup state to start preemption */
-static void a4xx_preemption_start(struct adreno_device *adreno_dev,
-		struct adreno_ringbuffer *rb)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	uint32_t val;
-
-	/*
-	 * Setup scratch registers from which the GPU will program the
-	 * registers required to start execution of new ringbuffer
-	 * set ringbuffer address
-	 */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG8,
-		rb->buffer_desc.gpuaddr);
-	kgsl_regread(device, A4XX_CP_RB_CNTL, &val);
-	/* scratch REG9 corresponds to CP_RB_CNTL register */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG9, val);
-	/* scratch REG10 corresponds to rptr address */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG10,
-			SCRATCH_RPTR_GPU_ADDR(device, rb->id));
-	/* scratch REG11 corresponds to rptr */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG11, adreno_get_rptr(rb));
-	/* scratch REG12 corresponds to wptr */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG12, rb->wptr);
-	/*
-	 * scratch REG13 corresponds to  IB1_BASE,
-	 * 0 since we do not do switches in between IB's
-	 */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG13, 0);
-	/* scratch REG14 corresponds to IB1_BUFSZ */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG14, 0);
-	/* scratch REG15 corresponds to IB2_BASE */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG15, 0);
-	/* scratch REG16 corresponds to  IB2_BUFSZ */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG16, 0);
-	/* scratch REG17 corresponds to GPR11 */
-	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG17, rb->gpr11);
-}
-
-/* a4xx_preemption_save() - Save the state after preemption is done */
-static void a4xx_preemption_save(struct adreno_device *adreno_dev,
-		struct adreno_ringbuffer *rb)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-
-	kgsl_regread(device, A4XX_CP_SCRATCH_REG23, &rb->gpr11);
-}
-
-static unsigned int a4xx_preemption_token(struct adreno_device *adreno_dev,
-			unsigned int *cmds, uint64_t gpuaddr)
-{
-	unsigned int *cmds_orig = cmds;
-
-	/* Turn on preemption flag */
-	/* preemption token - fill when pt switch command size is known */
-	*cmds++ = cp_type3_packet(CP_PREEMPT_TOKEN, 3);
-	*cmds++ = (uint)gpuaddr;
-	*cmds++ = 1;
-	/* generate interrupt on preemption completion */
-	*cmds++ = 1 << CP_PREEMPT_ORDINAL_INTERRUPT;
-
-	return (unsigned int) (cmds - cmds_orig);
-}
-
-static unsigned int a4xx_preemption_pre_ibsubmit(
-			struct adreno_device *adreno_dev,
-			struct adreno_ringbuffer *rb,
-			unsigned int *cmds,
-			struct kgsl_context *context)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	unsigned int *cmds_orig = cmds;
-	unsigned int cond_addr =
-		MEMSTORE_ID_GPU_ADDR(device, context->id, preempted);
-
-	cmds += a4xx_preemption_token(adreno_dev, cmds, cond_addr);
-
-	*cmds++ = cp_type3_packet(CP_COND_EXEC, 4);
-	*cmds++ = cond_addr;
-	*cmds++ = cond_addr;
-	*cmds++ = 1;
-	*cmds++ = 7;
-
-	/* clear preemption flag */
-	*cmds++ = cp_type3_packet(CP_MEM_WRITE, 2);
-	*cmds++ = cond_addr;
-	*cmds++ = 0;
-	*cmds++ = cp_type3_packet(CP_WAIT_MEM_WRITES, 1);
-	*cmds++ = 0;
-	*cmds++ = cp_type3_packet(CP_WAIT_FOR_ME, 1);
-	*cmds++ = 0;
-
-	return (unsigned int) (cmds - cmds_orig);
-}
-
 /*
  * a4xx_is_sptp_idle() - A430 SP/TP should be off to be considered idle
  * @adreno_dev: The adreno device pointer
@@ -713,6 +618,8 @@ static void a4xx_start(struct adreno_device *adreno_dev)
 		gpudev->vbif_xin_halt_ctrl0_mask =
 			A405_VBIF_XIN_HALT_CTRL0_MASK;
 
+	adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE);
+
 	a4xx_protect_init(adreno_dev);
 }
 
@@ -1753,6 +1660,19 @@ static struct adreno_coresight a4xx_coresight = {
 	.groups = a4xx_coresight_groups,
 };
 
+static void a4xx_preempt_callback(struct adreno_device *adreno_dev, int bit)
+{
+	if (atomic_read(&adreno_dev->preempt.state) != ADRENO_PREEMPT_TRIGGERED)
+		return;
+
+	trace_adreno_hw_preempt_trig_to_comp_int(adreno_dev->cur_rb,
+			      adreno_dev->next_rb,
+			      adreno_get_rptr(adreno_dev->cur_rb),
+			      adreno_get_rptr(adreno_dev->next_rb));
+
+	adreno_dispatcher_schedule(KGSL_DEVICE(adreno_dev));
+}
+
 #define A4XX_INT_MASK \
 	((1 << A4XX_INT_RBBM_AHB_ERROR) |		\
 	 (1 << A4XX_INT_RBBM_REG_TIMEOUT) |		\
@@ -1790,7 +1710,7 @@ static struct adreno_irq_funcs a4xx_irq_funcs[32] = {
 	/* 6 - RBBM_ATB_ASYNC_OVERFLOW */
 	ADRENO_IRQ_CALLBACK(a4xx_err_callback),
 	ADRENO_IRQ_CALLBACK(NULL), /* 7 - RBBM_GPC_ERR */
-	ADRENO_IRQ_CALLBACK(adreno_dispatcher_preempt_callback), /* 8 - CP_SW */
+	ADRENO_IRQ_CALLBACK(a4xx_preempt_callback), /* 8 - CP_SW */
 	ADRENO_IRQ_CALLBACK(a4xx_err_callback), /* 9 - CP_OPCODE_ERROR */
 	/* 10 - CP_RESERVED_BIT_ERROR */
 	ADRENO_IRQ_CALLBACK(a4xx_err_callback),
@@ -1831,417 +1751,6 @@ static struct adreno_snapshot_data a4xx_snapshot_data = {
 	.sect_sizes = &a4xx_snap_sizes,
 };
 
-#define ADRENO_RB_PREEMPT_TOKEN_DWORDS		125
-
-static int a4xx_submit_preempt_token(struct adreno_ringbuffer *rb,
-					struct adreno_ringbuffer *incoming_rb)
-{
-	struct adreno_device *adreno_dev = ADRENO_RB_DEVICE(rb);
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	unsigned int *ringcmds, *start;
-	int ptname;
-	struct kgsl_pagetable *pt;
-	int pt_switch_sizedwords = 0, total_sizedwords = 20;
-	unsigned link[ADRENO_RB_PREEMPT_TOKEN_DWORDS];
-	uint i;
-
-	if (incoming_rb->preempted_midway) {
-
-		kgsl_sharedmem_readl(&incoming_rb->pagetable_desc,
-			&ptname, offsetof(
-			struct adreno_ringbuffer_pagetable_info,
-			current_rb_ptname));
-		pt = kgsl_mmu_get_pt_from_ptname(&(device->mmu),
-			ptname);
-		/*
-		 * always expect a valid pt, else pt refcounting is
-		 * messed up or current pt tracking has a bug which
-		 * could lead to eventual disaster
-		 */
-		BUG_ON(!pt);
-		/* set the ringbuffer for incoming RB */
-		pt_switch_sizedwords =
-			adreno_iommu_set_pt_generate_cmds(incoming_rb,
-							&link[0], pt);
-		total_sizedwords += pt_switch_sizedwords;
-	}
-
-	/*
-	 *  Allocate total_sizedwords space in RB, this is the max space
-	 *  required.
-	 */
-	ringcmds = adreno_ringbuffer_allocspace(rb, total_sizedwords);
-
-	if (IS_ERR(ringcmds))
-		return PTR_ERR(ringcmds);
-
-	start = ringcmds;
-
-	*ringcmds++ = cp_packet(adreno_dev, CP_SET_PROTECTED_MODE, 1);
-	*ringcmds++ = 0;
-
-	if (incoming_rb->preempted_midway) {
-		for (i = 0; i < pt_switch_sizedwords; i++)
-			*ringcmds++ = link[i];
-	}
-
-	*ringcmds++ = cp_register(adreno_dev, adreno_getreg(adreno_dev,
-			ADRENO_REG_CP_PREEMPT_DISABLE), 1);
-	*ringcmds++ = 0;
-
-	*ringcmds++ = cp_packet(adreno_dev, CP_SET_PROTECTED_MODE, 1);
-	*ringcmds++ = 1;
-
-	ringcmds += a4xx_preemption_token(adreno_dev, ringcmds,
-				device->memstore.gpuaddr +
-				MEMSTORE_RB_OFFSET(rb, preempted));
-
-	if ((uint)(ringcmds - start) > total_sizedwords) {
-		KGSL_DRV_ERR(device, "Insufficient rb size allocated\n");
-		BUG();
-	}
-
-	/*
-	 * If we have commands less than the space reserved in RB
-	 *  adjust the wptr accordingly
-	 */
-	rb->wptr = rb->wptr - (total_sizedwords - (uint)(ringcmds - start));
-
-	/* submit just the preempt token */
-	mb();
-	kgsl_pwrscale_busy(device);
-	adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR, rb->wptr);
-	return 0;
-}
-
-/**
- * a4xx_preempt_trig_state() - Schedule preemption in TRIGGERRED
- * state
- * @adreno_dev: Device which is in TRIGGERRED state
- */
-static void a4xx_preempt_trig_state(
-			struct adreno_device *adreno_dev)
-{
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	unsigned int rbbase, val;
-
-	/*
-	 * Hardware not yet idle means that preemption interrupt
-	 * may still occur, nothing to do here until interrupt signals
-	 * completion of preemption, just return here
-	 */
-	if (!adreno_hw_isidle(adreno_dev))
-		return;
-
-	/*
-	 * We just changed states, reschedule dispatcher to change
-	 * preemption states
-	 */
-	if (ADRENO_DISPATCHER_PREEMPT_TRIGGERED !=
-		atomic_read(&dispatcher->preemption_state)) {
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-
-	/*
-	 * H/W is idle and we did not get a preemption interrupt, may
-	 * be device went idle w/o encountering any preempt token or
-	 * we already preempted w/o interrupt
-	 */
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_BASE, &rbbase);
-	 /* Did preemption occur, if so then change states and return */
-	if (rbbase != adreno_dev->cur_rb->buffer_desc.gpuaddr) {
-		adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT_DEBUG, &val);
-		if (val && rbbase == adreno_dev->next_rb->buffer_desc.gpuaddr) {
-			KGSL_DRV_INFO(device,
-			"Preemption completed without interrupt\n");
-			trace_adreno_hw_preempt_trig_to_comp(adreno_dev->cur_rb,
-					adreno_dev->next_rb,
-					adreno_get_rptr(adreno_dev->cur_rb),
-					adreno_get_rptr(adreno_dev->next_rb));
-			atomic_set(&dispatcher->preemption_state,
-				ADRENO_DISPATCHER_PREEMPT_COMPLETE);
-			adreno_dispatcher_schedule(device);
-			return;
-		}
-		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-		/* reschedule dispatcher to take care of the fault */
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-	/*
-	 * Check if preempt token was submitted after preemption trigger, if so
-	 * then preemption should have occurred, since device is already idle it
-	 * means something went wrong - trigger FT
-	 */
-	if (dispatcher->preempt_token_submit) {
-		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-		/* reschedule dispatcher to take care of the fault */
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-	/*
-	 * Preempt token was not submitted after preemption trigger so device
-	 * may have gone idle before preemption could occur, if there are
-	 * commands that got submitted to current RB after triggering preemption
-	 * then submit them as those commands may have a preempt token in them
-	 */
-	if (!adreno_rb_empty(adreno_dev->cur_rb)) {
-		/*
-		 * Memory barrier before informing the
-		 * hardware of new commands
-		 */
-		mb();
-		kgsl_pwrscale_busy(device);
-		adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR,
-			adreno_dev->cur_rb->wptr);
-		return;
-	}
-
-	/* Submit preempt token to make preemption happen */
-	if (adreno_drawctxt_switch(adreno_dev, adreno_dev->cur_rb, NULL, 0))
-		BUG();
-	if (a4xx_submit_preempt_token(adreno_dev->cur_rb,
-						adreno_dev->next_rb))
-		BUG();
-	dispatcher->preempt_token_submit = 1;
-	adreno_dev->cur_rb->wptr_preempt_end = adreno_dev->cur_rb->wptr;
-	trace_adreno_hw_preempt_token_submit(adreno_dev->cur_rb,
-			adreno_dev->next_rb,
-			adreno_get_rptr(adreno_dev->cur_rb),
-			adreno_get_rptr(adreno_dev->next_rb));
-}
-
-/**
- * a4xx_preempt_clear_state() - Schedule preemption in
- * CLEAR state. Preemption can be issued in this state.
- * @adreno_dev: Device which is in CLEAR state
- */
-static void a4xx_preempt_clear_state(
-			struct adreno_device *adreno_dev)
-
-{
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct adreno_ringbuffer *highest_busy_rb;
-	int switch_low_to_high;
-	int ret;
-
-	/* Device not awake means there is nothing to do */
-	if (!kgsl_state_is_awake(device))
-		return;
-
-	highest_busy_rb = adreno_dispatcher_get_highest_busy_rb(adreno_dev);
-	if (!highest_busy_rb)
-		return;
-
-	switch_low_to_high = adreno_compare_prio_level(
-					highest_busy_rb->id,
-					adreno_dev->cur_rb->id);
-
-	/* already current then return */
-	if (!switch_low_to_high)
-		return;
-
-	if (switch_low_to_high < 0) {
-		/*
-		 * if switching to lower priority make sure that the rptr and
-		 * wptr are equal, when the lower rb is not starved
-		 */
-		if (!adreno_rb_empty(adreno_dev->cur_rb))
-			return;
-		/*
-		 * switch to default context because when we switch back
-		 * to higher context then its not known which pt will
-		 * be current, so by making it default here the next
-		 * commands submitted will set the right pt
-		 */
-		ret = adreno_drawctxt_switch(adreno_dev,
-				adreno_dev->cur_rb,
-				NULL, 0);
-		/*
-		 * lower priority RB has to wait until space opens up in
-		 * higher RB
-		 */
-		if (ret)
-			return;
-
-		adreno_writereg(adreno_dev,
-			ADRENO_REG_CP_PREEMPT_DISABLE, 1);
-	}
-
-	/*
-	 * setup registers to do the switch to highest priority RB
-	 * which is not empty or may be starving away(poor thing)
-	 */
-	a4xx_preemption_start(adreno_dev, highest_busy_rb);
-
-	atomic_set(&dispatcher->preemption_state,
-			ADRENO_DISPATCHER_PREEMPT_TRIGGERED);
-
-	adreno_dev->next_rb = highest_busy_rb;
-	mod_timer(&dispatcher->preempt_timer, jiffies +
-		msecs_to_jiffies(ADRENO_DISPATCH_PREEMPT_TIMEOUT));
-
-	trace_adreno_hw_preempt_clear_to_trig(adreno_dev->cur_rb,
-			adreno_dev->next_rb,
-			adreno_get_rptr(adreno_dev->cur_rb),
-			adreno_get_rptr(adreno_dev->next_rb));
-	/* issue PREEMPT trigger */
-	adreno_writereg(adreno_dev, ADRENO_REG_CP_PREEMPT, 1);
-
-	/* submit preempt token packet to ensure preemption */
-	if (switch_low_to_high < 0) {
-		ret = a4xx_submit_preempt_token(
-			adreno_dev->cur_rb, adreno_dev->next_rb);
-		/*
-		 * unexpected since we are submitting this when rptr = wptr,
-		 * this was checked above already
-		 */
-		BUG_ON(ret);
-		dispatcher->preempt_token_submit = 1;
-		adreno_dev->cur_rb->wptr_preempt_end = adreno_dev->cur_rb->wptr;
-	} else {
-		dispatcher->preempt_token_submit = 0;
-		adreno_dispatcher_schedule(device);
-		adreno_dev->cur_rb->wptr_preempt_end = 0xFFFFFFFF;
-	}
-}
-
-/**
- * a4xx_preempt_complete_state() - Schedule preemption in
- * COMPLETE state
- * @adreno_dev: Device which is in COMPLETE state
- */
-static void a4xx_preempt_complete_state(
-			struct adreno_device *adreno_dev)
-
-{
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct adreno_dispatcher_cmdqueue *dispatch_q;
-	unsigned int wptr, rbbase;
-	unsigned int val, val1;
-	unsigned int prevrptr;
-
-	del_timer_sync(&dispatcher->preempt_timer);
-
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT, &val);
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT_DEBUG, &val1);
-
-	if (val || !val1) {
-		KGSL_DRV_ERR(device,
-		"Invalid state after preemption CP_PREEMPT: %08x, CP_PREEMPT_DEBUG: %08x\n",
-		val, val1);
-		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_BASE, &rbbase);
-	if (rbbase != adreno_dev->next_rb->buffer_desc.gpuaddr) {
-		KGSL_DRV_ERR(device,
-		"RBBASE incorrect after preemption, expected %x got %016llx\b",
-		rbbase,
-		adreno_dev->next_rb->buffer_desc.gpuaddr);
-		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-
-	a4xx_preemption_save(adreno_dev, adreno_dev->cur_rb);
-
-	dispatch_q = &(adreno_dev->cur_rb->dispatch_q);
-	/* new RB is the current RB */
-	trace_adreno_hw_preempt_comp_to_clear(adreno_dev->next_rb,
-			adreno_dev->cur_rb,
-			adreno_get_rptr(adreno_dev->next_rb),
-			adreno_get_rptr(adreno_dev->cur_rb));
-	adreno_dev->prev_rb = adreno_dev->cur_rb;
-	adreno_dev->cur_rb = adreno_dev->next_rb;
-	adreno_dev->cur_rb->preempted_midway = 0;
-	adreno_dev->cur_rb->wptr_preempt_end = 0xFFFFFFFF;
-	adreno_dev->next_rb = NULL;
-
-	if (adreno_disp_preempt_fair_sched) {
-		/* starved rb is now scheduled so unhalt dispatcher */
-		if (ADRENO_DISPATCHER_RB_STARVE_TIMER_ELAPSED ==
-			adreno_dev->cur_rb->starve_timer_state)
-			adreno_put_gpu_halt(adreno_dev);
-		adreno_dev->cur_rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_SCHEDULED;
-		adreno_dev->cur_rb->sched_timer = jiffies;
-		/*
-		 * If the outgoing RB is has commands then set the
-		 * busy time for it
-		 */
-		if (!adreno_rb_empty(adreno_dev->prev_rb)) {
-			adreno_dev->prev_rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_INIT;
-			adreno_dev->prev_rb->sched_timer = jiffies;
-		} else {
-			adreno_dev->prev_rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT;
-		}
-	}
-	atomic_set(&dispatcher->preemption_state,
-		ADRENO_DISPATCHER_PREEMPT_CLEAR);
-
-	prevrptr = adreno_get_rptr(adreno_dev->prev_rb);
-
-	if (adreno_compare_prio_level(adreno_dev->prev_rb->id,
-				adreno_dev->cur_rb->id) < 0) {
-		if (adreno_dev->prev_rb->wptr_preempt_end != prevrptr)
-			adreno_dev->prev_rb->preempted_midway = 1;
-	} else if (adreno_dev->prev_rb->wptr_preempt_end != prevrptr)
-		BUG();
-
-	/* submit wptr if required for new rb */
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_WPTR, &wptr);
-	if (adreno_dev->cur_rb->wptr != wptr) {
-		kgsl_pwrscale_busy(device);
-		adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR,
-					adreno_dev->cur_rb->wptr);
-	}
-	/* clear preemption register */
-	adreno_writereg(adreno_dev, ADRENO_REG_CP_PREEMPT_DEBUG, 0);
-	adreno_preempt_process_dispatch_queue(adreno_dev, dispatch_q);
-}
-
-static void a4xx_preemption_schedule(
-				struct adreno_device *adreno_dev)
-{
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-
-	if (!adreno_is_preemption_enabled(adreno_dev))
-		return;
-
-	mutex_lock(&device->mutex);
-
-	switch (atomic_read(&dispatcher->preemption_state)) {
-	case ADRENO_DISPATCHER_PREEMPT_CLEAR:
-		a4xx_preempt_clear_state(adreno_dev);
-		break;
-	case ADRENO_DISPATCHER_PREEMPT_TRIGGERED:
-		a4xx_preempt_trig_state(adreno_dev);
-		/*
-		 * if we transitioned to next state then fall-through
-		 * processing to next state
-		 */
-		if (!adreno_preempt_state(adreno_dev,
-			ADRENO_DISPATCHER_PREEMPT_COMPLETE))
-			break;
-	case ADRENO_DISPATCHER_PREEMPT_COMPLETE:
-		a4xx_preempt_complete_state(adreno_dev);
-		break;
-	default:
-		BUG();
-	}
-
-	mutex_unlock(&device->mutex);
-}
-
 struct adreno_gpudev adreno_a4xx_gpudev = {
 	.reg_offsets = &a4xx_reg_offsets,
 	.ft_perf_counters = a4xx_ft_perf_counters,
@@ -2267,4 +1776,5 @@ struct adreno_gpudev adreno_a4xx_gpudev = {
 	.regulator_disable = a4xx_regulator_disable,
 	.preemption_pre_ibsubmit = a4xx_preemption_pre_ibsubmit,
 	.preemption_schedule = a4xx_preemption_schedule,
+	.preemption_init = a4xx_preemption_init,
 };
diff --git a/drivers/gpu/msm/adreno_a4xx.h b/drivers/gpu/msm/adreno_a4xx.h
index e425dc8e9f7b..5dabc26fd34f 100644
--- a/drivers/gpu/msm/adreno_a4xx.h
+++ b/drivers/gpu/msm/adreno_a4xx.h
@@ -47,6 +47,15 @@
 		"RBBM_DPM_THERMAL_YELLOW_ERR" }, \
 	{ BIT(A4XX_INT_RBBM_DPM_THERMAL_RED_ERR), "RBBM_DPM_THERMAL_RED_ERR" }
 
+unsigned int a4xx_preemption_pre_ibsubmit(struct adreno_device *adreno_dev,
+			struct adreno_ringbuffer *rb,
+			unsigned int *cmds,
+			struct kgsl_context *context);
+
+void a4xx_preemption_schedule(struct adreno_device *adreno_dev);
+
+int a4xx_preemption_init(struct adreno_device *adreno_dev);
+
 void a4xx_snapshot(struct adreno_device *adreno_dev,
 		struct kgsl_snapshot *snapshot);
 
diff --git a/drivers/gpu/msm/adreno_a4xx_preempt.c b/drivers/gpu/msm/adreno_a4xx_preempt.c
new file mode 100644
index 000000000000..4087ac60c89e
--- /dev/null
+++ b/drivers/gpu/msm/adreno_a4xx_preempt.c
@@ -0,0 +1,571 @@
+/* Copyright (c) 2013-2016, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ */
+
+#include "adreno.h"
+#include "adreno_a4xx.h"
+#include "adreno_trace.h"
+#include "adreno_pm4types.h"
+
+#define ADRENO_RB_PREEMPT_TOKEN_DWORDS		125
+
+static void a4xx_preemption_timer(unsigned long data)
+{
+	struct adreno_device *adreno_dev = (struct adreno_device *) data;
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned int cur_rptr = adreno_get_rptr(adreno_dev->cur_rb);
+	unsigned int next_rptr = adreno_get_rptr(adreno_dev->next_rb);
+
+	KGSL_DRV_ERR(device,
+		"Preemption timed out. cur_rb rptr/wptr %x/%x id %d, next_rb rptr/wptr %x/%x id %d, disp_state: %d\n",
+		cur_rptr, adreno_dev->cur_rb->wptr, adreno_dev->cur_rb->id,
+		next_rptr, adreno_dev->next_rb->wptr, adreno_dev->next_rb->id,
+		atomic_read(&adreno_dev->preempt.state));
+
+	adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
+	adreno_dispatcher_schedule(device);
+}
+
+static unsigned int a4xx_preemption_token(struct adreno_device *adreno_dev,
+			unsigned int *cmds, uint64_t gpuaddr)
+{
+	unsigned int *cmds_orig = cmds;
+
+	/* Turn on preemption flag */
+	/* preemption token - fill when pt switch command size is known */
+	*cmds++ = cp_type3_packet(CP_PREEMPT_TOKEN, 3);
+	*cmds++ = (uint)gpuaddr;
+	*cmds++ = 1;
+	/* generate interrupt on preemption completion */
+	*cmds++ = 1 << CP_PREEMPT_ORDINAL_INTERRUPT;
+
+	return (unsigned int) (cmds - cmds_orig);
+}
+
+unsigned int a4xx_preemption_pre_ibsubmit(struct adreno_device *adreno_dev,
+		struct adreno_ringbuffer *rb, unsigned int *cmds,
+		struct kgsl_context *context)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned int *cmds_orig = cmds;
+	unsigned int cond_addr = device->memstore.gpuaddr +
+		MEMSTORE_ID_GPU_ADDR(device, context->id, preempted);
+
+	cmds += a4xx_preemption_token(adreno_dev, cmds, cond_addr);
+
+	*cmds++ = cp_type3_packet(CP_COND_EXEC, 4);
+	*cmds++ = cond_addr;
+	*cmds++ = cond_addr;
+	*cmds++ = 1;
+	*cmds++ = 7;
+
+	/* clear preemption flag */
+	*cmds++ = cp_type3_packet(CP_MEM_WRITE, 2);
+	*cmds++ = cond_addr;
+	*cmds++ = 0;
+	*cmds++ = cp_type3_packet(CP_WAIT_MEM_WRITES, 1);
+	*cmds++ = 0;
+	*cmds++ = cp_type3_packet(CP_WAIT_FOR_ME, 1);
+	*cmds++ = 0;
+
+	return (unsigned int) (cmds - cmds_orig);
+}
+
+
+static void a4xx_preemption_start(struct adreno_device *adreno_dev,
+		struct adreno_ringbuffer *rb)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	uint32_t val;
+
+	/*
+	 * Setup scratch registers from which the GPU will program the
+	 * registers required to start execution of new ringbuffer
+	 * set ringbuffer address
+	 */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG8,
+		rb->buffer_desc.gpuaddr);
+	kgsl_regread(device, A4XX_CP_RB_CNTL, &val);
+	/* scratch REG9 corresponds to CP_RB_CNTL register */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG9, val);
+	/* scratch REG10 corresponds to rptr address */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG10,
+		SCRATCH_RPTR_GPU_ADDR(device, rb->id));
+	/* scratch REG11 corresponds to rptr */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG11, adreno_get_rptr(rb));
+	/* scratch REG12 corresponds to wptr */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG12, rb->wptr);
+	/*
+	 * scratch REG13 corresponds to  IB1_BASE,
+	 * 0 since we do not do switches in between IB's
+	 */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG13, 0);
+	/* scratch REG14 corresponds to IB1_BUFSZ */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG14, 0);
+	/* scratch REG15 corresponds to IB2_BASE */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG15, 0);
+	/* scratch REG16 corresponds to  IB2_BUFSZ */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG16, 0);
+	/* scratch REG17 corresponds to GPR11 */
+	kgsl_regwrite(device, A4XX_CP_SCRATCH_REG17, rb->gpr11);
+}
+
+static void a4xx_preemption_save(struct adreno_device *adreno_dev,
+		struct adreno_ringbuffer *rb)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+
+	kgsl_regread(device, A4XX_CP_SCRATCH_REG23, &rb->gpr11);
+}
+
+
+static int a4xx_submit_preempt_token(struct adreno_ringbuffer *rb,
+					struct adreno_ringbuffer *incoming_rb)
+{
+	struct adreno_device *adreno_dev = ADRENO_RB_DEVICE(rb);
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned int *ringcmds, *start;
+	int ptname;
+	struct kgsl_pagetable *pt;
+	int pt_switch_sizedwords = 0, total_sizedwords = 20;
+	unsigned link[ADRENO_RB_PREEMPT_TOKEN_DWORDS];
+	uint i;
+
+	if (incoming_rb->preempted_midway) {
+
+		kgsl_sharedmem_readl(&incoming_rb->pagetable_desc,
+			&ptname, PT_INFO_OFFSET(current_rb_ptname));
+		pt = kgsl_mmu_get_pt_from_ptname(&(device->mmu),
+			ptname);
+		/* set the ringbuffer for incoming RB */
+		pt_switch_sizedwords =
+			adreno_iommu_set_pt_generate_cmds(incoming_rb,
+							&link[0], pt);
+		total_sizedwords += pt_switch_sizedwords;
+	}
+
+	/*
+	 *  Allocate total_sizedwords space in RB, this is the max space
+	 *  required.
+	 */
+	ringcmds = adreno_ringbuffer_allocspace(rb, total_sizedwords);
+
+	if (IS_ERR(ringcmds))
+		return PTR_ERR(ringcmds);
+
+	start = ringcmds;
+
+	*ringcmds++ = cp_packet(adreno_dev, CP_SET_PROTECTED_MODE, 1);
+	*ringcmds++ = 0;
+
+	if (incoming_rb->preempted_midway) {
+		for (i = 0; i < pt_switch_sizedwords; i++)
+			*ringcmds++ = link[i];
+	}
+
+	*ringcmds++ = cp_register(adreno_dev, adreno_getreg(adreno_dev,
+			ADRENO_REG_CP_PREEMPT_DISABLE), 1);
+	*ringcmds++ = 0;
+
+	*ringcmds++ = cp_packet(adreno_dev, CP_SET_PROTECTED_MODE, 1);
+	*ringcmds++ = 1;
+
+	ringcmds += a4xx_preemption_token(adreno_dev, ringcmds,
+				device->memstore.gpuaddr +
+				MEMSTORE_RB_OFFSET(rb, preempted));
+
+	if ((uint)(ringcmds - start) > total_sizedwords)
+		KGSL_DRV_ERR(device, "Insufficient rb size allocated\n");
+
+	/*
+	 * If we have commands less than the space reserved in RB
+	 *  adjust the wptr accordingly
+	 */
+	rb->wptr = rb->wptr - (total_sizedwords - (uint)(ringcmds - start));
+
+	/* submit just the preempt token */
+	mb();
+	kgsl_pwrscale_busy(device);
+	adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR, rb->wptr);
+	return 0;
+}
+
+static void a4xx_preempt_trig_state(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned int rbbase, val;
+	int ret;
+
+	/*
+	 * Hardware not yet idle means that preemption interrupt
+	 * may still occur, nothing to do here until interrupt signals
+	 * completion of preemption, just return here
+	 */
+	if (!adreno_hw_isidle(adreno_dev))
+		return;
+
+	/*
+	 * We just changed states, reschedule dispatcher to change
+	 * preemption states
+	 */
+
+	if (atomic_read(&adreno_dev->preempt.state) !=
+		ADRENO_PREEMPT_TRIGGERED) {
+		adreno_dispatcher_schedule(device);
+		return;
+	}
+
+	/*
+	 * H/W is idle and we did not get a preemption interrupt, may
+	 * be device went idle w/o encountering any preempt token or
+	 * we already preempted w/o interrupt
+	 */
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_BASE, &rbbase);
+	 /* Did preemption occur, if so then change states and return */
+	if (rbbase != adreno_dev->cur_rb->buffer_desc.gpuaddr) {
+		adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT_DEBUG, &val);
+		if (val && rbbase == adreno_dev->next_rb->buffer_desc.gpuaddr) {
+			KGSL_DRV_INFO(device,
+			"Preemption completed without interrupt\n");
+			trace_adreno_hw_preempt_trig_to_comp(adreno_dev->cur_rb,
+					adreno_dev->next_rb,
+					adreno_get_rptr(adreno_dev->cur_rb),
+					adreno_get_rptr(adreno_dev->next_rb));
+			adreno_set_preempt_state(adreno_dev,
+				ADRENO_PREEMPT_COMPLETE);
+			adreno_dispatcher_schedule(device);
+			return;
+		}
+		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
+		/* reschedule dispatcher to take care of the fault */
+		adreno_dispatcher_schedule(device);
+		return;
+	}
+	/*
+	 * Check if preempt token was submitted after preemption trigger, if so
+	 * then preemption should have occurred, since device is already idle it
+	 * means something went wrong - trigger FT
+	 */
+	if (adreno_dev->preempt.token_submit) {
+		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
+		/* reschedule dispatcher to take care of the fault */
+		adreno_dispatcher_schedule(device);
+		return;
+	}
+	/*
+	 * Preempt token was not submitted after preemption trigger so device
+	 * may have gone idle before preemption could occur, if there are
+	 * commands that got submitted to current RB after triggering preemption
+	 * then submit them as those commands may have a preempt token in them
+	 */
+	if (!adreno_rb_empty(adreno_dev->cur_rb)) {
+		/*
+		 * Memory barrier before informing the
+		 * hardware of new commands
+		 */
+		mb();
+		kgsl_pwrscale_busy(device);
+		adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR,
+			adreno_dev->cur_rb->wptr);
+		return;
+	}
+
+	/* Submit preempt token to make preemption happen */
+	ret = adreno_drawctxt_switch(adreno_dev, adreno_dev->cur_rb,
+		NULL, 0);
+	if (ret)
+		KGSL_DRV_ERR(device,
+			"Unable to switch context to NULL: %d\n", ret);
+
+	ret = a4xx_submit_preempt_token(adreno_dev->cur_rb,
+						adreno_dev->next_rb);
+	if (ret)
+		KGSL_DRV_ERR(device,
+			"Unable to submit preempt token: %d\n", ret);
+
+	adreno_dev->preempt.token_submit = true;
+	adreno_dev->cur_rb->wptr_preempt_end = adreno_dev->cur_rb->wptr;
+	trace_adreno_hw_preempt_token_submit(adreno_dev->cur_rb,
+			adreno_dev->next_rb,
+			adreno_get_rptr(adreno_dev->cur_rb),
+			adreno_get_rptr(adreno_dev->next_rb));
+}
+
+static struct adreno_ringbuffer *a4xx_next_ringbuffer(
+		struct adreno_device *adreno_dev)
+{
+	struct adreno_ringbuffer *rb, *next = NULL;
+	int i;
+
+	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
+		if (!adreno_rb_empty(rb) && next == NULL) {
+			next = rb;
+			continue;
+		}
+
+		if (!adreno_disp_preempt_fair_sched)
+			continue;
+
+		switch (rb->starve_timer_state) {
+		case ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT:
+			if (!adreno_rb_empty(rb) &&
+				adreno_dev->cur_rb != rb) {
+				rb->starve_timer_state =
+				ADRENO_DISPATCHER_RB_STARVE_TIMER_INIT;
+				rb->sched_timer = jiffies;
+			}
+			break;
+		case ADRENO_DISPATCHER_RB_STARVE_TIMER_INIT:
+			if (time_after(jiffies, rb->sched_timer +
+				msecs_to_jiffies(
+					adreno_dispatch_starvation_time))) {
+				rb->starve_timer_state =
+				ADRENO_DISPATCHER_RB_STARVE_TIMER_ELAPSED;
+				/* halt dispatcher to remove starvation */
+				adreno_get_gpu_halt(adreno_dev);
+			}
+			break;
+		case ADRENO_DISPATCHER_RB_STARVE_TIMER_SCHEDULED:
+			/*
+			 * If the RB has not been running for the minimum
+			 * time slice then allow it to run
+			 */
+			if (!adreno_rb_empty(rb) && time_before(jiffies,
+				adreno_dev->cur_rb->sched_timer +
+				msecs_to_jiffies(adreno_dispatch_time_slice)))
+				next = rb;
+			else
+				rb->starve_timer_state =
+				ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT;
+			break;
+		case ADRENO_DISPATCHER_RB_STARVE_TIMER_ELAPSED:
+		default:
+			break;
+		}
+	}
+
+	return next;
+}
+
+static void a4xx_preempt_clear_state(struct adreno_device *adreno_dev)
+
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct adreno_ringbuffer *highest_busy_rb;
+	int switch_low_to_high;
+	int ret;
+
+	/* Device not awake means there is nothing to do */
+	if (!kgsl_state_is_awake(device))
+		return;
+
+	highest_busy_rb = a4xx_next_ringbuffer(adreno_dev);
+	if (!highest_busy_rb || highest_busy_rb == adreno_dev->cur_rb)
+		return;
+
+	switch_low_to_high = adreno_compare_prio_level(
+					highest_busy_rb->id,
+					adreno_dev->cur_rb->id);
+
+	if (switch_low_to_high < 0) {
+		/*
+		 * if switching to lower priority make sure that the rptr and
+		 * wptr are equal, when the lower rb is not starved
+		 */
+		if (!adreno_rb_empty(adreno_dev->cur_rb))
+			return;
+		/*
+		 * switch to default context because when we switch back
+		 * to higher context then its not known which pt will
+		 * be current, so by making it default here the next
+		 * commands submitted will set the right pt
+		 */
+		ret = adreno_drawctxt_switch(adreno_dev,
+				adreno_dev->cur_rb,
+				NULL, 0);
+		/*
+		 * lower priority RB has to wait until space opens up in
+		 * higher RB
+		 */
+		if (ret) {
+			KGSL_DRV_ERR(device,
+				"Unable to switch context to NULL: %d",
+				ret);
+
+			return;
+		}
+
+		adreno_writereg(adreno_dev,
+			ADRENO_REG_CP_PREEMPT_DISABLE, 1);
+	}
+
+	/*
+	 * setup registers to do the switch to highest priority RB
+	 * which is not empty or may be starving away(poor thing)
+	 */
+	a4xx_preemption_start(adreno_dev, highest_busy_rb);
+
+	adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_TRIGGERED);
+
+	adreno_dev->next_rb = highest_busy_rb;
+	mod_timer(&adreno_dev->preempt.timer, jiffies +
+		msecs_to_jiffies(ADRENO_PREEMPT_TIMEOUT));
+
+	trace_adreno_hw_preempt_clear_to_trig(adreno_dev->cur_rb,
+			adreno_dev->next_rb,
+			adreno_get_rptr(adreno_dev->cur_rb),
+			adreno_get_rptr(adreno_dev->next_rb));
+	/* issue PREEMPT trigger */
+	adreno_writereg(adreno_dev, ADRENO_REG_CP_PREEMPT, 1);
+
+	/* submit preempt token packet to ensure preemption */
+	if (switch_low_to_high < 0) {
+		ret = a4xx_submit_preempt_token(
+			adreno_dev->cur_rb, adreno_dev->next_rb);
+		KGSL_DRV_ERR(device,
+			"Unable to submit preempt token: %d\n", ret);
+		adreno_dev->preempt.token_submit = true;
+		adreno_dev->cur_rb->wptr_preempt_end = adreno_dev->cur_rb->wptr;
+	} else {
+		adreno_dev->preempt.token_submit = false;
+		adreno_dispatcher_schedule(device);
+		adreno_dev->cur_rb->wptr_preempt_end = 0xFFFFFFFF;
+	}
+}
+
+static void a4xx_preempt_complete_state(struct adreno_device *adreno_dev)
+
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned int wptr, rbbase;
+	unsigned int val, val1;
+	unsigned int prevrptr;
+
+	del_timer_sync(&adreno_dev->preempt.timer);
+
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT, &val);
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT_DEBUG, &val1);
+
+	if (val || !val1) {
+		KGSL_DRV_ERR(device,
+		"Invalid state after preemption CP_PREEMPT: %08x, CP_PREEMPT_DEBUG: %08x\n",
+		val, val1);
+		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
+		adreno_dispatcher_schedule(device);
+		return;
+	}
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_BASE, &rbbase);
+	if (rbbase != adreno_dev->next_rb->buffer_desc.gpuaddr) {
+		KGSL_DRV_ERR(device,
+		"RBBASE incorrect after preemption, expected %x got %016llx\b",
+		rbbase,
+		adreno_dev->next_rb->buffer_desc.gpuaddr);
+		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
+		adreno_dispatcher_schedule(device);
+		return;
+	}
+
+	a4xx_preemption_save(adreno_dev, adreno_dev->cur_rb);
+
+	/* new RB is the current RB */
+	trace_adreno_hw_preempt_comp_to_clear(adreno_dev->next_rb,
+			adreno_dev->cur_rb,
+			adreno_get_rptr(adreno_dev->next_rb),
+			adreno_get_rptr(adreno_dev->cur_rb));
+
+	adreno_dev->prev_rb = adreno_dev->cur_rb;
+	adreno_dev->cur_rb = adreno_dev->next_rb;
+	adreno_dev->cur_rb->preempted_midway = 0;
+	adreno_dev->cur_rb->wptr_preempt_end = 0xFFFFFFFF;
+	adreno_dev->next_rb = NULL;
+
+	if (adreno_disp_preempt_fair_sched) {
+		/* starved rb is now scheduled so unhalt dispatcher */
+		if (ADRENO_DISPATCHER_RB_STARVE_TIMER_ELAPSED ==
+			adreno_dev->cur_rb->starve_timer_state)
+			adreno_put_gpu_halt(adreno_dev);
+		adreno_dev->cur_rb->starve_timer_state =
+				ADRENO_DISPATCHER_RB_STARVE_TIMER_SCHEDULED;
+		adreno_dev->cur_rb->sched_timer = jiffies;
+		/*
+		 * If the outgoing RB is has commands then set the
+		 * busy time for it
+		 */
+		if (!adreno_rb_empty(adreno_dev->prev_rb)) {
+			adreno_dev->prev_rb->starve_timer_state =
+				ADRENO_DISPATCHER_RB_STARVE_TIMER_INIT;
+			adreno_dev->prev_rb->sched_timer = jiffies;
+		} else {
+			adreno_dev->prev_rb->starve_timer_state =
+				ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT;
+		}
+	}
+	adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE);
+
+	prevrptr = adreno_get_rptr(adreno_dev->prev_rb);
+
+	if (adreno_compare_prio_level(adreno_dev->prev_rb->id,
+				adreno_dev->cur_rb->id) < 0) {
+		if (adreno_dev->prev_rb->wptr_preempt_end != prevrptr)
+			adreno_dev->prev_rb->preempted_midway = 1;
+	}
+
+	/* submit wptr if required for new rb */
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_WPTR, &wptr);
+	if (adreno_dev->cur_rb->wptr != wptr) {
+		kgsl_pwrscale_busy(device);
+		adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR,
+					adreno_dev->cur_rb->wptr);
+	}
+	/* clear preemption register */
+	adreno_writereg(adreno_dev, ADRENO_REG_CP_PREEMPT_DEBUG, 0);
+}
+
+void a4xx_preemption_schedule(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+
+	if (!adreno_is_preemption_enabled(adreno_dev))
+		return;
+
+	mutex_lock(&device->mutex);
+
+	switch (atomic_read(&adreno_dev->preempt.state)) {
+	case ADRENO_PREEMPT_NONE:
+		a4xx_preempt_clear_state(adreno_dev);
+		break;
+	case ADRENO_PREEMPT_TRIGGERED:
+		a4xx_preempt_trig_state(adreno_dev);
+		/*
+		 * if we transitioned to next state then fall-through
+		 * processing to next state
+		 */
+		if (!adreno_in_preempt_state(adreno_dev,
+			ADRENO_PREEMPT_COMPLETE))
+			break;
+	case ADRENO_PREEMPT_COMPLETE:
+		a4xx_preempt_complete_state(adreno_dev);
+		break;
+	default:
+		break;
+	}
+
+	mutex_unlock(&device->mutex);
+}
+
+int a4xx_preemption_init(struct adreno_device *adreno_dev)
+{
+	setup_timer(&adreno_dev->preempt.timer, a4xx_preemption_timer,
+		(unsigned long) adreno_dev);
+
+	return 0;
+}
diff --git a/drivers/gpu/msm/adreno_a5xx.c b/drivers/gpu/msm/adreno_a5xx.c
index cee6c8dbb60e..96f72c59e4cd 100644
--- a/drivers/gpu/msm/adreno_a5xx.c
+++ b/drivers/gpu/msm/adreno_a5xx.c
@@ -60,19 +60,12 @@ static const struct adreno_vbif_platform a5xx_vbif_platforms[] = {
 	{ adreno_is_a506, a530_vbif },
 };
 
-#define PREEMPT_RECORD(_field) \
-		offsetof(struct a5xx_cp_preemption_record, _field)
-
-#define PREEMPT_SMMU_RECORD(_field) \
-		offsetof(struct a5xx_cp_smmu_info, _field)
-
 static void a5xx_irq_storm_worker(struct work_struct *work);
 static int _read_fw2_block_header(uint32_t *header, uint32_t id,
 	uint32_t major, uint32_t minor);
 static void a5xx_gpmu_reset(struct work_struct *work);
 static int a5xx_gpmu_init(struct adreno_device *adreno_dev);
 
-
 /**
  * Number of times to check if the regulator enabled before
  * giving up and returning failure.
@@ -108,8 +101,9 @@ static void spin_idle_debug(struct kgsl_device *device,
 	kgsl_regread(device, A5XX_CP_HW_FAULT, &hwfault);
 
 	dev_err(device->dev,
-		" rb=%X/%X rbbm_status=%8.8X/%8.8X int_0_status=%8.8X\n",
-		rptr, wptr, status, status3, intstatus);
+		"rb=%d pos=%X/%X rbbm_status=%8.8X/%8.8X int_0_status=%8.8X\n",
+		adreno_dev->cur_rb->id, rptr, wptr, status, status3, intstatus);
+
 	dev_err(device->dev, " hwfault=%8.8X\n", hwfault);
 
 	kgsl_device_snapshot(device, NULL);
@@ -179,267 +173,6 @@ static void a5xx_check_features(struct adreno_device *adreno_dev)
 	adreno_efuse_unmap(adreno_dev);
 }
 
-/*
- * a5xx_preemption_start() - Setup state to start preemption
- */
-static void a5xx_preemption_start(struct adreno_device *adreno_dev,
-		struct adreno_ringbuffer *rb)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct kgsl_iommu *iommu = KGSL_IOMMU_PRIV(device);
-	uint64_t ttbr0;
-	uint32_t contextidr;
-	struct kgsl_pagetable *pt;
-	bool switch_default_pt = true;
-
-	kgsl_sharedmem_writel(device, &rb->preemption_desc,
-		PREEMPT_RECORD(wptr), rb->wptr);
-	kgsl_sharedmem_writel(device, &rb->preemption_desc,
-		PREEMPT_RECORD(rptr), adreno_get_rptr(rb));
-	kgsl_regwrite(device, A5XX_CP_CONTEXT_SWITCH_RESTORE_ADDR_LO,
-		lower_32_bits(rb->preemption_desc.gpuaddr));
-	kgsl_regwrite(device, A5XX_CP_CONTEXT_SWITCH_RESTORE_ADDR_HI,
-		upper_32_bits(rb->preemption_desc.gpuaddr));
-	kgsl_sharedmem_readq(&rb->pagetable_desc, &ttbr0,
-		offsetof(struct adreno_ringbuffer_pagetable_info, ttbr0));
-	kgsl_sharedmem_readl(&rb->pagetable_desc, &contextidr,
-		offsetof(struct adreno_ringbuffer_pagetable_info, contextidr));
-
-	spin_lock(&kgsl_driver.ptlock);
-	list_for_each_entry(pt, &kgsl_driver.pagetable_list, list) {
-		if (kgsl_mmu_pagetable_get_ttbr0(pt) == ttbr0) {
-			switch_default_pt = false;
-			break;
-		}
-	}
-	spin_unlock(&kgsl_driver.ptlock);
-
-	if (switch_default_pt) {
-		ttbr0 = kgsl_mmu_pagetable_get_ttbr0(
-				device->mmu.defaultpagetable);
-		contextidr = kgsl_mmu_pagetable_get_contextidr(
-				device->mmu.defaultpagetable);
-	}
-
-	kgsl_sharedmem_writeq(device, &iommu->smmu_info,
-		offsetof(struct a5xx_cp_smmu_info, ttbr0), ttbr0);
-	kgsl_sharedmem_writel(device, &iommu->smmu_info,
-		offsetof(struct a5xx_cp_smmu_info, context_idr), contextidr);
-}
-
-#define _CP_CNTL (((ilog2(4) << 8) & 0x1F00) | \
-			(ilog2(KGSL_RB_DWORDS >> 1) & 0x3F))
-
-#ifdef CONFIG_QCOM_KGSL_IOMMU
-static int a5xx_preemption_iommu_init(struct adreno_device *adreno_dev)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct kgsl_iommu *iommu = KGSL_IOMMU_PRIV(device);
-
-	/* Allocate mem for storing preemption smmu record */
-	return kgsl_allocate_global(device, &iommu->smmu_info, PAGE_SIZE,
-		KGSL_MEMFLAGS_GPUREADONLY, KGSL_MEMDESC_PRIVILEGED);
-}
-#else
-static int a5xx_preemption_iommu_init(struct adreno_device *adreno_dev)
-{
-	return -ENODEV;
-}
-#endif
-
-static int a5xx_preemption_init(struct adreno_device *adreno_dev)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct adreno_ringbuffer *rb;
-	int ret;
-	unsigned int i;
-	uint64_t addr;
-
-	/* We are dependent on IOMMU to make preemption go on the CP side */
-	if (kgsl_mmu_get_mmutype(device) != KGSL_MMU_TYPE_IOMMU)
-		return -ENODEV;
-
-	/* Allocate mem for storing preemption counters */
-	ret = kgsl_allocate_global(device, &adreno_dev->preemption_counters,
-		adreno_dev->num_ringbuffers *
-		A5XX_CP_CTXRECORD_PREEMPTION_COUNTER_SIZE, 0, 0);
-	if (ret)
-		return ret;
-
-	addr = adreno_dev->preemption_counters.gpuaddr;
-
-	/* Allocate mem for storing preemption switch record */
-	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
-		ret = kgsl_allocate_global(device,
-			&rb->preemption_desc, A5XX_CP_CTXRECORD_SIZE_IN_BYTES,
-			0, KGSL_MEMDESC_PRIVILEGED);
-		if (ret)
-			return ret;
-
-		/* Initialize the context switch record here */
-		kgsl_sharedmem_writel(device, &rb->preemption_desc,
-			PREEMPT_RECORD(magic), A5XX_CP_CTXRECORD_MAGIC_REF);
-		kgsl_sharedmem_writel(device, &rb->preemption_desc,
-			PREEMPT_RECORD(info), 0);
-		kgsl_sharedmem_writel(device, &rb->preemption_desc,
-			PREEMPT_RECORD(data), 0);
-		kgsl_sharedmem_writel(device, &rb->preemption_desc,
-			PREEMPT_RECORD(cntl), _CP_CNTL);
-		kgsl_sharedmem_writeq(device, &rb->preemption_desc,
-				PREEMPT_RECORD(rptr_addr),
-				SCRATCH_RPTR_GPU_ADDR(device, i));
-		kgsl_sharedmem_writel(device, &rb->preemption_desc,
-			PREEMPT_RECORD(rptr), 0);
-		kgsl_sharedmem_writel(device, &rb->preemption_desc,
-			PREEMPT_RECORD(wptr), 0);
-		kgsl_sharedmem_writeq(device, &rb->preemption_desc,
-			PREEMPT_RECORD(rbase),
-			adreno_dev->ringbuffers[i].buffer_desc.gpuaddr);
-		kgsl_sharedmem_writeq(device, &rb->preemption_desc,
-			PREEMPT_RECORD(counter), addr);
-
-		addr += A5XX_CP_CTXRECORD_PREEMPTION_COUNTER_SIZE;
-	}
-
-	return a5xx_preemption_iommu_init(adreno_dev);
-}
-
-/*
- * a5xx_preemption_token() - Preempt token on a5xx
- * PM4 commands for preempt token on a5xx. These commands are
- * submitted to ringbuffer to trigger preemption.
- */
-static unsigned int a5xx_preemption_token(struct adreno_device *adreno_dev,
-			unsigned int *cmds)
-{
-	unsigned int *cmds_orig = cmds;
-
-	*cmds++ = cp_type7_packet(CP_CONTEXT_SWITCH_YIELD, 4);
-	/* Write NULL to the address to skip the data write */
-	cmds += cp_gpuaddr(adreno_dev, cmds, 0x0);
-	*cmds++ = 1;
-	/* generate interrupt on preemption completion */
-	*cmds++ = 1;
-
-	return (unsigned int) (cmds - cmds_orig);
-}
-
-/*
- * a5xx_preemption_pre_ibsubmit() - Below PM4 commands are
- * added at the beginning of every cmdbatch submission.
- */
-static unsigned int a5xx_preemption_pre_ibsubmit(
-			struct adreno_device *adreno_dev,
-			struct adreno_ringbuffer *rb,
-			unsigned int *cmds, struct kgsl_context *context)
-{
-	unsigned int *cmds_orig = cmds;
-	uint64_t gpuaddr = rb->preemption_desc.gpuaddr;
-	unsigned int preempt_style = 0;
-
-	if (context) {
-		/*
-		 * Preemption from secure to unsecure needs Zap shader to be
-		 * run to clear all secure content. CP does not know during
-		 * preemption if it is switching between secure and unsecure
-		 * contexts so restrict Secure contexts to be preempted at
-		 * ringbuffer level.
-		 */
-		if (context->flags & KGSL_CONTEXT_SECURE)
-			preempt_style = KGSL_CONTEXT_PREEMPT_STYLE_RINGBUFFER;
-		else
-			preempt_style = ADRENO_PREEMPT_STYLE(context->flags);
-	}
-
-	/*
-	 * CP_PREEMPT_ENABLE_GLOBAL(global preemption) can only be set by KMD
-	 * in ringbuffer.
-	 * 1) set global preemption to 0x0 to disable global preemption.
-	 *    Only RB level preemption is allowed in this mode
-	 * 2) Set global preemption to defer(0x2) for finegrain preemption.
-	 *    when global preemption is set to defer(0x2),
-	 *    CP_PREEMPT_ENABLE_LOCAL(local preemption) determines the
-	 *    preemption point. Local preemption
-	 *    can be enabled by both UMD(within IB) and KMD.
-	 */
-	*cmds++ = cp_type7_packet(CP_PREEMPT_ENABLE_GLOBAL, 1);
-	*cmds++ = ((preempt_style == KGSL_CONTEXT_PREEMPT_STYLE_FINEGRAIN)
-				? 2 : 0);
-
-	/* Turn CP protection OFF */
-	*cmds++ = cp_type7_packet(CP_SET_PROTECTED_MODE, 1);
-	*cmds++ = 0;
-
-	/*
-	 * CP during context switch will save context switch info to
-	 * a5xx_cp_preemption_record pointed by CONTEXT_SWITCH_SAVE_ADDR
-	 */
-	*cmds++ = cp_type4_packet(A5XX_CP_CONTEXT_SWITCH_SAVE_ADDR_LO, 1);
-	*cmds++ = lower_32_bits(gpuaddr);
-	*cmds++ = cp_type4_packet(A5XX_CP_CONTEXT_SWITCH_SAVE_ADDR_HI, 1);
-	*cmds++ = upper_32_bits(gpuaddr);
-
-	/* Turn CP protection ON */
-	*cmds++ = cp_type7_packet(CP_SET_PROTECTED_MODE, 1);
-	*cmds++ = 1;
-
-	/*
-	 * Enable local preemption for finegrain preemption in case of
-	 * a misbehaving IB
-	 */
-	if (preempt_style == KGSL_CONTEXT_PREEMPT_STYLE_FINEGRAIN) {
-		*cmds++ = cp_type7_packet(CP_PREEMPT_ENABLE_LOCAL, 1);
-		*cmds++ = 1;
-	} else {
-		*cmds++ = cp_type7_packet(CP_PREEMPT_ENABLE_LOCAL, 1);
-		*cmds++ = 0;
-	}
-
-	/* Enable CP_CONTEXT_SWITCH_YIELD packets in the IB2s */
-	*cmds++ = cp_type7_packet(CP_YIELD_ENABLE, 1);
-	*cmds++ = 2;
-
-	return (unsigned int) (cmds - cmds_orig);
-}
-
-/*
- * a5xx_preemption_yield_enable() - Below PM4 commands are
- * added after every cmdbatch submission.
- */
-static int a5xx_preemption_yield_enable(unsigned int *cmds)
-{
-	/*
-	 * SRM -- set render mode (ex binning, direct render etc)
-	 * SRM is set by UMD usually at start of IB to tell CP the type of
-	 * preemption.
-	 * KMD needs to set SRM to NULL to indicate CP that rendering is
-	 * done by IB.
-	 */
-	*cmds++ = cp_type7_packet(CP_SET_RENDER_MODE, 5);
-	*cmds++ = 0;
-	*cmds++ = 0;
-	*cmds++ = 0;
-	*cmds++ = 0;
-	*cmds++ = 0;
-
-	*cmds++ = cp_type7_packet(CP_YIELD_ENABLE, 1);
-	*cmds++ = 1;
-
-	return 8;
-}
-
-/*
- * a5xx_preemption_post_ibsubmit() - Below PM4 commands are
- * added after every cmdbatch submission.
- */
-static unsigned int a5xx_preemption_post_ibsubmit(
-			struct adreno_device *adreno_dev,
-			unsigned int *cmds)
-
-{
-	return a5xx_preemption_token(adreno_dev, cmds);
-}
-
 static void a5xx_platform_setup(struct adreno_device *adreno_dev)
 {
 	uint64_t addr;
@@ -1962,12 +1695,8 @@ out:
 static void a5xx_start(struct adreno_device *adreno_dev)
 {
 	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct kgsl_iommu *iommu = KGSL_IOMMU_PRIV(device);
 	struct adreno_gpudev *gpudev = ADRENO_GPU_DEVICE(adreno_dev);
-	unsigned int i, bit;
-	struct adreno_ringbuffer *rb;
-	uint64_t def_ttbr0;
-	uint32_t contextidr;
+	unsigned int bit;
 
 	adreno_vbif_start(adreno_dev, a5xx_vbif_platforms,
 			ARRAY_SIZE(a5xx_vbif_platforms));
@@ -2168,48 +1897,14 @@ static void a5xx_start(struct adreno_device *adreno_dev)
 
 	}
 
-	if (adreno_is_preemption_enabled(adreno_dev)) {
-		struct kgsl_pagetable *pt = device->mmu.defaultpagetable;
-
-		def_ttbr0 = kgsl_mmu_pagetable_get_ttbr0(pt);
-		contextidr = kgsl_mmu_pagetable_get_contextidr(pt);
-
-		/* Initialize the context switch record here */
-		kgsl_sharedmem_writel(device, &iommu->smmu_info,
-				PREEMPT_SMMU_RECORD(magic),
-				A5XX_CP_SMMU_INFO_MAGIC_REF);
-		kgsl_sharedmem_writeq(device, &iommu->smmu_info,
-				PREEMPT_SMMU_RECORD(ttbr0), def_ttbr0);
-		/*
-		 * The CP doesn't actually use the asid field, so
-		 * put a bad value into it until it is removed from
-		 * the preemption record.
-		 */
-		kgsl_sharedmem_writeq(device, &iommu->smmu_info,
-				PREEMPT_SMMU_RECORD(asid),
-				0xdecafbad);
-		kgsl_sharedmem_writeq(device, &iommu->smmu_info,
-				PREEMPT_SMMU_RECORD(context_idr),
-				contextidr);
-		adreno_writereg64(adreno_dev,
-				ADRENO_REG_CP_CONTEXT_SWITCH_SMMU_INFO_LO,
-				ADRENO_REG_CP_CONTEXT_SWITCH_SMMU_INFO_HI,
-				iommu->smmu_info.gpuaddr);
-
-		FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
-			kgsl_sharedmem_writel(device, &rb->preemption_desc,
-				PREEMPT_RECORD(rptr), 0);
-			kgsl_sharedmem_writel(device, &rb->preemption_desc,
-				PREEMPT_RECORD(wptr), 0);
-			kgsl_sharedmem_writeq(device, &rb->pagetable_desc,
-			  offsetof(struct adreno_ringbuffer_pagetable_info,
-			  ttbr0), def_ttbr0);
-		}
-	}
-
+	a5xx_preemption_start(adreno_dev);
 	a5xx_protect_init(adreno_dev);
 }
 
+/*
+ * Follow the ME_INIT sequence with a preemption yield to allow the GPU to move
+ * to a different ringbuffer, if desired
+ */
 static int _preemption_init(
 			struct adreno_device *adreno_dev,
 			struct adreno_ringbuffer *rb, unsigned int *cmds,
@@ -2284,7 +1979,7 @@ static int a5xx_post_start(struct adreno_device *adreno_dev)
 	if (adreno_is_preemption_enabled(adreno_dev))
 		cmds += _preemption_init(adreno_dev, rb, cmds, NULL);
 
-	rb->wptr = rb->wptr - (42 - (cmds - start));
+	rb->_wptr = rb->_wptr - (42 - (cmds - start));
 
 	ret = adreno_ringbuffer_submit_spin(rb, NULL, 2000);
 	if (ret)
@@ -2598,7 +2293,8 @@ static int a5xx_rb_start(struct adreno_device *adreno_dev,
 	 * in certain circumstances.
 	 */
 
-	adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_CNTL, _CP_CNTL);
+	adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_CNTL,
+		A5XX_CP_RB_CNTL_DEFAULT);
 
 	adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_BASE,
 			rb->buffer_desc.gpuaddr);
@@ -3412,6 +3108,8 @@ static void a5xx_cp_callback(struct adreno_device *adreno_dev, int bit)
 		prev = cur;
 	}
 
+	a5xx_preemption_trigger(adreno_dev);
+
 	kgsl_schedule_work(&device->event_work);
 	adreno_dispatcher_schedule(device);
 }
@@ -3496,9 +3194,6 @@ void a5x_gpc_err_int_callback(struct adreno_device *adreno_dev, int bit)
 	 (1 << A5XX_INT_RBBM_ATB_ASYNC_OVERFLOW) |		\
 	 (1 << A5XX_INT_RBBM_GPC_ERROR) |		\
 	 (1 << A5XX_INT_CP_HW_ERROR) |	\
-	 (1 << A5XX_INT_CP_IB1) |			\
-	 (1 << A5XX_INT_CP_IB2) |			\
-	 (1 << A5XX_INT_CP_RB) |			\
 	 (1 << A5XX_INT_CP_CACHE_FLUSH_TS) |		\
 	 (1 << A5XX_INT_RBBM_ATB_BUS_OVERFLOW) |	\
 	 (1 << A5XX_INT_UCHE_OOB_ACCESS) |		\
@@ -3521,7 +3216,7 @@ static struct adreno_irq_funcs a5xx_irq_funcs[32] = {
 	/* 6 - RBBM_ATB_ASYNC_OVERFLOW */
 	ADRENO_IRQ_CALLBACK(a5xx_err_callback),
 	ADRENO_IRQ_CALLBACK(a5x_gpc_err_int_callback), /* 7 - GPC_ERR */
-	ADRENO_IRQ_CALLBACK(adreno_dispatcher_preempt_callback),/* 8 - CP_SW */
+	ADRENO_IRQ_CALLBACK(a5xx_preempt_callback),/* 8 - CP_SW */
 	ADRENO_IRQ_CALLBACK(a5xx_cp_hw_err_callback), /* 9 - CP_HW_ERROR */
 	/* 10 - CP_CCU_FLUSH_DEPTH_TS */
 	ADRENO_IRQ_CALLBACK(NULL),
@@ -3529,9 +3224,9 @@ static struct adreno_irq_funcs a5xx_irq_funcs[32] = {
 	ADRENO_IRQ_CALLBACK(NULL),
 	 /* 12 - CP_CCU_RESOLVE_TS */
 	ADRENO_IRQ_CALLBACK(NULL),
-	ADRENO_IRQ_CALLBACK(adreno_cp_callback), /* 13 - CP_IB2_INT */
-	ADRENO_IRQ_CALLBACK(adreno_cp_callback), /* 14 - CP_IB1_INT */
-	ADRENO_IRQ_CALLBACK(adreno_cp_callback), /* 15 - CP_RB_INT */
+	ADRENO_IRQ_CALLBACK(NULL), /* 13 - CP_IB2_INT */
+	ADRENO_IRQ_CALLBACK(NULL), /* 14 - CP_IB1_INT */
+	ADRENO_IRQ_CALLBACK(NULL), /* 15 - CP_RB_INT */
 	/* 16 - CCP_UNUSED_1 */
 	ADRENO_IRQ_CALLBACK(NULL),
 	ADRENO_IRQ_CALLBACK(NULL), /* 17 - CP_RB_DONE_TS */
@@ -3768,292 +3463,6 @@ static struct adreno_coresight a5xx_coresight = {
 	.groups = a5xx_coresight_groups,
 };
 
-/**
- * a5xx_preempt_trig_state() - Schedule preemption in TRIGGERRED
- * state
- * @adreno_dev: Device which is in TRIGGERRED state
- */
-static void a5xx_preempt_trig_state(
-			struct adreno_device *adreno_dev)
-{
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	unsigned int preempt_busy;
-	uint64_t rbbase;
-
-	/*
-	 * triggered preemption, check for busy bits, if not set go to complete
-	 * bit 0: When high indicates CP is not done with preemption.
-	 * bit 4: When high indicates that the CP is actively switching between
-	 *        application contexts.
-	 * Check both the bits to make sure CP is done with preemption.
-	 */
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT, &preempt_busy);
-	if (!(preempt_busy & 0x11)) {
-
-		adreno_readreg64(adreno_dev, ADRENO_REG_CP_RB_BASE,
-				 ADRENO_REG_CP_RB_BASE_HI, &rbbase);
-		/* Did preemption occur, if so then change states and return */
-		if (rbbase != adreno_dev->cur_rb->buffer_desc.gpuaddr) {
-			if (rbbase ==
-				adreno_dev->next_rb->buffer_desc.gpuaddr) {
-				KGSL_DRV_INFO(device,
-				"Preemption completed without interrupt\n");
-				trace_adreno_hw_preempt_trig_to_comp(
-					adreno_dev->cur_rb,
-					adreno_dev->next_rb,
-					adreno_get_rptr(adreno_dev->cur_rb),
-					adreno_get_rptr(adreno_dev->next_rb));
-				atomic_set(&dispatcher->preemption_state,
-					ADRENO_DISPATCHER_PREEMPT_COMPLETE);
-			} else {
-				/*
-				 * Something wrong with preemption.
-				 * Set fault and reschedule dispatcher to take
-				 * care of fault.
-				 */
-				adreno_set_gpu_fault(adreno_dev,
-					ADRENO_PREEMPT_FAULT);
-			}
-			adreno_dispatcher_schedule(device);
-			return;
-		}
-	}
-
-	/*
-	 * Preemption is still happening.
-	 * Hardware not yet idle means that preemption interrupt
-	 * may still occur, nothing to do here until interrupt signals
-	 * completion of preemption, just return here
-	 */
-	if (!adreno_hw_isidle(adreno_dev))
-		return;
-
-	/*
-	 * We just changed states, reschedule dispatcher to change
-	 * preemption states
-	 */
-	if (ADRENO_DISPATCHER_PREEMPT_TRIGGERED !=
-		atomic_read(&dispatcher->preemption_state)) {
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-
-
-	adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-
-	/* reschedule dispatcher to take care of the fault */
-	adreno_dispatcher_schedule(device);
-}
-
-/**
- * a5xx_preempt_clear_state() - Schedule preemption in CLEAR
- * state. Preemption can be issued in this state.
- * @adreno_dev: Device which is in CLEAR state
- */
-static void a5xx_preempt_clear_state(
-			struct adreno_device *adreno_dev)
-
-{
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct adreno_ringbuffer *highest_busy_rb;
-	int switch_low_to_high;
-
-	/* Device not awake means there is nothing to do */
-	if (!kgsl_state_is_awake(device))
-		return;
-
-	highest_busy_rb = adreno_dispatcher_get_highest_busy_rb(adreno_dev);
-	if (!highest_busy_rb)
-		return;
-
-	switch_low_to_high = adreno_compare_prio_level(
-		highest_busy_rb->id, adreno_dev->cur_rb->id);
-
-	/* already current then return */
-	if (!switch_low_to_high)
-		return;
-
-	if (switch_low_to_high < 0) {
-
-		if (!adreno_hw_isidle(adreno_dev)) {
-			adreno_dispatcher_schedule(device);
-			return;
-		}
-
-		/*
-		 * if switching to lower priority make sure that the rptr and
-		 * wptr are equal, when the lower rb is not starved
-		 */
-		if (!adreno_rb_empty(adreno_dev->cur_rb))
-			return;
-	}
-
-	/*
-	 * setup memory to do the switch to highest priority RB
-	 * which is not empty or may be starving away(poor thing)
-	 */
-	a5xx_preemption_start(adreno_dev, highest_busy_rb);
-
-	atomic_set(&dispatcher->preemption_state,
-			ADRENO_DISPATCHER_PREEMPT_TRIGGERED);
-
-	adreno_dev->next_rb = highest_busy_rb;
-	mod_timer(&dispatcher->preempt_timer, jiffies +
-		msecs_to_jiffies(ADRENO_DISPATCH_PREEMPT_TIMEOUT));
-
-	trace_adreno_hw_preempt_clear_to_trig(adreno_dev->cur_rb,
-			adreno_dev->next_rb,
-			adreno_get_rptr(adreno_dev->cur_rb),
-			adreno_get_rptr(adreno_dev->next_rb));
-	/* issue PREEMPT trigger */
-	adreno_writereg(adreno_dev, ADRENO_REG_CP_PREEMPT, 1);
-
-	adreno_dispatcher_schedule(device);
-}
-
-/**
- * a5xx_preempt_complete_state() - Schedule preemption in
- * COMPLETE state
- * @adreno_dev: Device which is in COMPLETE state
- */
-static void a5xx_preempt_complete_state(
-			struct adreno_device *adreno_dev)
-
-{
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct adreno_dispatcher_cmdqueue *dispatch_q;
-	uint64_t rbbase;
-	unsigned int wptr;
-	unsigned int val;
-	static unsigned long wait_for_preemption_complete;
-
-	del_timer_sync(&dispatcher->preempt_timer);
-
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT, &val);
-
-	if (val) {
-		/*
-		 * Wait for 50ms for preemption state to be updated by CP
-		 * before triggering hang
-		 */
-		if (wait_for_preemption_complete == 0)
-			wait_for_preemption_complete = jiffies +
-						msecs_to_jiffies(50);
-		if (time_after(jiffies, wait_for_preemption_complete)) {
-			wait_for_preemption_complete = 0;
-			KGSL_DRV_ERR(device,
-			"Invalid state after preemption CP_PREEMPT:%08x STOP:%1x BUSY:%1x\n",
-					 val, (val & 0x1), (val & 0x10)>>4);
-			adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-		}
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-
-	wait_for_preemption_complete = 0;
-	adreno_readreg64(adreno_dev, ADRENO_REG_CP_RB_BASE,
-				ADRENO_REG_CP_RB_BASE_HI, &rbbase);
-	if (rbbase != adreno_dev->next_rb->buffer_desc.gpuaddr) {
-		KGSL_DRV_ERR(device,
-		"RBBASE incorrect after preemption, expected %016llx got %016llx\b",
-		rbbase,
-		adreno_dev->next_rb->buffer_desc.gpuaddr);
-		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-		adreno_dispatcher_schedule(device);
-		return;
-	}
-
-	dispatch_q = &(adreno_dev->cur_rb->dispatch_q);
-	/* new RB is the current RB */
-	trace_adreno_hw_preempt_comp_to_clear(adreno_dev->next_rb,
-			adreno_dev->cur_rb,
-			adreno_get_rptr(adreno_dev->next_rb),
-			adreno_get_rptr(adreno_dev->cur_rb));
-	adreno_dev->prev_rb = adreno_dev->cur_rb;
-	adreno_dev->cur_rb = adreno_dev->next_rb;
-	adreno_dev->cur_rb->preempted_midway = 0;
-	adreno_dev->cur_rb->wptr_preempt_end = 0xFFFFFFFF;
-	adreno_dev->next_rb = NULL;
-
-	if (adreno_disp_preempt_fair_sched) {
-		/* starved rb is now scheduled so unhalt dispatcher */
-		if (ADRENO_DISPATCHER_RB_STARVE_TIMER_ELAPSED ==
-			adreno_dev->cur_rb->starve_timer_state)
-			adreno_put_gpu_halt(adreno_dev);
-		adreno_dev->cur_rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_SCHEDULED;
-		adreno_dev->cur_rb->sched_timer = jiffies;
-		/*
-		 * If the outgoing RB is has commands then set the
-		 * busy time for it
-		 */
-
-		if (!adreno_rb_empty(adreno_dev->prev_rb)) {
-			adreno_dev->prev_rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_INIT;
-			adreno_dev->prev_rb->sched_timer = jiffies;
-		} else {
-			adreno_dev->prev_rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT;
-		}
-	}
-
-	atomic_set(&dispatcher->preemption_state,
-		ADRENO_DISPATCHER_PREEMPT_CLEAR);
-
-	/* submit wptr if required for new rb */
-	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_WPTR, &wptr);
-	if (adreno_dev->cur_rb->wptr != wptr) {
-		kgsl_pwrscale_busy(device);
-		adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR,
-					adreno_dev->cur_rb->wptr);
-	}
-
-	adreno_preempt_process_dispatch_queue(adreno_dev, dispatch_q);
-}
-
-static void a5xx_preemption_schedule(struct adreno_device *adreno_dev)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-
-	if (!adreno_is_preemption_enabled(adreno_dev))
-		return;
-
-	mutex_lock(&device->mutex);
-
-	/*
-	 * This barrier is needed for most updated preemption_state
-	 * to be read.
-	 */
-	smp_mb();
-
-	switch (atomic_read(&dispatcher->preemption_state)) {
-	case ADRENO_DISPATCHER_PREEMPT_CLEAR:
-		a5xx_preempt_clear_state(adreno_dev);
-		break;
-	case ADRENO_DISPATCHER_PREEMPT_TRIGGERED:
-		a5xx_preempt_trig_state(adreno_dev);
-		/*
-		 * if we transitioned to next state then fall-through
-		 * processing to next state
-		 */
-		if (!adreno_preempt_state(adreno_dev,
-			ADRENO_DISPATCHER_PREEMPT_COMPLETE))
-			break;
-	case ADRENO_DISPATCHER_PREEMPT_COMPLETE:
-		a5xx_preempt_complete_state(adreno_dev);
-		break;
-	default:
-		BUG();
-	}
-
-	mutex_unlock(&device->mutex);
-}
-
 struct adreno_gpudev adreno_a5xx_gpudev = {
 	.reg_offsets = &a5xx_reg_offsets,
 	.ft_perf_counters = a5xx_ft_perf_counters,
diff --git a/drivers/gpu/msm/adreno_a5xx.h b/drivers/gpu/msm/adreno_a5xx.h
index 6ce95ff7bdbf..7965bb7b5440 100644
--- a/drivers/gpu/msm/adreno_a5xx.h
+++ b/drivers/gpu/msm/adreno_a5xx.h
@@ -112,6 +112,8 @@ void a5xx_crashdump_init(struct adreno_device *adreno_dev);
 
 void a5xx_hwcg_set(struct adreno_device *adreno_dev, bool on);
 
+#define A5XX_CP_RB_CNTL_DEFAULT (((ilog2(4) << 8) & 0x1F00) | \
+		(ilog2(KGSL_RB_DWORDS >> 1) & 0x3F))
 /* GPMU interrupt multiplexor */
 #define FW_INTR_INFO			(0)
 #define LLM_ACK_ERR_INTR		(1)
@@ -232,4 +234,22 @@ static inline bool lm_on(struct adreno_device *adreno_dev)
 	return ADRENO_FEATURE(adreno_dev, ADRENO_LM) &&
 		test_bit(ADRENO_LM_CTRL, &adreno_dev->pwrctrl_flag);
 }
+
+/* Preemption functions */
+void a5xx_preemption_trigger(struct adreno_device *adreno_dev);
+void a5xx_preemption_schedule(struct adreno_device *adreno_dev);
+void a5xx_preemption_start(struct adreno_device *adreno_dev);
+int a5xx_preemption_init(struct adreno_device *adreno_dev);
+int a5xx_preemption_yield_enable(unsigned int *cmds);
+
+unsigned int a5xx_preemption_post_ibsubmit(struct adreno_device *adreno_dev,
+		unsigned int *cmds);
+unsigned int a5xx_preemption_pre_ibsubmit(
+			struct adreno_device *adreno_dev,
+			struct adreno_ringbuffer *rb,
+			unsigned int *cmds, struct kgsl_context *context);
+
+
+void a5xx_preempt_callback(struct adreno_device *adreno_dev, int bit);
+
 #endif
diff --git a/drivers/gpu/msm/adreno_a5xx_preempt.c b/drivers/gpu/msm/adreno_a5xx_preempt.c
new file mode 100644
index 000000000000..c1463b824c67
--- /dev/null
+++ b/drivers/gpu/msm/adreno_a5xx_preempt.c
@@ -0,0 +1,574 @@
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ */
+
+#include "adreno.h"
+#include "adreno_a5xx.h"
+#include "a5xx_reg.h"
+#include "adreno_trace.h"
+#include "adreno_pm4types.h"
+
+#define PREEMPT_RECORD(_field) \
+		offsetof(struct a5xx_cp_preemption_record, _field)
+
+#define PREEMPT_SMMU_RECORD(_field) \
+		offsetof(struct a5xx_cp_smmu_info, _field)
+
+static void _update_wptr(struct adreno_device *adreno_dev)
+{
+	struct adreno_ringbuffer *rb = adreno_dev->cur_rb;
+	unsigned int wptr;
+	unsigned long flags;
+
+	spin_lock_irqsave(&rb->preempt_lock, flags);
+
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_RB_WPTR, &wptr);
+
+	if (wptr != rb->wptr) {
+		adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR,
+			rb->wptr);
+
+		rb->dispatch_q.expires = jiffies +
+			msecs_to_jiffies(adreno_cmdbatch_timeout);
+	}
+
+	spin_unlock_irqrestore(&rb->preempt_lock, flags);
+}
+
+static inline bool adreno_move_preempt_state(struct adreno_device *adreno_dev,
+	enum adreno_preempt_states old, enum adreno_preempt_states new)
+{
+	return (atomic_cmpxchg(&adreno_dev->preempt.state, old, new) == old);
+}
+
+static void _a5xx_preemption_done(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned int status;
+
+	/*
+	 * In the very unlikely case that the power is off, do nothing - the
+	 * state will be reset on power up and everybody will be happy
+	 */
+
+	if (!kgsl_state_is_awake(device))
+		return;
+
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT, &status);
+
+	if (status != 0) {
+		KGSL_DRV_ERR(device,
+			"Preemption not complete: status=%X cur=%d R/W=%X/%X next=%d R/W=%X/%X\n",
+			status, adreno_dev->cur_rb->id,
+			adreno_get_rptr(adreno_dev->cur_rb),
+			adreno_dev->cur_rb->wptr, adreno_dev->next_rb->id,
+			adreno_get_rptr(adreno_dev->next_rb),
+			adreno_dev->next_rb->wptr);
+
+		/* Set a fault and restart */
+		adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
+		adreno_dispatcher_schedule(device);
+
+		return;
+	}
+
+	del_timer_sync(&adreno_dev->preempt.timer);
+
+	trace_adreno_preempt_done(adreno_dev->cur_rb, adreno_dev->next_rb);
+
+	/* Clean up all the bits */
+	adreno_dev->prev_rb = adreno_dev->cur_rb;
+	adreno_dev->cur_rb = adreno_dev->next_rb;
+	adreno_dev->next_rb = NULL;
+
+	/* Update the wptr for the new command queue */
+	_update_wptr(adreno_dev);
+
+	/* Update the dispatcher timer for the new command queue */
+	mod_timer(&adreno_dev->dispatcher.timer,
+		adreno_dev->cur_rb->dispatch_q.expires);
+
+	/* Clear the preempt state */
+	adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE);
+}
+
+static void _a5xx_preemption_fault(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	unsigned int status;
+
+	/*
+	 * If the power is on check the preemption status one more time - if it
+	 * was successful then just transition to the complete state
+	 */
+	if (kgsl_state_is_awake(device)) {
+		adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT, &status);
+
+		if (status == 0) {
+			adreno_set_preempt_state(adreno_dev,
+				ADRENO_PREEMPT_COMPLETE);
+
+			adreno_dispatcher_schedule(device);
+			return;
+		}
+	}
+
+	KGSL_DRV_ERR(device,
+		"Preemption timed out: cur=%d R/W=%X/%X, next=%d R/W=%X/%X\n",
+		adreno_dev->cur_rb->id,
+		adreno_get_rptr(adreno_dev->cur_rb), adreno_dev->cur_rb->wptr,
+		adreno_dev->next_rb->id,
+		adreno_get_rptr(adreno_dev->next_rb),
+		adreno_dev->next_rb->wptr);
+
+	adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
+	adreno_dispatcher_schedule(device);
+}
+
+static void _a5xx_preemption_worker(struct work_struct *work)
+{
+	struct adreno_preemption *preempt = container_of(work,
+		struct adreno_preemption, work);
+	struct adreno_device *adreno_dev = container_of(preempt,
+		struct adreno_device, preempt);
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+
+	/* Need to take the mutex to make sure that the power stays on */
+	mutex_lock(&device->mutex);
+
+	if (adreno_in_preempt_state(adreno_dev, ADRENO_PREEMPT_FAULTED))
+		_a5xx_preemption_fault(adreno_dev);
+
+	mutex_unlock(&device->mutex);
+}
+
+static void _a5xx_preemption_timer(unsigned long data)
+{
+	struct adreno_device *adreno_dev = (struct adreno_device *) data;
+
+	/* We should only be here from a triggered state */
+	if (!adreno_move_preempt_state(adreno_dev,
+		ADRENO_PREEMPT_TRIGGERED, ADRENO_PREEMPT_FAULTED))
+		return;
+
+	/* Schedule the worker to take care of the details */
+	queue_work(system_unbound_wq, &adreno_dev->preempt.work);
+}
+
+/* Find the highest priority active ringbuffer */
+static struct adreno_ringbuffer *a5xx_next_ringbuffer(
+		struct adreno_device *adreno_dev)
+{
+	struct adreno_ringbuffer *rb;
+	unsigned long flags;
+	unsigned int i;
+
+	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
+		bool empty;
+
+		spin_lock_irqsave(&rb->preempt_lock, flags);
+		empty = adreno_rb_empty(rb);
+		spin_unlock_irqrestore(&rb->preempt_lock, flags);
+
+		if (empty == false)
+			return rb;
+	}
+
+	return NULL;
+}
+
+void a5xx_preemption_trigger(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct kgsl_iommu *iommu = KGSL_IOMMU_PRIV(device);
+	struct adreno_ringbuffer *next;
+	uint64_t ttbr0;
+	unsigned int contextidr;
+	unsigned long flags;
+
+	/* Put ourselves into a possible trigger state */
+	if (!adreno_move_preempt_state(adreno_dev,
+		ADRENO_PREEMPT_NONE, ADRENO_PREEMPT_START))
+		return;
+
+	/* Get the next ringbuffer to preempt in */
+	next = a5xx_next_ringbuffer(adreno_dev);
+
+	/*
+	 * Nothing to do if every ringbuffer is empty or if the current
+	 * ringbuffer is the only active one
+	 */
+	if (next == NULL || next == adreno_dev->cur_rb) {
+		/*
+		 * Update any critical things that might have been skipped while
+		 * we were looking for a new ringbuffer
+		 */
+
+		if (next != NULL) {
+			_update_wptr(adreno_dev);
+
+			mod_timer(&adreno_dev->dispatcher.timer,
+				adreno_dev->cur_rb->dispatch_q.expires);
+		}
+
+		adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE);
+		return;
+	}
+
+	/* Turn off the dispatcher timer */
+	del_timer(&adreno_dev->dispatcher.timer);
+
+	/*
+	 * This is the most critical section - we need to take care not to race
+	 * until we have programmed the CP for the switch
+	 */
+
+	spin_lock_irqsave(&next->preempt_lock, flags);
+
+	/* Get the pagetable from the pagetable info */
+	kgsl_sharedmem_readq(&next->pagetable_desc, &ttbr0,
+		PT_INFO_OFFSET(ttbr0));
+	kgsl_sharedmem_readl(&next->pagetable_desc, &contextidr,
+		PT_INFO_OFFSET(contextidr));
+
+	kgsl_sharedmem_writel(device, &next->preemption_desc,
+		PREEMPT_RECORD(wptr), next->wptr);
+
+	spin_unlock_irqrestore(&next->preempt_lock, flags);
+
+	/* And write it to the smmu info */
+	kgsl_sharedmem_writeq(device, &iommu->smmu_info,
+		PREEMPT_SMMU_RECORD(ttbr0), ttbr0);
+	kgsl_sharedmem_writel(device, &iommu->smmu_info,
+		PREEMPT_SMMU_RECORD(context_idr), contextidr);
+
+	kgsl_regwrite(device, A5XX_CP_CONTEXT_SWITCH_RESTORE_ADDR_LO,
+		lower_32_bits(next->preemption_desc.gpuaddr));
+	kgsl_regwrite(device, A5XX_CP_CONTEXT_SWITCH_RESTORE_ADDR_HI,
+		upper_32_bits(next->preemption_desc.gpuaddr));
+
+	adreno_dev->next_rb = next;
+
+	/* Start the timer to detect a stuck preemption */
+	mod_timer(&adreno_dev->preempt.timer,
+		jiffies + msecs_to_jiffies(ADRENO_PREEMPT_TIMEOUT));
+
+	trace_adreno_preempt_trigger(adreno_dev->cur_rb, adreno_dev->next_rb);
+
+	adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_TRIGGERED);
+
+	/* Trigger the preemption */
+	adreno_writereg(adreno_dev, ADRENO_REG_CP_PREEMPT, 1);
+}
+
+void a5xx_preempt_callback(struct adreno_device *adreno_dev, int bit)
+{
+	unsigned int status;
+
+	if (!adreno_move_preempt_state(adreno_dev,
+		ADRENO_PREEMPT_TRIGGERED, ADRENO_PREEMPT_PENDING))
+		return;
+
+	adreno_readreg(adreno_dev, ADRENO_REG_CP_PREEMPT, &status);
+
+	if (status != 0) {
+		KGSL_DRV_ERR(KGSL_DEVICE(adreno_dev),
+			"preempt interrupt with non-zero status: %X\n", status);
+
+		/*
+		 * Under the assumption that this is a race between the
+		 * interrupt and the register, schedule the worker to clean up.
+		 * If the status still hasn't resolved itself by the time we get
+		 * there then we have to assume something bad happened
+		 */
+		adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_COMPLETE);
+		adreno_dispatcher_schedule(KGSL_DEVICE(adreno_dev));
+		return;
+	}
+
+	del_timer(&adreno_dev->preempt.timer);
+
+	trace_adreno_preempt_done(adreno_dev->cur_rb,
+		adreno_dev->next_rb);
+
+	adreno_dev->prev_rb = adreno_dev->cur_rb;
+	adreno_dev->cur_rb = adreno_dev->next_rb;
+	adreno_dev->next_rb = NULL;
+
+	/* Update the wptr if it changed while preemption was ongoing */
+	_update_wptr(adreno_dev);
+
+	/* Update the dispatcher timer for the new command queue */
+	mod_timer(&adreno_dev->dispatcher.timer,
+		adreno_dev->cur_rb->dispatch_q.expires);
+
+	adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE);
+}
+
+void a5xx_preemption_schedule(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+
+	if (!adreno_is_preemption_enabled(adreno_dev))
+		return;
+
+	mutex_lock(&device->mutex);
+
+	if (adreno_in_preempt_state(adreno_dev, ADRENO_PREEMPT_COMPLETE))
+		_a5xx_preemption_done(adreno_dev);
+
+	a5xx_preemption_trigger(adreno_dev);
+
+	mutex_unlock(&device->mutex);
+}
+
+unsigned int a5xx_preemption_pre_ibsubmit(
+			struct adreno_device *adreno_dev,
+			struct adreno_ringbuffer *rb,
+			unsigned int *cmds, struct kgsl_context *context)
+{
+	unsigned int *cmds_orig = cmds;
+	uint64_t gpuaddr = rb->preemption_desc.gpuaddr;
+	unsigned int preempt_style = 0;
+
+	if (context) {
+		/*
+		 * Preemption from secure to unsecure needs Zap shader to be
+		 * run to clear all secure content. CP does not know during
+		 * preemption if it is switching between secure and unsecure
+		 * contexts so restrict Secure contexts to be preempted at
+		 * ringbuffer level.
+		 */
+		if (context->flags & KGSL_CONTEXT_SECURE)
+			preempt_style = KGSL_CONTEXT_PREEMPT_STYLE_RINGBUFFER;
+		else
+			preempt_style = ADRENO_PREEMPT_STYLE(context->flags);
+	}
+
+	/*
+	 * CP_PREEMPT_ENABLE_GLOBAL(global preemption) can only be set by KMD
+	 * in ringbuffer.
+	 * 1) set global preemption to 0x0 to disable global preemption.
+	 *    Only RB level preemption is allowed in this mode
+	 * 2) Set global preemption to defer(0x2) for finegrain preemption.
+	 *    when global preemption is set to defer(0x2),
+	 *    CP_PREEMPT_ENABLE_LOCAL(local preemption) determines the
+	 *    preemption point. Local preemption
+	 *    can be enabled by both UMD(within IB) and KMD.
+	 */
+	*cmds++ = cp_type7_packet(CP_PREEMPT_ENABLE_GLOBAL, 1);
+	*cmds++ = ((preempt_style == KGSL_CONTEXT_PREEMPT_STYLE_FINEGRAIN)
+				? 2 : 0);
+
+	/* Turn CP protection OFF */
+	*cmds++ = cp_type7_packet(CP_SET_PROTECTED_MODE, 1);
+	*cmds++ = 0;
+
+	/*
+	 * CP during context switch will save context switch info to
+	 * a5xx_cp_preemption_record pointed by CONTEXT_SWITCH_SAVE_ADDR
+	 */
+	*cmds++ = cp_type4_packet(A5XX_CP_CONTEXT_SWITCH_SAVE_ADDR_LO, 1);
+	*cmds++ = lower_32_bits(gpuaddr);
+	*cmds++ = cp_type4_packet(A5XX_CP_CONTEXT_SWITCH_SAVE_ADDR_HI, 1);
+	*cmds++ = upper_32_bits(gpuaddr);
+
+	/* Turn CP protection ON */
+	*cmds++ = cp_type7_packet(CP_SET_PROTECTED_MODE, 1);
+	*cmds++ = 1;
+
+	/*
+	 * Enable local preemption for finegrain preemption in case of
+	 * a misbehaving IB
+	 */
+	if (preempt_style == KGSL_CONTEXT_PREEMPT_STYLE_FINEGRAIN) {
+		*cmds++ = cp_type7_packet(CP_PREEMPT_ENABLE_LOCAL, 1);
+		*cmds++ = 1;
+	} else {
+		*cmds++ = cp_type7_packet(CP_PREEMPT_ENABLE_LOCAL, 1);
+		*cmds++ = 0;
+	}
+
+	/* Enable CP_CONTEXT_SWITCH_YIELD packets in the IB2s */
+	*cmds++ = cp_type7_packet(CP_YIELD_ENABLE, 1);
+	*cmds++ = 2;
+
+	return (unsigned int) (cmds - cmds_orig);
+}
+
+int a5xx_preemption_yield_enable(unsigned int *cmds)
+{
+	/*
+	 * SRM -- set render mode (ex binning, direct render etc)
+	 * SRM is set by UMD usually at start of IB to tell CP the type of
+	 * preemption.
+	 * KMD needs to set SRM to NULL to indicate CP that rendering is
+	 * done by IB.
+	 */
+	*cmds++ = cp_type7_packet(CP_SET_RENDER_MODE, 5);
+	*cmds++ = 0;
+	*cmds++ = 0;
+	*cmds++ = 0;
+	*cmds++ = 0;
+	*cmds++ = 0;
+
+	*cmds++ = cp_type7_packet(CP_YIELD_ENABLE, 1);
+	*cmds++ = 1;
+
+	return 8;
+}
+
+unsigned int a5xx_preemption_post_ibsubmit(struct adreno_device *adreno_dev,
+	unsigned int *cmds)
+{
+	int dwords = 0;
+
+	cmds[dwords++] = cp_type7_packet(CP_CONTEXT_SWITCH_YIELD, 4);
+	/* Write NULL to the address to skip the data write */
+	dwords += cp_gpuaddr(adreno_dev, &cmds[dwords], 0x0);
+	cmds[dwords++] = 1;
+	/* generate interrupt on preemption completion */
+	cmds[dwords++] = 1;
+
+	return dwords;
+}
+
+void a5xx_preemption_start(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct kgsl_iommu *iommu = KGSL_IOMMU_PRIV(device);
+	struct adreno_ringbuffer *rb;
+	unsigned int i;
+
+	if (!adreno_is_preemption_enabled(adreno_dev))
+		return;
+
+	/* Force the state to be clear */
+	adreno_set_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE);
+
+	kgsl_sharedmem_writel(device, &iommu->smmu_info,
+		PREEMPT_SMMU_RECORD(magic), A5XX_CP_SMMU_INFO_MAGIC_REF);
+	kgsl_sharedmem_writeq(device, &iommu->smmu_info,
+		PREEMPT_SMMU_RECORD(ttbr0), MMU_DEFAULT_TTBR0(device));
+
+	/* The CP doesn't use the asid record, so poison it */
+	kgsl_sharedmem_writel(device, &iommu->smmu_info,
+		PREEMPT_SMMU_RECORD(asid), 0xDECAFBAD);
+	kgsl_sharedmem_writel(device, &iommu->smmu_info,
+		PREEMPT_SMMU_RECORD(context_idr),
+		MMU_DEFAULT_CONTEXTIDR(device));
+
+	adreno_writereg64(adreno_dev,
+			ADRENO_REG_CP_CONTEXT_SWITCH_SMMU_INFO_LO,
+			ADRENO_REG_CP_CONTEXT_SWITCH_SMMU_INFO_HI,
+			iommu->smmu_info.gpuaddr);
+
+	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
+		kgsl_sharedmem_writel(device, &rb->preemption_desc,
+			PREEMPT_RECORD(rptr), 0);
+		kgsl_sharedmem_writel(device, &rb->preemption_desc,
+			PREEMPT_RECORD(wptr), 0);
+
+		adreno_ringbuffer_set_pagetable(rb,
+			device->mmu.defaultpagetable);
+	}
+
+}
+
+static int a5xx_preemption_ringbuffer_init(struct adreno_device *adreno_dev,
+		struct adreno_ringbuffer *rb, uint64_t counteraddr)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	int ret;
+
+	ret = kgsl_allocate_global(device, &rb->preemption_desc,
+		A5XX_CP_CTXRECORD_SIZE_IN_BYTES, 0, KGSL_MEMDESC_PRIVILEGED);
+	if (ret)
+		return ret;
+
+	kgsl_sharedmem_writel(device, &rb->preemption_desc,
+		PREEMPT_RECORD(magic), A5XX_CP_CTXRECORD_MAGIC_REF);
+	kgsl_sharedmem_writel(device, &rb->preemption_desc,
+		PREEMPT_RECORD(info), 0);
+	kgsl_sharedmem_writel(device, &rb->preemption_desc,
+		PREEMPT_RECORD(data), 0);
+	kgsl_sharedmem_writel(device, &rb->preemption_desc,
+		PREEMPT_RECORD(cntl), A5XX_CP_RB_CNTL_DEFAULT);
+	kgsl_sharedmem_writel(device, &rb->preemption_desc,
+		PREEMPT_RECORD(rptr), 0);
+	kgsl_sharedmem_writel(device, &rb->preemption_desc,
+		PREEMPT_RECORD(wptr), 0);
+	kgsl_sharedmem_writeq(device, &rb->preemption_desc,
+		PREEMPT_RECORD(rptr_addr), SCRATCH_RPTR_GPU_ADDR(device,
+			rb->id));
+	kgsl_sharedmem_writeq(device, &rb->preemption_desc,
+		PREEMPT_RECORD(rbase), rb->buffer_desc.gpuaddr);
+	kgsl_sharedmem_writeq(device, &rb->preemption_desc,
+		PREEMPT_RECORD(counter), counteraddr);
+
+	return 0;
+}
+
+#ifdef CONFIG_QCOM_KGSL_IOMMU
+static int a5xx_preemption_iommu_init(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct kgsl_iommu *iommu = KGSL_IOMMU_PRIV(device);
+
+	/* Allocate mem for storing preemption smmu record */
+	return kgsl_allocate_global(device, &iommu->smmu_info, PAGE_SIZE,
+		KGSL_MEMFLAGS_GPUREADONLY, KGSL_MEMDESC_PRIVILEGED);
+}
+#else
+static int a5xx_preemption_iommu_init(struct adreno_device *adreno_dev)
+{
+	return -ENODEV;
+}
+#endif
+
+int a5xx_preemption_init(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct adreno_preemption *preempt = &adreno_dev->preempt;
+	struct adreno_ringbuffer *rb;
+	int ret;
+	unsigned int i;
+	uint64_t addr;
+
+	/* We are dependent on IOMMU to make preemption go on the CP side */
+	if (kgsl_mmu_get_mmutype(device) != KGSL_MMU_TYPE_IOMMU)
+		return -ENODEV;
+
+	INIT_WORK(&preempt->work, _a5xx_preemption_worker);
+
+	setup_timer(&preempt->timer, _a5xx_preemption_timer,
+		(unsigned long) adreno_dev);
+
+	/* Allocate mem for storing preemption counters */
+	ret = kgsl_allocate_global(device, &preempt->counters,
+		adreno_dev->num_ringbuffers *
+		A5XX_CP_CTXRECORD_PREEMPTION_COUNTER_SIZE, 0, 0);
+	if (ret)
+		return ret;
+
+	addr = preempt->counters.gpuaddr;
+
+	/* Allocate mem for storing preemption switch record */
+	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
+		ret = a5xx_preemption_ringbuffer_init(adreno_dev, rb, addr);
+		if (ret)
+			return ret;
+
+		addr += A5XX_CP_CTXRECORD_PREEMPTION_COUNTER_SIZE;
+	}
+
+	return a5xx_preemption_iommu_init(adreno_dev);
+}
diff --git a/drivers/gpu/msm/adreno_debugfs.c b/drivers/gpu/msm/adreno_debugfs.c
index 1a1db3ab3dc9..9cbcd06d7658 100644
--- a/drivers/gpu/msm/adreno_debugfs.c
+++ b/drivers/gpu/msm/adreno_debugfs.c
@@ -226,8 +226,7 @@ static void cmdbatch_print(struct seq_file *s, struct kgsl_cmdbatch *cmdbatch)
 	if (cmdbatch->flags & KGSL_CONTEXT_SYNC)
 		return;
 
-	seq_printf(s, "\t%d: ib: expires: %lu",
-		cmdbatch->timestamp, cmdbatch->expires);
+	seq_printf(s, "\t%d: ", cmdbatch->timestamp);
 
 	seq_puts(s, " flags: ");
 	print_flags(s, cmdbatch_flags, ARRAY_SIZE(cmdbatch_flags),
diff --git a/drivers/gpu/msm/adreno_dispatch.c b/drivers/gpu/msm/adreno_dispatch.c
index a549d0d7f23e..fe92a5d30d90 100644
--- a/drivers/gpu/msm/adreno_dispatch.c
+++ b/drivers/gpu/msm/adreno_dispatch.c
@@ -28,10 +28,10 @@
 #define CMDQUEUE_NEXT(_i, _s) (((_i) + 1) % (_s))
 
 /* Time in ms after which the dispatcher tries to schedule an unscheduled RB */
-static unsigned int _dispatch_starvation_time = 2000;
+unsigned int adreno_dispatch_starvation_time = 2000;
 
 /* Amount of time in ms that a starved RB is permitted to execute for */
-static unsigned int _dispatch_time_slice = 25;
+unsigned int adreno_dispatch_time_slice = 25;
 
 /*
  * If set then dispatcher tries to schedule lower priority RB's after if they
@@ -78,6 +78,24 @@ unsigned int adreno_cmdbatch_timeout = 2000;
 /* Interval for reading and comparing fault detection registers */
 static unsigned int _fault_timer_interval = 200;
 
+#define CMDQUEUE_RB(_cmdqueue) \
+	((struct adreno_ringbuffer *) \
+		container_of((_cmdqueue), struct adreno_ringbuffer, dispatch_q))
+
+#define CMDQUEUE(_ringbuffer) (&(_ringbuffer)->dispatch_q)
+
+static int adreno_dispatch_retire_cmdqueue(struct adreno_device *adreno_dev,
+		struct adreno_dispatcher_cmdqueue *cmdqueue);
+
+static inline bool cmdqueue_is_current(
+		struct adreno_dispatcher_cmdqueue *cmdqueue)
+{
+	struct adreno_ringbuffer *rb = CMDQUEUE_RB(cmdqueue);
+	struct adreno_device *adreno_dev = ADRENO_RB_DEVICE(rb);
+
+	return (adreno_dev->cur_rb == rb);
+}
+
 static void _add_context(struct adreno_device *adreno_dev,
 		struct adreno_context *drawctxt)
 {
@@ -602,11 +620,15 @@ static int sendcmd(struct adreno_device *adreno_dev,
 		dispatch_q->inflight--;
 
 		/*
+		 * Don't log a message in case of:
 		 * -ENOENT means that the context was detached before the
-		 *  command was submitted - don't log a message in that case
+		 * command was submitted
+		 * -ENOSPC means that there temporarily isn't any room in the
+		 *  ringbuffer
+		 *  -PROTO means that a fault is currently being worked
 		 */
 
-		if (ret != -ENOENT)
+		if (ret != -ENOENT && ret != -ENOSPC && ret != -EPROTO)
 			KGSL_DRV_ERR(device,
 				"Unable to submit command to the ringbuffer %d\n",
 				ret);
@@ -627,23 +649,24 @@ static int sendcmd(struct adreno_device *adreno_dev,
 		ADRENO_DISPATCH_CMDQUEUE_SIZE;
 
 	/*
-	 * If this is the first command in the pipe then the GPU will
-	 * immediately start executing it so we can start the expiry timeout on
-	 * the command batch here.  Subsequent command batches will have their
-	 * timer started when the previous command batch is retired.
-	 * Set the timer if the cmdbatch was submitted to current
-	 * active RB else this timer will need to be set when the
-	 * RB becomes active, also if dispatcher is not is CLEAR
-	 * state then the cmdbatch it is currently executing is
-	 * unclear so do not set timer in that case either.
+	 * For the first submission in any given command queue update the
+	 * expected expire time - this won't actually be used / updated until
+	 * the command queue in question goes current, but universally setting
+	 * it here avoids the possibilty of some race conditions with preempt
 	 */
-	if (1 == dispatch_q->inflight &&
-		(&(adreno_dev->cur_rb->dispatch_q)) == dispatch_q &&
-		adreno_preempt_state(adreno_dev,
-			ADRENO_DISPATCHER_PREEMPT_CLEAR)) {
-		cmdbatch->expires = jiffies +
+
+	if (dispatch_q->inflight == 1)
+		dispatch_q->expires = jiffies +
 			msecs_to_jiffies(adreno_cmdbatch_timeout);
-		mod_timer(&dispatcher->timer, cmdbatch->expires);
+
+	/*
+	 * If we believe ourselves to be current and preemption isn't a thing,
+	 * then set up the timer.  If this misses, then preemption is indeed a
+	 * thing and the timer will be set up in due time
+	 */
+	if (!adreno_in_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE)) {
+		if (cmdqueue_is_current(dispatch_q))
+			mod_timer(&dispatcher->timer, dispatch_q->expires);
 	}
 
 
@@ -930,91 +953,6 @@ static int get_timestamp(struct adreno_context *drawctxt,
 }
 
 /**
- * adreno_dispatcher_preempt_timer() - Timer that triggers when preemption has
- * not completed
- * @data: Pointer to adreno device that did not preempt in timely manner
- */
-static void adreno_dispatcher_preempt_timer(unsigned long data)
-{
-	struct adreno_device *adreno_dev = (struct adreno_device *) data;
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
-	unsigned int cur_rptr = adreno_get_rptr(adreno_dev->cur_rb);
-	unsigned int next_rptr = adreno_get_rptr(adreno_dev->cur_rb);
-
-	KGSL_DRV_ERR(device,
-	"Preemption timed out. cur_rb rptr/wptr %x/%x id %d, next_rb rptr/wptr %x/%x id %d, disp_state: %d\n",
-	cur_rptr, adreno_dev->cur_rb->wptr, adreno_dev->cur_rb->id,
-	next_rptr, adreno_dev->next_rb->wptr, adreno_dev->next_rb->id,
-	atomic_read(&dispatcher->preemption_state));
-	adreno_set_gpu_fault(adreno_dev, ADRENO_PREEMPT_FAULT);
-	adreno_dispatcher_schedule(device);
-}
-
-/**
- * adreno_dispatcher_get_highest_busy_rb() - Returns the highest priority RB
- * which is busy
- * @adreno_dev: Device whose RB is returned
- */
-struct adreno_ringbuffer *adreno_dispatcher_get_highest_busy_rb(
-					struct adreno_device *adreno_dev)
-{
-	struct adreno_ringbuffer *rb, *highest_busy_rb = NULL;
-	int i;
-	unsigned int rptr;
-
-	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
-		rptr = adreno_get_rptr(rb);
-		if (rptr != rb->wptr && !highest_busy_rb) {
-			highest_busy_rb = rb;
-			goto done;
-		}
-
-		if (!adreno_disp_preempt_fair_sched)
-			continue;
-
-		switch (rb->starve_timer_state) {
-		case ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT:
-			if (rptr != rb->wptr &&
-				adreno_dev->cur_rb != rb) {
-				rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_INIT;
-				rb->sched_timer = jiffies;
-			}
-			break;
-		case ADRENO_DISPATCHER_RB_STARVE_TIMER_INIT:
-			if (time_after(jiffies, rb->sched_timer +
-				msecs_to_jiffies(_dispatch_starvation_time))) {
-				rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_ELAPSED;
-				/* halt dispatcher to remove starvation */
-				adreno_get_gpu_halt(adreno_dev);
-			}
-			break;
-		case ADRENO_DISPATCHER_RB_STARVE_TIMER_SCHEDULED:
-			BUG_ON(adreno_dev->cur_rb != rb);
-			/*
-			 * If the RB has not been running for the minimum
-			 * time slice then allow it to run
-			 */
-			if ((rptr != rb->wptr) && time_before(jiffies,
-				adreno_dev->cur_rb->sched_timer +
-				msecs_to_jiffies(_dispatch_time_slice)))
-				highest_busy_rb = rb;
-			else
-				rb->starve_timer_state =
-				ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT;
-			break;
-		case ADRENO_DISPATCHER_RB_STARVE_TIMER_ELAPSED:
-		default:
-			break;
-		}
-	}
-done:
-	return highest_busy_rb;
-}
-
-/**
  * adreno_dispactcher_queue_cmd() - Queue a new command in the context
  * @adreno_dev: Pointer to the adreno device struct
  * @drawctxt: Pointer to the adreno draw context
@@ -1797,7 +1735,7 @@ static int dispatcher_do_fault(struct adreno_device *adreno_dev)
 	/* Turn off all the timers */
 	del_timer_sync(&dispatcher->timer);
 	del_timer_sync(&dispatcher->fault_timer);
-	del_timer_sync(&dispatcher->preempt_timer);
+	del_timer_sync(&adreno_dev->preempt.timer);
 
 	mutex_lock(&device->mutex);
 
@@ -1823,8 +1761,8 @@ static int dispatcher_do_fault(struct adreno_device *adreno_dev)
 	 * retire cmdbatches from all the dispatch_q's before starting recovery
 	 */
 	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
-		adreno_dispatch_process_cmdqueue(adreno_dev,
-			&(rb->dispatch_q), 0);
+		adreno_dispatch_retire_cmdqueue(adreno_dev,
+			&(rb->dispatch_q));
 		/* Select the active dispatch_q */
 		if (base == rb->buffer_desc.gpuaddr) {
 			dispatch_q = &(rb->dispatch_q);
@@ -1842,7 +1780,7 @@ static int dispatcher_do_fault(struct adreno_device *adreno_dev)
 		}
 	}
 
-	if (dispatch_q && (dispatch_q->tail != dispatch_q->head)) {
+	if (!adreno_cmdqueue_is_empty(dispatch_q)) {
 		cmdbatch = dispatch_q->cmd_q[dispatch_q->head];
 		trace_adreno_cmdbatch_fault(cmdbatch, fault);
 	}
@@ -1868,8 +1806,6 @@ static int dispatcher_do_fault(struct adreno_device *adreno_dev)
 
 	/* Reset the dispatcher queue */
 	dispatcher->inflight = 0;
-	atomic_set(&dispatcher->preemption_state,
-		ADRENO_DISPATCHER_PREEMPT_CLEAR);
 
 	/* Reset the GPU and make sure halt is not set during recovery */
 	halt = adreno_gpu_halt(adreno_dev);
@@ -1961,140 +1897,170 @@ static void cmdbatch_profile_ticks(struct adreno_device *adreno_dev,
 	*retire = entry->retired;
 }
 
-int adreno_dispatch_process_cmdqueue(struct adreno_device *adreno_dev,
-				struct adreno_dispatcher_cmdqueue *dispatch_q,
-				int long_ib_detect)
+static void retire_cmdbatch(struct adreno_device *adreno_dev,
+		struct kgsl_cmdbatch *cmdbatch)
 {
-	struct adreno_dispatcher *dispatcher = &(adreno_dev->dispatcher);
-	uint64_t start_ticks = 0, retire_ticks = 0;
+	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
+	struct adreno_context *drawctxt = ADRENO_CONTEXT(cmdbatch->context);
+	uint64_t start = 0, end = 0;
 
-	struct adreno_dispatcher_cmdqueue *active_q =
-			&(adreno_dev->cur_rb->dispatch_q);
+	if (cmdbatch->fault_recovery != 0) {
+		set_bit(ADRENO_CONTEXT_FAULT, &cmdbatch->context->priv);
+		_print_recovery(KGSL_DEVICE(adreno_dev), cmdbatch);
+	}
+
+	if (test_bit(CMDBATCH_FLAG_PROFILE, &cmdbatch->priv))
+		cmdbatch_profile_ticks(adreno_dev, cmdbatch, &start, &end);
+
+	trace_adreno_cmdbatch_retired(cmdbatch, (int) dispatcher->inflight,
+		start, end, ADRENO_CMDBATCH_RB(cmdbatch),
+		adreno_get_rptr(drawctxt->rb));
+
+	drawctxt->submit_retire_ticks[drawctxt->ticks_index] =
+		end - cmdbatch->submit_ticks;
+
+	drawctxt->ticks_index = (drawctxt->ticks_index + 1) %
+		SUBMIT_RETIRE_TICKS_SIZE;
+
+	kgsl_cmdbatch_destroy(cmdbatch);
+}
+
+static int adreno_dispatch_retire_cmdqueue(struct adreno_device *adreno_dev,
+		struct adreno_dispatcher_cmdqueue *cmdqueue)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
 	int count = 0;
 
-	while (dispatch_q->head != dispatch_q->tail) {
+	while (!adreno_cmdqueue_is_empty(cmdqueue)) {
 		struct kgsl_cmdbatch *cmdbatch =
-			dispatch_q->cmd_q[dispatch_q->head];
-		struct adreno_context *drawctxt;
-		BUG_ON(cmdbatch == NULL);
+			cmdqueue->cmd_q[cmdqueue->head];
 
-		drawctxt = ADRENO_CONTEXT(cmdbatch->context);
+		if (!kgsl_check_timestamp(device, cmdbatch->context,
+			cmdbatch->timestamp))
+			break;
 
-		/*
-		 * First try to expire the timestamp. This happens if the
-		 * context is valid and the timestamp expired normally or if the
-		 * context was destroyed before the command batch was finished
-		 * in the GPU.  Either way retire the command batch advance the
-		 * pointers and continue processing the queue
-		 */
+		retire_cmdbatch(adreno_dev, cmdbatch);
 
-		if (kgsl_check_timestamp(KGSL_DEVICE(adreno_dev),
-			cmdbatch->context, cmdbatch->timestamp)) {
+		dispatcher->inflight--;
+		cmdqueue->inflight--;
 
-			/*
-			 * If the cmdbatch in question had faulted announce its
-			 * successful completion to the world
-			 */
+		cmdqueue->cmd_q[cmdqueue->head] = NULL;
 
-			if (cmdbatch->fault_recovery != 0) {
-				/* Mark the context as faulted and recovered */
-				set_bit(ADRENO_CONTEXT_FAULT,
-					&cmdbatch->context->priv);
+		cmdqueue->head = CMDQUEUE_NEXT(cmdqueue->head,
+			ADRENO_DISPATCH_CMDQUEUE_SIZE);
 
-				_print_recovery(KGSL_DEVICE(adreno_dev),
-					cmdbatch);
-			}
+		count++;
+	}
 
-			/* Reduce the number of inflight command batches */
-			dispatcher->inflight--;
-			dispatch_q->inflight--;
+	return count;
+}
 
-			/*
-			 * If kernel profiling is enabled get the submit and
-			 * retired ticks from the buffer
-			 */
+static void _adreno_dispatch_check_timeout(struct adreno_device *adreno_dev,
+		struct adreno_dispatcher_cmdqueue *cmdqueue)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct kgsl_cmdbatch *cmdbatch = cmdqueue->cmd_q[cmdqueue->head];
 
-			if (test_bit(CMDBATCH_FLAG_PROFILE, &cmdbatch->priv))
-				cmdbatch_profile_ticks(adreno_dev, cmdbatch,
-					&start_ticks, &retire_ticks);
+	/* Don't timeout if the timer hasn't expired yet (duh) */
+	if (time_is_after_jiffies(cmdqueue->expires))
+		return;
 
-			trace_adreno_cmdbatch_retired(cmdbatch,
-				(int) dispatcher->inflight, start_ticks,
-				retire_ticks, ADRENO_CMDBATCH_RB(cmdbatch),
-				adreno_get_rptr(drawctxt->rb));
+	/* Don't timeout if the IB timeout is disabled globally */
+	if (!adreno_long_ib_detect(adreno_dev))
+		return;
 
-			/* Record the delta between submit and retire ticks */
-			drawctxt->submit_retire_ticks[drawctxt->ticks_index] =
-				retire_ticks - cmdbatch->submit_ticks;
+	/* Don't time out if the context has disabled it */
+	if (cmdbatch->context->flags & KGSL_CONTEXT_NO_FAULT_TOLERANCE)
+		return;
 
-			drawctxt->ticks_index = (drawctxt->ticks_index + 1)
-				% SUBMIT_RETIRE_TICKS_SIZE;
+	pr_context(device, cmdbatch->context, "gpu timeout ctx %d ts %d\n",
+		cmdbatch->context->id, cmdbatch->timestamp);
 
-			/* Zero the old entry*/
-			dispatch_q->cmd_q[dispatch_q->head] = NULL;
+	adreno_set_gpu_fault(adreno_dev, ADRENO_TIMEOUT_FAULT);
+}
 
-			/* Advance the buffer head */
-			dispatch_q->head = CMDQUEUE_NEXT(dispatch_q->head,
-				ADRENO_DISPATCH_CMDQUEUE_SIZE);
+static int adreno_dispatch_process_cmdqueue(struct adreno_device *adreno_dev,
+		struct adreno_dispatcher_cmdqueue *cmdqueue)
+{
+	int count = adreno_dispatch_retire_cmdqueue(adreno_dev, cmdqueue);
 
-			/* Destroy the retired command batch */
-			kgsl_cmdbatch_destroy(cmdbatch);
+	/* Nothing to do if there are no pending commands */
+	if (adreno_cmdqueue_is_empty(cmdqueue))
+		return count;
 
-			/* Update the expire time for the next command batch */
+	/* Don't update the cmdqueue timeout if we are about to preempt out */
+	if (!adreno_in_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE))
+		return count;
 
-			if (dispatch_q->inflight > 0 &&
-				dispatch_q == active_q) {
-				cmdbatch =
-					dispatch_q->cmd_q[dispatch_q->head];
-				cmdbatch->expires = jiffies +
-					msecs_to_jiffies(
-					adreno_cmdbatch_timeout);
-			}
+	/* Don't update the cmdqueue timeout if it isn't active */
+	if (!cmdqueue_is_current(cmdqueue))
+		return count;
 
-			count++;
-			continue;
-		}
-		/*
-		 * Break here if fault detection is disabled for the context or
-		 * if the long running IB detection is disaled device wide or
-		 * if the dispatch q is not active
-		 * Long running command buffers will be allowed to run to
-		 * completion - but badly behaving command buffers (infinite
-		 * shaders etc) can end up running forever.
-		 */
+	/*
+	 * If the current ringbuffer retired any commands then universally
+	 * reset the timeout
+	 */
 
-		if (!long_ib_detect ||
-			drawctxt->base.flags & KGSL_CONTEXT_NO_FAULT_TOLERANCE
-			|| dispatch_q != active_q)
-			break;
+	if (count) {
+		cmdqueue->expires = jiffies +
+			msecs_to_jiffies(adreno_cmdbatch_timeout);
+		return count;
+	}
 
-		/*
-		 * The last line of defense is to check if the command batch has
-		 * timed out. If we get this far but the timeout hasn't expired
-		 * yet then the GPU is still ticking away
-		 */
+	/*
+	 * If we get here then 1) the ringbuffer is current and 2) we haven't
+	 * retired anything.  Check to see if the timeout if valid for the
+	 * current cmdbatch and fault if it has expired
+	 */
+	_adreno_dispatch_check_timeout(adreno_dev, cmdqueue);
+	return 0;
+}
 
-		if (time_is_after_jiffies(cmdbatch->expires))
-			break;
+/* Update the dispatcher timers */
+static void _dispatcher_update_timers(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
 
-		/* Boom goes the dynamite */
+	/* Kick the idle timer */
+	mutex_lock(&device->mutex);
+	kgsl_pwrscale_update(device);
+	mod_timer(&device->idle_timer,
+		jiffies + device->pwrctrl.interval_timeout);
+	mutex_unlock(&device->mutex);
 
-		pr_context(KGSL_DEVICE(adreno_dev), cmdbatch->context,
-			"gpu timeout ctx %d ts %d\n",
-			cmdbatch->context->id, cmdbatch->timestamp);
+	/* Check to see if we need to update the command timer */
+	if (adreno_in_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE)) {
+		struct adreno_dispatcher_cmdqueue *cmdqueue =
+			CMDQUEUE(adreno_dev->cur_rb);
 
-		adreno_set_gpu_fault(adreno_dev, ADRENO_TIMEOUT_FAULT);
-		break;
+		if (!adreno_cmdqueue_is_empty(cmdqueue))
+			mod_timer(&dispatcher->timer, cmdqueue->expires);
 	}
-	return count;
 }
 
-/**
- * adreno_dispatcher_work() - Master work handler for the dispatcher
- * @work: Pointer to the work struct for the current work queue
- *
- * Process expired commands and send new ones.
- */
+/* Take down the dispatcher and release any power states */
+static void _dispatcher_power_down(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct adreno_dispatcher *dispatcher = &adreno_dev->dispatcher;
+
+	mutex_lock(&device->mutex);
+
+	if (test_and_clear_bit(ADRENO_DISPATCHER_ACTIVE, &dispatcher->priv))
+		complete_all(&dispatcher->idle_gate);
+
+	del_timer_sync(&dispatcher->fault_timer);
+
+	if (test_bit(ADRENO_DISPATCHER_POWER, &dispatcher->priv)) {
+		kgsl_active_count_put(device);
+		clear_bit(ADRENO_DISPATCHER_POWER, &dispatcher->priv);
+	}
+
+	mutex_unlock(&device->mutex);
+}
+
 static void adreno_dispatcher_work(struct work_struct *work)
 {
 	struct adreno_dispatcher *dispatcher =
@@ -2104,95 +2070,50 @@ static void adreno_dispatcher_work(struct work_struct *work)
 	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
 	struct adreno_gpudev *gpudev = ADRENO_GPU_DEVICE(adreno_dev);
 	int count = 0;
-	int cur_rb_id = adreno_dev->cur_rb->id;
+	unsigned int i = 0;
 
 	mutex_lock(&dispatcher->mutex);
 
-	if (ADRENO_DISPATCHER_PREEMPT_CLEAR ==
-		atomic_read(&dispatcher->preemption_state))
-		/* process the active q*/
-		count = adreno_dispatch_process_cmdqueue(adreno_dev,
-			&(adreno_dev->cur_rb->dispatch_q),
-			adreno_long_ib_detect(adreno_dev));
-
-	else if (ADRENO_DISPATCHER_PREEMPT_TRIGGERED ==
-		atomic_read(&dispatcher->preemption_state))
-		count = adreno_dispatch_process_cmdqueue(adreno_dev,
-			&(adreno_dev->cur_rb->dispatch_q), 0);
-
-	/* Check if gpu fault occurred */
-	if (dispatcher_do_fault(adreno_dev))
-		goto done;
-
-	if (gpudev->preemption_schedule)
-		gpudev->preemption_schedule(adreno_dev);
-
-	if (cur_rb_id != adreno_dev->cur_rb->id) {
-		struct adreno_dispatcher_cmdqueue *dispatch_q =
-			&(adreno_dev->cur_rb->dispatch_q);
-		/* active level switched, clear new level cmdbatches */
-		count = adreno_dispatch_process_cmdqueue(adreno_dev,
-			dispatch_q,
-			adreno_long_ib_detect(adreno_dev));
-		/*
-		 * If GPU has already completed all the commands in new incoming
-		 * RB then we may not get another interrupt due to which
-		 * dispatcher may not run again. Schedule dispatcher here so
-		 * we can come back and process the other RB's if required
-		 */
-		if (dispatch_q->head == dispatch_q->tail)
-			adreno_dispatcher_schedule(device);
-	}
 	/*
-	 * If inflight went to 0, queue back up the event processor to catch
-	 * stragglers
+	 * As long as there are inflight commands, process retired comamnds from
+	 * all cmdqueues
 	 */
-	if (dispatcher->inflight == 0 && count)
-		kgsl_schedule_work(&device->event_work);
-
-	/* Try to dispatch new commands */
-	_adreno_dispatcher_issuecmds(adreno_dev);
-
-done:
-	/* Either update the timer for the next command batch or disable it */
-	if (dispatcher->inflight) {
-		struct kgsl_cmdbatch *cmdbatch =
-			adreno_dev->cur_rb->dispatch_q.cmd_q[
-				adreno_dev->cur_rb->dispatch_q.head];
-		if (cmdbatch && adreno_preempt_state(adreno_dev,
-					ADRENO_DISPATCHER_PREEMPT_CLEAR))
-			/* Update the timeout timer for the next cmdbatch */
-			mod_timer(&dispatcher->timer, cmdbatch->expires);
-
-		/* There are still things in flight - update the idle counts */
-		mutex_lock(&device->mutex);
-		kgsl_pwrscale_update(device);
-		mod_timer(&device->idle_timer, jiffies +
-				device->pwrctrl.interval_timeout);
-		mutex_unlock(&device->mutex);
-	} else {
-		/* There is nothing left in the pipeline.  Shut 'er down boys */
-		mutex_lock(&device->mutex);
+	for (i = 0; i < adreno_dev->num_ringbuffers; i++) {
+		struct adreno_dispatcher_cmdqueue *cmdqueue =
+			CMDQUEUE(&adreno_dev->ringbuffers[i]);
 
-		if (test_and_clear_bit(ADRENO_DISPATCHER_ACTIVE,
-			&dispatcher->priv))
-			complete_all(&dispatcher->idle_gate);
+		count += adreno_dispatch_process_cmdqueue(adreno_dev,
+			cmdqueue);
+		if (dispatcher->inflight == 0)
+			break;
+	}
 
-		/*
-		 * Stop the fault timer before decrementing the active count to
-		 * avoid reading the hardware registers while we are trying to
-		 * turn clocks off
-		 */
-		del_timer_sync(&dispatcher->fault_timer);
+	/*
+	 * dispatcher_do_fault() returns 0 if no faults occurred. If that is the
+	 * case, then clean up preemption and try to schedule more work
+	 */
+	if (dispatcher_do_fault(adreno_dev) == 0) {
+		/* Clean up after preemption */
+		if (gpudev->preemption_schedule)
+			gpudev->preemption_schedule(adreno_dev);
 
-		if (test_bit(ADRENO_DISPATCHER_POWER, &dispatcher->priv)) {
-			kgsl_active_count_put(device);
-			clear_bit(ADRENO_DISPATCHER_POWER, &dispatcher->priv);
-		}
+		/* Re-kick the event engine to catch stragglers */
+		if (dispatcher->inflight == 0 && count != 0)
+			kgsl_schedule_work(&device->event_work);
 
-		mutex_unlock(&device->mutex);
+		/* Run the scheduler for to dispatch new commands */
+		_adreno_dispatcher_issuecmds(adreno_dev);
 	}
 
+	/*
+	 * If there are commands pending, update the timers, otherwise release
+	 * the power state to prepare for power down
+	 */
+	if (dispatcher->inflight > 0)
+		_dispatcher_update_timers(adreno_dev);
+	else
+		_dispatcher_power_down(adreno_dev);
+
 	mutex_unlock(&dispatcher->mutex);
 }
 
@@ -2314,7 +2235,7 @@ void adreno_dispatcher_close(struct adreno_device *adreno_dev)
 	FOR_EACH_RINGBUFFER(adreno_dev, rb, i) {
 		struct adreno_dispatcher_cmdqueue *dispatch_q =
 			&(rb->dispatch_q);
-		while (dispatch_q->head != dispatch_q->tail) {
+		while (!adreno_cmdqueue_is_empty(dispatch_q)) {
 			kgsl_cmdbatch_destroy(
 				dispatch_q->cmd_q[dispatch_q->head]);
 			dispatch_q->head = (dispatch_q->head + 1)
@@ -2404,9 +2325,9 @@ static DISPATCHER_UINT_ATTR(fault_throttle_burst, 0644, 0,
 static DISPATCHER_UINT_ATTR(disp_preempt_fair_sched, 0644, 0,
 	adreno_disp_preempt_fair_sched);
 static DISPATCHER_UINT_ATTR(dispatch_time_slice, 0644, 0,
-	_dispatch_time_slice);
+	adreno_dispatch_time_slice);
 static DISPATCHER_UINT_ATTR(dispatch_starvation_time, 0644, 0,
-	_dispatch_starvation_time);
+	adreno_dispatch_starvation_time);
 
 static struct attribute *dispatcher_attrs[] = {
 	&dispatcher_attr_inflight.attr,
@@ -2483,9 +2404,6 @@ int adreno_dispatcher_init(struct adreno_device *adreno_dev)
 	setup_timer(&dispatcher->fault_timer, adreno_dispatcher_fault_timer,
 		(unsigned long) adreno_dev);
 
-	setup_timer(&dispatcher->preempt_timer, adreno_dispatcher_preempt_timer,
-		(unsigned long) adreno_dev);
-
 	INIT_WORK(&dispatcher->work, adreno_dispatcher_work);
 
 	init_completion(&dispatcher->idle_gate);
@@ -2494,9 +2412,6 @@ int adreno_dispatcher_init(struct adreno_device *adreno_dev)
 	plist_head_init(&dispatcher->pending);
 	spin_lock_init(&dispatcher->plist_lock);
 
-	atomic_set(&dispatcher->preemption_state,
-		ADRENO_DISPATCHER_PREEMPT_CLEAR);
-
 	ret = kobject_init_and_add(&dispatcher->kobj, &ktype_dispatcher,
 		&device->dev->kobj, "dispatch");
 
@@ -2553,51 +2468,3 @@ int adreno_dispatcher_idle(struct adreno_device *adreno_dev)
 	adreno_dispatcher_schedule(device);
 	return ret;
 }
-
-void adreno_preempt_process_dispatch_queue(struct adreno_device *adreno_dev,
-	struct adreno_dispatcher_cmdqueue *dispatch_q)
-{
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
-	struct kgsl_cmdbatch *cmdbatch;
-
-	if (dispatch_q->head != dispatch_q->tail) {
-		/*
-		 * retire cmdbacthes from previous q, and don't check for
-		 * timeout since the cmdbatch may have been preempted
-		 */
-		adreno_dispatch_process_cmdqueue(adreno_dev,
-							dispatch_q, 0);
-	}
-
-	/* set the timer for the first cmdbatch of active dispatch_q */
-	dispatch_q = &(adreno_dev->cur_rb->dispatch_q);
-	if (dispatch_q->head != dispatch_q->tail) {
-		cmdbatch = dispatch_q->cmd_q[dispatch_q->head];
-		cmdbatch->expires = jiffies +
-			msecs_to_jiffies(adreno_cmdbatch_timeout);
-	}
-	kgsl_schedule_work(&device->event_work);
-}
-
-/**
- * adreno_dispatcher_preempt_callback() - Callback funcion for CP_SW interrupt
- * @adreno_dev: The device on which the interrupt occurred
- * @bit: Interrupt bit in the interrupt status register
- */
-void adreno_dispatcher_preempt_callback(struct adreno_device *adreno_dev,
-					int bit)
-{
-	struct adreno_dispatcher *dispatcher = &(adreno_dev->dispatcher);
-
-	if (ADRENO_DISPATCHER_PREEMPT_TRIGGERED !=
-			atomic_read(&dispatcher->preemption_state))
-		return;
-
-	trace_adreno_hw_preempt_trig_to_comp_int(adreno_dev->cur_rb,
-			      adreno_dev->next_rb,
-			      adreno_get_rptr(adreno_dev->cur_rb),
-			      adreno_get_rptr(adreno_dev->next_rb));
-	atomic_set(&dispatcher->preemption_state,
-			ADRENO_DISPATCHER_PREEMPT_COMPLETE);
-	adreno_dispatcher_schedule(KGSL_DEVICE(adreno_dev));
-}
diff --git a/drivers/gpu/msm/adreno_dispatch.h b/drivers/gpu/msm/adreno_dispatch.h
index 308d5b936819..699c3e4adb27 100644
--- a/drivers/gpu/msm/adreno_dispatch.h
+++ b/drivers/gpu/msm/adreno_dispatch.h
@@ -11,29 +11,13 @@
  *
  */
 
-
 #ifndef ____ADRENO_DISPATCHER_H
 #define ____ADRENO_DISPATCHER_H
 
-/* Time to allow preemption to complete (in ms) */
-#define ADRENO_DISPATCH_PREEMPT_TIMEOUT 10000
-
 extern unsigned int adreno_disp_preempt_fair_sched;
 extern unsigned int adreno_cmdbatch_timeout;
-
-/**
- * enum adreno_dispatcher_preempt_states - States of dispatcher for ringbuffer
- * preemption
- * @ADRENO_DISPATCHER_PREEMPT_CLEAR: No preemption is underway,
- * only 1 preemption can be underway at any point
- * @ADRENO_DISPATCHER_PREEMPT_TRIGGERED: A preemption is underway
- * @ADRENO_DISPATCHER_PREEMPT_COMPLETE: A preemption has just completed
- */
-enum adreno_dispatcher_preempt_states {
-	ADRENO_DISPATCHER_PREEMPT_CLEAR = 0,
-	ADRENO_DISPATCHER_PREEMPT_TRIGGERED,
-	ADRENO_DISPATCHER_PREEMPT_COMPLETE,
-};
+extern unsigned int adreno_dispatch_starvation_time;
+extern unsigned int adreno_dispatch_time_slice;
 
 /**
  * enum adreno_dispatcher_starve_timer_states - Starvation control states of
@@ -71,6 +55,7 @@ enum adreno_dispatcher_starve_timer_states {
  * @head: Head pointer to the q
  * @tail: Queues tail pointer
  * @active_context_count: Number of active contexts seen in this rb cmdqueue
+ * @expires: The jiffies value at which this cmdqueue has run too long
  */
 struct adreno_dispatcher_cmdqueue {
 	struct kgsl_cmdbatch *cmd_q[ADRENO_DISPATCH_CMDQUEUE_SIZE];
@@ -78,6 +63,7 @@ struct adreno_dispatcher_cmdqueue {
 	unsigned int head;
 	unsigned int tail;
 	int active_context_count;
+	unsigned long expires;
 };
 
 /**
@@ -92,11 +78,6 @@ struct adreno_dispatcher_cmdqueue {
  * @work: work_struct to put the dispatcher in a work queue
  * @kobj: kobject for the dispatcher directory in the device sysfs node
  * @idle_gate: Gate to wait on for dispatcher to idle
- * @preemption_state: Indicated what state the dispatcher is in, states are
- * defined by enum adreno_dispatcher_preempt_states
- * @preempt_token_submit: Indicates if a preempt token has been subnitted in
- * current ringbuffer.
- * @preempt_timer: Timer to track if preemption occured within specified time
  * @disp_preempt_fair_sched: If set then dispatcher will try to be fair to
  * starving RB's by scheduling them in and enforcing a minimum time slice
  * for every RB that is scheduled to run on the device
@@ -113,9 +94,6 @@ struct adreno_dispatcher {
 	struct work_struct work;
 	struct kobject kobj;
 	struct completion idle_gate;
-	atomic_t preemption_state;
-	int preempt_token_submit;
-	struct timer_list preempt_timer;
 	unsigned int disp_preempt_fair_sched;
 };
 
@@ -141,12 +119,12 @@ void adreno_dispatcher_queue_context(struct kgsl_device *device,
 		struct adreno_context *drawctxt);
 void adreno_dispatcher_preempt_callback(struct adreno_device *adreno_dev,
 					int bit);
-struct adreno_ringbuffer *adreno_dispatcher_get_highest_busy_rb(
-					struct adreno_device *adreno_dev);
-int adreno_dispatch_process_cmdqueue(struct adreno_device *adreno_dev,
-				struct adreno_dispatcher_cmdqueue *dispatch_q,
-				int long_ib_detect);
 void adreno_preempt_process_dispatch_queue(struct adreno_device *adreno_dev,
 	struct adreno_dispatcher_cmdqueue *dispatch_q);
 
+static inline bool adreno_cmdqueue_is_empty(
+		struct adreno_dispatcher_cmdqueue *cmdqueue)
+{
+	return (cmdqueue != NULL && cmdqueue->head == cmdqueue->tail);
+}
 #endif /* __ADRENO_DISPATCHER_H */
diff --git a/drivers/gpu/msm/adreno_drawctxt.c b/drivers/gpu/msm/adreno_drawctxt.c
index d8498d938b6a..54ae1f022367 100644
--- a/drivers/gpu/msm/adreno_drawctxt.c
+++ b/drivers/gpu/msm/adreno_drawctxt.c
@@ -471,12 +471,25 @@ void adreno_drawctxt_detach(struct kgsl_context *context)
 	if (rb->drawctxt_active == drawctxt) {
 		if (adreno_dev->cur_rb == rb) {
 			if (!kgsl_active_count_get(device)) {
-				adreno_drawctxt_switch(adreno_dev, rb, NULL, 0);
+				ret = adreno_drawctxt_switch(adreno_dev, rb,
+					NULL, 0);
 				kgsl_active_count_put(device);
 			} else
 				BUG();
 		} else
-			adreno_drawctxt_switch(adreno_dev, rb, NULL, 0);
+			ret = adreno_drawctxt_switch(adreno_dev, rb, NULL, 0);
+
+		if (ret != 0) {
+			KGSL_DRV_ERR(device,
+				"Unable to switch the context to NULL: %d\n",
+				ret);
+		}
+
+		/*
+		 * Keep going ahead if we can't switch the context - failure
+		 * isn't always fatal, but sometimes it is. I like those
+		 * chances!
+		 */
 	}
 	mutex_unlock(&device->mutex);
 
@@ -597,11 +610,8 @@ int adreno_drawctxt_switch(struct adreno_device *adreno_dev,
 		new_pt = device->mmu.defaultpagetable;
 	}
 	ret = adreno_ringbuffer_set_pt_ctx(rb, new_pt, drawctxt);
-	if (ret) {
-		KGSL_DRV_ERR(device,
-			"Failed to set pagetable on rb %d\n", rb->id);
+	if (ret)
 		return ret;
-	}
 
 	/* Put the old instance of the active drawctxt */
 	if (rb->drawctxt_active)
diff --git a/drivers/gpu/msm/adreno_ioctl.c b/drivers/gpu/msm/adreno_ioctl.c
index 519087a77b83..0d5e3e094c36 100644
--- a/drivers/gpu/msm/adreno_ioctl.c
+++ b/drivers/gpu/msm/adreno_ioctl.c
@@ -103,7 +103,7 @@ static long adreno_ioctl_preemption_counters_query(
 		levels_to_copy = gpudev->num_prio_levels;
 
 	if (copy_to_user((void __user *) (uintptr_t) read->counters,
-			adreno_dev->preemption_counters.hostptr,
+			adreno_dev->preempt.counters.hostptr,
 			levels_to_copy * size_level))
 		return -EFAULT;
 
diff --git a/drivers/gpu/msm/adreno_iommu.c b/drivers/gpu/msm/adreno_iommu.c
index f44b1cda96bf..d3f96a227d4a 100644
--- a/drivers/gpu/msm/adreno_iommu.c
+++ b/drivers/gpu/msm/adreno_iommu.c
@@ -361,8 +361,7 @@ static unsigned int _adreno_mmu_set_pt_update_condition(
 	 */
 	*cmds++ = cp_mem_packet(adreno_dev, CP_MEM_WRITE, 2, 1);
 	cmds += cp_gpuaddr(adreno_dev, cmds, (rb->pagetable_desc.gpuaddr +
-		offsetof(struct adreno_ringbuffer_pagetable_info,
-		switch_pt_enable)));
+		PT_INFO_OFFSET(switch_pt_enable)));
 	*cmds++ = 1;
 	*cmds++ = cp_packet(adreno_dev, CP_WAIT_MEM_WRITES, 1);
 	*cmds++ = 0;
@@ -376,14 +375,11 @@ static unsigned int _adreno_mmu_set_pt_update_condition(
 	*cmds++ = (1 << 8) | (1 << 4) | 3;
 	cmds += cp_gpuaddr(adreno_dev, cmds,
 	   (adreno_dev->ringbuffers[0].pagetable_desc.gpuaddr +
-		   offsetof(struct adreno_ringbuffer_pagetable_info,
-		   current_global_ptname)));
+		PT_INFO_OFFSET(current_global_ptname)));
 	*cmds++ = ptname;
 	*cmds++ = 0xFFFFFFFF;
-	cmds += cp_gpuaddr(adreno_dev, cmds,
-		   (rb->pagetable_desc.gpuaddr +
-		   offsetof(struct adreno_ringbuffer_pagetable_info,
-		   switch_pt_enable)));
+	cmds += cp_gpuaddr(adreno_dev, cmds, (rb->pagetable_desc.gpuaddr +
+		PT_INFO_OFFSET(switch_pt_enable)));
 	*cmds++ = 0;
 	*cmds++ = cp_packet(adreno_dev, CP_WAIT_MEM_WRITES, 1);
 	*cmds++ = 0;
@@ -407,23 +403,18 @@ static unsigned int _adreno_iommu_pt_update_pid_to_mem(
 	unsigned int *cmds_orig = cmds;
 
 	*cmds++ = cp_mem_packet(adreno_dev, CP_MEM_WRITE, 2, 1);
-	cmds += cp_gpuaddr(adreno_dev, cmds,
-		  (rb->pagetable_desc.gpuaddr +
-		  offsetof(struct adreno_ringbuffer_pagetable_info,
-		  current_rb_ptname)));
+	cmds += cp_gpuaddr(adreno_dev, cmds, (rb->pagetable_desc.gpuaddr +
+		PT_INFO_OFFSET(current_rb_ptname)));
 	*cmds++ = ptname;
 	*cmds++ = cp_mem_packet(adreno_dev, CP_MEM_WRITE, 2, 1);
 	cmds += cp_gpuaddr(adreno_dev, cmds,
-		  (adreno_dev->ringbuffers[0].pagetable_desc.gpuaddr +
-		  offsetof(struct adreno_ringbuffer_pagetable_info,
-		  current_global_ptname)));
+		(adreno_dev->ringbuffers[0].pagetable_desc.gpuaddr +
+		PT_INFO_OFFSET(current_global_ptname)));
 	*cmds++ = ptname;
 	/* pagetable switch done, Housekeeping: set the switch_pt_enable to 0 */
 	*cmds++ = cp_mem_packet(adreno_dev, CP_MEM_WRITE, 2, 1);
-	cmds += cp_gpuaddr(adreno_dev, cmds,
-			(rb->pagetable_desc.gpuaddr +
-			offsetof(struct adreno_ringbuffer_pagetable_info,
-			switch_pt_enable)));
+	cmds += cp_gpuaddr(adreno_dev, cmds, (rb->pagetable_desc.gpuaddr +
+		PT_INFO_OFFSET(switch_pt_enable)));
 	*cmds++ = 0;
 	*cmds++ = cp_packet(adreno_dev, CP_WAIT_MEM_WRITES, 1);
 	*cmds++ = 0;
@@ -445,14 +436,10 @@ static unsigned int _adreno_iommu_set_pt_v1(struct adreno_ringbuffer *rb,
 	/* set flag that indicates whether pt switch is required*/
 	cmds += _adreno_mmu_set_pt_update_condition(rb, cmds, ptname);
 	*cmds++ = cp_mem_packet(adreno_dev, CP_COND_EXEC, 4, 2);
-	cmds += cp_gpuaddr(adreno_dev, cmds,
-			(rb->pagetable_desc.gpuaddr +
-			offsetof(struct adreno_ringbuffer_pagetable_info,
-			switch_pt_enable)));
-	cmds += cp_gpuaddr(adreno_dev, cmds,
-			(rb->pagetable_desc.gpuaddr +
-			offsetof(struct adreno_ringbuffer_pagetable_info,
-			switch_pt_enable)));
+	cmds += cp_gpuaddr(adreno_dev, cmds, (rb->pagetable_desc.gpuaddr +
+		PT_INFO_OFFSET(switch_pt_enable)));
+	cmds += cp_gpuaddr(adreno_dev, cmds, (rb->pagetable_desc.gpuaddr +
+		PT_INFO_OFFSET(switch_pt_enable)));
 	*cmds++ = 1;
 	/* Exec count to be filled later */
 	cond_exec_ptr = cmds;
@@ -567,7 +554,7 @@ static unsigned int _adreno_iommu_set_pt_v2_a5xx(struct kgsl_device *device,
 
 	*cmds++ = cp_mem_packet(adreno_dev, CP_MEM_WRITE, 4, 1);
 	cmds += cp_gpuaddr(adreno_dev, cmds, (rb->pagetable_desc.gpuaddr +
-		offsetof(struct adreno_ringbuffer_pagetable_info, ttbr0)));
+		PT_INFO_OFFSET(ttbr0)));
 	*cmds++ = lower_32_bits(ttbr0);
 	*cmds++ = upper_32_bits(ttbr0);
 	*cmds++ = contextidr;
@@ -747,26 +734,11 @@ static int _set_pagetable_cpu(struct adreno_ringbuffer *rb,
 		if (result)
 			return result;
 		/* write the new pt set to memory var */
-		kgsl_sharedmem_writel(device,
-			&adreno_dev->ringbuffers[0].pagetable_desc,
-			offsetof(
-			struct adreno_ringbuffer_pagetable_info,
-			current_global_ptname), new_pt->name);
+		adreno_ringbuffer_set_global(adreno_dev, new_pt->name);
 	}
 
 	/* Update the RB pagetable info here */
-	kgsl_sharedmem_writel(device, &rb->pagetable_desc,
-		offsetof(
-		struct adreno_ringbuffer_pagetable_info,
-		current_rb_ptname), new_pt->name);
-	kgsl_sharedmem_writeq(device, &rb->pagetable_desc,
-		offsetof(
-		struct adreno_ringbuffer_pagetable_info,
-		ttbr0), kgsl_mmu_pagetable_get_ttbr0(new_pt));
-	kgsl_sharedmem_writel(device, &rb->pagetable_desc,
-		offsetof(
-		struct adreno_ringbuffer_pagetable_info,
-		contextidr), kgsl_mmu_pagetable_get_contextidr(new_pt));
+	adreno_ringbuffer_set_pagetable(rb, new_pt);
 
 	return 0;
 }
@@ -896,10 +868,8 @@ int adreno_iommu_set_pt_ctx(struct adreno_ringbuffer *rb,
 			result = _set_pagetable_gpu(rb, new_pt);
 	}
 
-	if (result) {
-		KGSL_DRV_ERR(device, "Error switching pagetable %d\n", result);
+	if (result)
 		return result;
-	}
 
 	/* Context switch */
 	if (cpu_path)
@@ -907,8 +877,5 @@ int adreno_iommu_set_pt_ctx(struct adreno_ringbuffer *rb,
 	else
 		result = _set_ctxt_gpu(rb, drawctxt);
 
-	if (result)
-		KGSL_DRV_ERR(device, "Error switching context %d\n", result);
-
 	return result;
 }
diff --git a/drivers/gpu/msm/adreno_ringbuffer.c b/drivers/gpu/msm/adreno_ringbuffer.c
index 383acb71a571..f93d433f406a 100644
--- a/drivers/gpu/msm/adreno_ringbuffer.c
+++ b/drivers/gpu/msm/adreno_ringbuffer.c
@@ -30,8 +30,6 @@
 #include "a3xx_reg.h"
 #include "adreno_a5xx.h"
 
-#define GSL_RB_NOP_SIZEDWORDS				2
-
 #define RB_HOSTPTR(_rb, _pos) \
 	((unsigned int *) ((_rb)->buffer_desc.hostptr + \
 		((_pos) * sizeof(unsigned int))))
@@ -50,86 +48,89 @@ static void _cff_write_ringbuffer(struct adreno_ringbuffer *rb)
 	if (device->cff_dump_enable == 0)
 		return;
 
-	/*
-	 * This code is predicated on the fact that we write a full block of
-	 * stuff without wrapping
-	 */
-	BUG_ON(rb->wptr < rb->last_wptr);
-
-	size = (rb->wptr - rb->last_wptr) * sizeof(unsigned int);
+	size = (rb->_wptr - rb->last_wptr) * sizeof(unsigned int);
 
 	hostptr = RB_HOSTPTR(rb, rb->last_wptr);
 	gpuaddr = RB_GPUADDR(rb, rb->last_wptr);
 
 	kgsl_cffdump_memcpy(device, gpuaddr, hostptr, size);
+	rb->last_wptr = rb->_wptr;
 }
 
-void adreno_ringbuffer_submit(struct adreno_ringbuffer *rb,
+static void adreno_get_submit_time(struct adreno_device *adreno_dev,
 		struct adreno_submit_time *time)
 {
-	struct adreno_device *adreno_dev = ADRENO_RB_DEVICE(rb);
-	BUG_ON(rb->wptr == 0);
-
-	/* Write the changes to CFF if so enabled */
-	_cff_write_ringbuffer(rb);
-
+	unsigned long flags;
 	/*
-	 * Read the current GPU ticks and wallclock for most accurate
-	 * profiling
+	 * Here we are attempting to create a mapping between the
+	 * GPU time domain (alwayson counter) and the CPU time domain
+	 * (local_clock) by sampling both values as close together as
+	 * possible. This is useful for many types of debugging and
+	 * profiling. In order to make this mapping as accurate as
+	 * possible, we must turn off interrupts to avoid running
+	 * interrupt handlers between the two samples.
 	 */
 
-	if (time != NULL) {
-		/*
-		 * Here we are attempting to create a mapping between the
-		 * GPU time domain (alwayson counter) and the CPU time domain
-		 * (local_clock) by sampling both values as close together as
-		 * possible. This is useful for many types of debugging and
-		 * profiling. In order to make this mapping as accurate as
-		 * possible, we must turn off interrupts to avoid running
-		 * interrupt handlers between the two samples.
-		 */
-		unsigned long flags;
-		local_irq_save(flags);
+	local_irq_save(flags);
 
-		/* Read always on registers */
-		if (!adreno_is_a3xx(adreno_dev)) {
-			adreno_readreg64(adreno_dev,
-				ADRENO_REG_RBBM_ALWAYSON_COUNTER_LO,
-				ADRENO_REG_RBBM_ALWAYSON_COUNTER_HI,
-				&time->ticks);
+	/* Read always on registers */
+	if (!adreno_is_a3xx(adreno_dev)) {
+		adreno_readreg64(adreno_dev,
+			ADRENO_REG_RBBM_ALWAYSON_COUNTER_LO,
+			ADRENO_REG_RBBM_ALWAYSON_COUNTER_HI,
+			&time->ticks);
 
-			/*
-			 * Mask hi bits as they may be incorrect on
-			 * a4x and some a5x
-			 */
-			if (ADRENO_GPUREV(adreno_dev) >= 400 &&
+		/* Mask hi bits as they may be incorrect on some targets */
+		if (ADRENO_GPUREV(adreno_dev) >= 400 &&
 				ADRENO_GPUREV(adreno_dev) <= ADRENO_REV_A530)
-				time->ticks &= 0xFFFFFFFF;
-		}
-		else
-			time->ticks = 0;
+			time->ticks &= 0xFFFFFFFF;
+	} else
+		time->ticks = 0;
 
-		/* Get the kernel clock for time since boot */
-		time->ktime = local_clock();
+	/* Get the kernel clock for time since boot */
+	time->ktime = local_clock();
 
-		/* Get the timeofday for the wall time (for the user) */
-		getnstimeofday(&time->utime);
+	/* Get the timeofday for the wall time (for the user) */
+	getnstimeofday(&time->utime);
 
-		local_irq_restore(flags);
-	}
+	local_irq_restore(flags);
+}
 
-	/* Memory barrier before informing the hardware of new commands */
-	mb();
+void adreno_ringbuffer_wptr(struct adreno_device *adreno_dev,
+		struct adreno_ringbuffer *rb)
+{
+	unsigned long flags;
 
-	if (adreno_preempt_state(adreno_dev, ADRENO_DISPATCHER_PREEMPT_CLEAR) &&
-		(adreno_dev->cur_rb == rb)) {
-		/*
-		 * Let the pwrscale policy know that new commands have
-		 * been submitted.
-		 */
-		kgsl_pwrscale_busy(KGSL_DEVICE(adreno_dev));
-		adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR, rb->wptr);
+	spin_lock_irqsave(&rb->preempt_lock, flags);
+	if (adreno_in_preempt_state(adreno_dev, ADRENO_PREEMPT_NONE)) {
+
+		if (adreno_dev->cur_rb == rb) {
+			/*
+			 * Let the pwrscale policy know that new commands have
+			 * been submitted.
+			*/
+			kgsl_pwrscale_busy(KGSL_DEVICE(adreno_dev));
+			adreno_writereg(adreno_dev, ADRENO_REG_CP_RB_WPTR,
+				rb->_wptr);
+		}
 	}
+
+	rb->wptr = rb->_wptr;
+	spin_unlock_irqrestore(&rb->preempt_lock, flags);
+}
+
+void adreno_ringbuffer_submit(struct adreno_ringbuffer *rb,
+		struct adreno_submit_time *time)
+{
+	struct adreno_device *adreno_dev = ADRENO_RB_DEVICE(rb);
+
+	/* Write the changes to CFF if so enabled */
+	_cff_write_ringbuffer(rb);
+
+	if (time != NULL)
+		adreno_get_submit_time(adreno_dev, time);
+
+	adreno_ringbuffer_wptr(adreno_dev, rb);
 }
 
 int adreno_ringbuffer_submit_spin(struct adreno_ringbuffer *rb,
@@ -141,125 +142,36 @@ int adreno_ringbuffer_submit_spin(struct adreno_ringbuffer *rb,
 	return adreno_spin_idle(adreno_dev, timeout);
 }
 
-static int
-adreno_ringbuffer_waitspace(struct adreno_ringbuffer *rb,
-				unsigned int numcmds, int wptr_ahead)
+unsigned int *adreno_ringbuffer_allocspace(struct adreno_ringbuffer *rb,
+		unsigned int dwords)
 {
-	int nopcount = 0;
-	unsigned int freecmds;
-	unsigned int wptr = rb->wptr;
-	unsigned int *cmds = NULL;
-	uint64_t gpuaddr;
-	unsigned long wait_time;
-	unsigned long wait_timeout = msecs_to_jiffies(ADRENO_IDLE_TIMEOUT);
-	unsigned int rptr;
 	struct adreno_device *adreno_dev = ADRENO_RB_DEVICE(rb);
+	unsigned int rptr = adreno_get_rptr(rb);
+	unsigned int ret;
 
-	/* if wptr ahead, fill the remaining with NOPs */
-	if (wptr_ahead) {
-		/* -1 for header */
-		nopcount = KGSL_RB_DWORDS - rb->wptr - 1;
-
-		cmds = RB_HOSTPTR(rb, rb->wptr);
-		gpuaddr = RB_GPUADDR(rb, rb->wptr);
-
-		rptr = adreno_get_rptr(rb);
-		/* For non current rb we don't expect the rptr to move */
-		if ((adreno_dev->cur_rb != rb ||
-				!adreno_preempt_state(adreno_dev,
-				ADRENO_DISPATCHER_PREEMPT_CLEAR)) &&
-			!rptr)
-			return -ENOSPC;
-
-		/* Make sure that rptr is not 0 before submitting
-		 * commands at the end of ringbuffer. We do not
-		 * want the rptr and wptr to become equal when
-		 * the ringbuffer is not empty */
-		wait_time = jiffies + wait_timeout;
-		while (!rptr) {
-			rptr = adreno_get_rptr(rb);
-			if (time_after(jiffies, wait_time))
-				return -ETIMEDOUT;
-		}
-
-		rb->wptr = 0;
-	}
-
-	rptr = adreno_get_rptr(rb);
-	freecmds = rptr - rb->wptr;
-	if (freecmds == 0 || freecmds > numcmds)
-		goto done;
-
-	/* non current rptr will not advance anyway or if preemption underway */
-	if (adreno_dev->cur_rb != rb ||
-		!adreno_preempt_state(adreno_dev,
-			ADRENO_DISPATCHER_PREEMPT_CLEAR)) {
-		rb->wptr = wptr;
-		return -ENOSPC;
-	}
-
-	wait_time = jiffies + wait_timeout;
-	/* wait for space in ringbuffer */
-	while (1) {
-		rptr = adreno_get_rptr(rb);
-
-		freecmds = rptr - rb->wptr;
-
-		if (freecmds == 0 || freecmds > numcmds)
-			break;
+	if (rptr <= rb->_wptr) {
+		unsigned int *cmds;
 
-		if (time_after(jiffies, wait_time)) {
-			KGSL_DRV_ERR(KGSL_DEVICE(adreno_dev),
-			"Timed out waiting for freespace in RB rptr: 0x%x, wptr: 0x%x, rb id %d\n",
-			rptr, wptr, rb->id);
-			return -ETIMEDOUT;
+		if (rb->_wptr + dwords <= (KGSL_RB_DWORDS - 2)) {
+			ret = rb->_wptr;
+			rb->_wptr = (rb->_wptr + dwords) % KGSL_RB_DWORDS;
+			return RB_HOSTPTR(rb, ret);
 		}
-	}
-done:
-	if (wptr_ahead) {
-		*cmds = cp_packet(adreno_dev, CP_NOP, nopcount);
-		kgsl_cffdump_write(KGSL_DEVICE(adreno_dev), gpuaddr, *cmds);
 
-	}
-	return 0;
-}
+		cmds = RB_HOSTPTR(rb, rb->_wptr);
+		*cmds = cp_packet(adreno_dev, CP_NOP,
+			KGSL_RB_DWORDS - rb->_wptr - 1);
 
-unsigned int *adreno_ringbuffer_allocspace(struct adreno_ringbuffer *rb,
-					unsigned int numcmds)
-{
-	unsigned int *ptr = NULL;
-	int ret = 0;
-	unsigned int rptr;
-	BUG_ON(numcmds >= KGSL_RB_DWORDS);
-
-	rptr = adreno_get_rptr(rb);
-	/* check for available space */
-	if (rb->wptr >= rptr) {
-		/* wptr ahead or equal to rptr */
-		/* reserve dwords for nop packet */
-		if ((rb->wptr + numcmds) > (KGSL_RB_DWORDS -
-				GSL_RB_NOP_SIZEDWORDS))
-			ret = adreno_ringbuffer_waitspace(rb, numcmds, 1);
-	} else {
-		/* wptr behind rptr */
-		if ((rb->wptr + numcmds) >= rptr)
-			ret = adreno_ringbuffer_waitspace(rb, numcmds, 0);
-		/* check for remaining space */
-		/* reserve dwords for nop packet */
-		if (!ret && (rb->wptr + numcmds) > (KGSL_RB_DWORDS -
-				GSL_RB_NOP_SIZEDWORDS))
-			ret = adreno_ringbuffer_waitspace(rb, numcmds, 1);
+		rb->_wptr = 0;
 	}
 
-	if (!ret) {
-		rb->last_wptr = rb->wptr;
-
-		ptr = (unsigned int *)rb->buffer_desc.hostptr + rb->wptr;
-		rb->wptr += numcmds;
-	} else
-		ptr = ERR_PTR(ret);
+	if (rb->_wptr + dwords < rptr) {
+		ret = rb->_wptr;
+		rb->_wptr = (rb->_wptr + dwords) % KGSL_RB_DWORDS;
+		return RB_HOSTPTR(rb, ret);
+	}
 
-	return ptr;
+	return ERR_PTR(-ENOSPC);
 }
 
 /**
@@ -282,6 +194,7 @@ int adreno_ringbuffer_start(struct adreno_device *adreno_dev,
 		kgsl_sharedmem_writel(device, &device->scratch,
 				SCRATCH_RPTR_OFFSET(rb->id), 0);
 		rb->wptr = 0;
+		rb->_wptr = 0;
 		rb->wptr_preempt_end = 0xFFFFFFFF;
 		rb->starve_timer_state =
 			ADRENO_DISPATCHER_RB_STARVE_TIMER_UNINIT;
@@ -323,6 +236,8 @@ static int _adreno_ringbuffer_probe(struct adreno_device *adreno_dev,
 	rb->timestamp = 0;
 	init_waitqueue_head(&rb->ts_expire_waitq);
 
+	spin_lock_init(&rb->preempt_lock);
+
 	/*
 	 * Allocate mem for storing RB pagetables and commands to
 	 * switch pagetable
@@ -457,6 +372,10 @@ adreno_ringbuffer_addcmds(struct adreno_ringbuffer *rb,
 		!(flags & KGSL_CMD_FLAGS_INTERNAL_ISSUE))
 		return -ENOENT;
 
+	/* On fault return error so that we don't keep submitting */
+	if (adreno_gpu_fault(adreno_dev) != 0)
+		return -EPROTO;
+
 	rb->timestamp++;
 
 	/* If this is a internal IB, use the global timestamp for it */
@@ -715,7 +634,7 @@ adreno_ringbuffer_addcmds(struct adreno_ringbuffer *rb,
 	 *  required. If we have commands less than the space reserved in RB
 	 *  adjust the wptr accordingly.
 	 */
-	rb->wptr = rb->wptr - (total_sizedwords - (ringcmds - start));
+	rb->_wptr = rb->_wptr - (total_sizedwords - (ringcmds - start));
 
 	adreno_ringbuffer_submit(rb, time);
 
@@ -1062,8 +981,16 @@ int adreno_ringbuffer_submitcmd(struct adreno_device *adreno_dev,
 	 * In the unlikely event of an error in the drawctxt switch,
 	 * treat it like a hang
 	 */
-	if (ret)
+	if (ret) {
+		/*
+		 * It is "normal" to get a -ENOSPC or a -ENOENT. Don't log it,
+		 * the upper layers know how to handle it
+		 */
+		if (ret != -ENOSPC && ret != -ENOENT)
+			KGSL_DRV_ERR(device,
+				"Unable to switch draw context: %d\n", ret);
 		goto done;
+	}
 
 	if (test_bit(CMDBATCH_FLAG_WFI, &cmdbatch->priv))
 		flags = KGSL_CMD_FLAGS_WFI;
diff --git a/drivers/gpu/msm/adreno_ringbuffer.h b/drivers/gpu/msm/adreno_ringbuffer.h
index f315653f49b0..9e6a2b2db05c 100644
--- a/drivers/gpu/msm/adreno_ringbuffer.h
+++ b/drivers/gpu/msm/adreno_ringbuffer.h
@@ -73,12 +73,16 @@ struct adreno_ringbuffer_pagetable_info {
 	unsigned int contextidr;
 };
 
+#define PT_INFO_OFFSET(_field) \
+	offsetof(struct adreno_ringbuffer_pagetable_info, _field)
+
 /**
  * struct adreno_ringbuffer - Definition for an adreno ringbuffer object
  * @flags: Internal control flags for the ringbuffer
- * @buffer_desc: Pointer to the ringbuffer memory descriptor
- * @wptr: Local copy of the wptr offset
- * @last_wptr: offset of the last H/W committed wptr
+ * @buffer_desc: Pointer to the ringbuffer memory descripto
+ * @_wptr: The next value of wptr to be written to the hardware on submit
+ * @wptr: Local copy of the wptr offset last written to hardware
+ * @last_wptr: offset of the last wptr that was written to CFF
  * @rb_ctx: The context that represents a ringbuffer
  * @id: Priority level of the ringbuffer, also used as an ID
  * @fault_detect_ts: The last retired global timestamp read during fault detect
@@ -100,10 +104,12 @@ struct adreno_ringbuffer_pagetable_info {
  * @sched_timer: Timer that tracks how long RB has been waiting to be scheduled
  * or how long it has been scheduled for after preempting in
  * @starve_timer_state: Indicates the state of the wait.
+ * @preempt_lock: Lock to protect the wptr pointer while it is being updated
  */
 struct adreno_ringbuffer {
 	uint32_t flags;
 	struct kgsl_memdesc buffer_desc;
+	unsigned int _wptr;
 	unsigned int wptr;
 	unsigned int last_wptr;
 	int id;
@@ -120,6 +126,7 @@ struct adreno_ringbuffer {
 	int preempted_midway;
 	unsigned long sched_timer;
 	enum adreno_dispatcher_starve_timer_states starve_timer_state;
+	spinlock_t preempt_lock;
 };
 
 /* Returns the current ringbuffer */
diff --git a/drivers/gpu/msm/adreno_snapshot.c b/drivers/gpu/msm/adreno_snapshot.c
index 6bdf73164902..b069b16c75ef 100644
--- a/drivers/gpu/msm/adreno_snapshot.c
+++ b/drivers/gpu/msm/adreno_snapshot.c
@@ -741,8 +741,7 @@ static size_t snapshot_global(struct kgsl_device *device, u8 *buf,
 
 	header->size = memdesc->size >> 2;
 	header->gpuaddr = memdesc->gpuaddr;
-	header->ptbase =
-		kgsl_mmu_pagetable_get_ttbr0(device->mmu.defaultpagetable);
+	header->ptbase = MMU_DEFAULT_TTBR0(device);
 	header->type = SNAPSHOT_GPU_OBJECT_GLOBAL;
 
 	memcpy(ptr, memdesc->hostptr, memdesc->size);
diff --git a/drivers/gpu/msm/adreno_trace.h b/drivers/gpu/msm/adreno_trace.h
index 0109a4c0a5e7..149cb0dff1a0 100644
--- a/drivers/gpu/msm/adreno_trace.h
+++ b/drivers/gpu/msm/adreno_trace.h
@@ -547,6 +547,37 @@ TRACE_EVENT(adreno_hw_preempt_token_submit,
 	)
 );
 
+TRACE_EVENT(adreno_preempt_trigger,
+	TP_PROTO(struct adreno_ringbuffer *cur, struct adreno_ringbuffer *next),
+	TP_ARGS(cur, next),
+	TP_STRUCT__entry(
+		__field(struct adreno_ringbuffer *, cur)
+		__field(struct adreno_ringbuffer *, next)
+	),
+	TP_fast_assign(
+		__entry->cur = cur;
+		__entry->next = next;
+	),
+	TP_printk("trigger from id=%d to id=%d",
+		__entry->cur->id, __entry->next->id
+	)
+);
+
+TRACE_EVENT(adreno_preempt_done,
+	TP_PROTO(struct adreno_ringbuffer *cur, struct adreno_ringbuffer *next),
+	TP_ARGS(cur, next),
+	TP_STRUCT__entry(
+		__field(struct adreno_ringbuffer *, cur)
+		__field(struct adreno_ringbuffer *, next)
+	),
+	TP_fast_assign(
+		__entry->cur = cur;
+		__entry->next = next;
+	),
+	TP_printk("done switch to id=%d from id=%d",
+		__entry->next->id, __entry->cur->id
+	)
+);
 #endif /* _ADRENO_TRACE_H */
 
 /* This part must be outside protection */
diff --git a/drivers/gpu/msm/kgsl_cmdbatch.h b/drivers/gpu/msm/kgsl_cmdbatch.h
index 1547ac02fdbf..d5cbf375b5d3 100644
--- a/drivers/gpu/msm/kgsl_cmdbatch.h
+++ b/drivers/gpu/msm/kgsl_cmdbatch.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2008-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -31,7 +31,6 @@
  * @fault_policy: Internal policy describing how to handle this command in case
  * of a fault
  * @fault_recovery: recovery actions actually tried for this batch
- * @expires: Point in time when the cmdbatch is considered to be hung
  * @refcount: kref structure to maintain the reference count
  * @cmdlist: List of IBs to issue
  * @memlist: List of all memory used in this command batch
@@ -61,7 +60,6 @@ struct kgsl_cmdbatch {
 	unsigned long priv;
 	unsigned long fault_policy;
 	unsigned long fault_recovery;
-	unsigned long expires;
 	struct kref refcount;
 	struct list_head cmdlist;
 	struct list_head memlist;
diff --git a/drivers/gpu/msm/kgsl_mmu.h b/drivers/gpu/msm/kgsl_mmu.h
index 3652aa2e6ec4..5339917911b1 100644
--- a/drivers/gpu/msm/kgsl_mmu.h
+++ b/drivers/gpu/msm/kgsl_mmu.h
@@ -21,6 +21,12 @@
 #define KGSL_MMU_GLOBAL_PT 0
 #define KGSL_MMU_SECURE_PT 1
 
+#define MMU_DEFAULT_TTBR0(_d) \
+	(kgsl_mmu_pagetable_get_ttbr0((_d)->mmu.defaultpagetable))
+
+#define MMU_DEFAULT_CONTEXTIDR(_d) \
+	(kgsl_mmu_pagetable_get_contextidr((_d)->mmu.defaultpagetable))
+
 struct kgsl_device;
 
 enum kgsl_mmutype {