diff --git a/drivers/gpu/msm/adreno.c b/drivers/gpu/msm/adreno.c
index 5170701d9e38..e9b656140363 100644
--- a/drivers/gpu/msm/adreno.c
+++ b/drivers/gpu/msm/adreno.c
@@ -223,15 +223,14 @@ unsigned int adreno_get_rptr(struct adreno_ringbuffer *rb)
 static void __iomem *efuse_base;
 static size_t efuse_len;
 
-int adreno_efuse_map(struct adreno_device *adreno_dev)
+int adreno_efuse_map(struct platform_device *pdev)
 {
-	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
 	struct resource *res;
 
 	if (efuse_base != NULL)
 		return 0;
 
-	res = platform_get_resource_byname(device->pdev, IORESOURCE_MEM,
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 		"qfprom_memory");
 
 	if (res == NULL)
@@ -245,7 +244,7 @@ int adreno_efuse_map(struct adreno_device *adreno_dev)
 	return 0;
 }
 
-void adreno_efuse_unmap(struct adreno_device *adreno_dev)
+void adreno_efuse_unmap(void)
 {
 	if (efuse_base != NULL) {
 		iounmap(efuse_base);
@@ -254,8 +253,7 @@ void adreno_efuse_unmap(struct adreno_device *adreno_dev)
 	}
 }
 
-int adreno_efuse_read_u32(struct adreno_device *adreno_dev, unsigned int offset,
-		unsigned int *val)
+int adreno_efuse_read_u32(unsigned int offset, unsigned int *val)
 {
 	if (efuse_base == NULL)
 		return -ENODEV;
@@ -263,11 +261,9 @@ int adreno_efuse_read_u32(struct adreno_device *adreno_dev, unsigned int offset,
 	if (offset >= efuse_len)
 		return -ERANGE;
 
-	if (val != NULL) {
-		*val = readl_relaxed(efuse_base + offset);
-		/* Make sure memory is updated before returning */
-		rmb();
-	}
+	*val = readl_relaxed(efuse_base + offset);
+	/* Make sure memory is updated before returning */
+	rmb();
 
 	return 0;
 }
@@ -1256,27 +1252,28 @@ static void adreno_isense_probe(struct kgsl_device *device)
 
 static void adreno_efuse_read_soc_hw_rev(struct adreno_device *adreno_dev)
 {
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
 	unsigned int val;
 	unsigned int soc_hw_rev[3];
 	int ret;
 
 	if (of_property_read_u32_array(
-		KGSL_DEVICE(adreno_dev)->pdev->dev.of_node,
+		device->pdev->dev.of_node,
 		"qcom,soc-hw-rev-efuse", soc_hw_rev, 3))
 		return;
 
-	ret = adreno_efuse_map(adreno_dev);
+	ret = adreno_efuse_map(device->pdev);
 	if (ret) {
-		dev_err(KGSL_DEVICE(adreno_dev)->dev,
+		dev_err(device->dev,
 			"Unable to map hardware revision fuse: ret=%d\n", ret);
 		return;
 	}
 
-	ret = adreno_efuse_read_u32(adreno_dev, soc_hw_rev[0], &val);
-	adreno_efuse_unmap(adreno_dev);
+	ret = adreno_efuse_read_u32(soc_hw_rev[0], &val);
+	adreno_efuse_unmap();
 
 	if (ret) {
-		dev_err(KGSL_DEVICE(adreno_dev)->dev,
+		dev_err(device->dev,
 			"Unable to read hardware revision fuse: ret=%d\n", ret);
 		return;
 	}
@@ -1298,11 +1295,11 @@ static bool adreno_is_gpu_disabled(struct adreno_device *adreno_dev)
 	 * Read the fuse value to disable GPU driver if fuse
 	 * is blown. By default(fuse value is 0) GPU is enabled.
 	 */
-	if (adreno_efuse_map(adreno_dev))
+	if (adreno_efuse_map(device->pdev))
 		return false;
 
-	ret = adreno_efuse_read_u32(adreno_dev, pte_row0_msb[0], &row0);
-	adreno_efuse_unmap(adreno_dev);
+	ret = adreno_efuse_read_u32(pte_row0_msb[0], &row0);
+	adreno_efuse_unmap();
 
 	if (ret)
 		return false;
@@ -1822,7 +1819,7 @@ static void _set_secvid(struct kgsl_device *device)
 	}
 }
 
-static int adreno_switch_to_unsecure_mode(struct adreno_device *adreno_dev,
+int adreno_switch_to_unsecure_mode(struct adreno_device *adreno_dev,
 				struct adreno_ringbuffer *rb)
 {
 	unsigned int *cmds;
@@ -1844,31 +1841,6 @@ static int adreno_switch_to_unsecure_mode(struct adreno_device *adreno_dev,
 	return ret;
 }
 
-int adreno_set_unsecured_mode(struct adreno_device *adreno_dev,
-		struct adreno_ringbuffer *rb)
-{
-	int ret = 0;
-
-	if (!adreno_is_a5xx(adreno_dev) && !adreno_is_a6xx(adreno_dev))
-		return -EINVAL;
-
-	if (ADRENO_QUIRK(adreno_dev, ADRENO_QUIRK_CRITICAL_PACKETS) &&
-			adreno_is_a5xx(adreno_dev)) {
-		ret = a5xx_critical_packet_submit(adreno_dev, rb);
-		if (ret)
-			return ret;
-	}
-
-	/* GPU comes up in secured mode, make it unsecured by default */
-	if (adreno_dev->zap_loaded)
-		ret = adreno_switch_to_unsecure_mode(adreno_dev, rb);
-	else
-		adreno_writereg(adreno_dev,
-				ADRENO_REG_RBBM_SECVID_TRUST_CONTROL, 0x0);
-
-	return ret;
-}
-
 static void adreno_set_active_ctxs_null(struct adreno_device *adreno_dev)
 {
 	int i;
diff --git a/drivers/gpu/msm/adreno.h b/drivers/gpu/msm/adreno.h
index 8979079138c1..67a5b57d1490 100644
--- a/drivers/gpu/msm/adreno.h
+++ b/drivers/gpu/msm/adreno.h
@@ -663,8 +663,6 @@ enum adreno_regs {
 	ADRENO_REG_RBBM_PERFCTR_RBBM_0_HI,
 	ADRENO_REG_RBBM_PERFCTR_LOAD_VALUE_LO,
 	ADRENO_REG_RBBM_PERFCTR_LOAD_VALUE_HI,
-	ADRENO_REG_RBBM_SECVID_TRUST_CONTROL,
-	ADRENO_REG_RBBM_SECVID_TRUST_CONFIG,
 	ADRENO_REG_RBBM_SECVID_TSB_CONTROL,
 	ADRENO_REG_RBBM_SECVID_TSB_TRUSTED_BASE,
 	ADRENO_REG_RBBM_SECVID_TSB_TRUSTED_BASE_HI,
@@ -913,10 +911,17 @@ long adreno_ioctl_helper(struct kgsl_device_private *dev_priv,
 		unsigned int cmd, unsigned long arg,
 		const struct kgsl_ioctl *cmds, int len);
 
-int a5xx_critical_packet_submit(struct adreno_device *adreno_dev,
-		struct adreno_ringbuffer *rb);
-int adreno_set_unsecured_mode(struct adreno_device *adreno_dev,
-		struct adreno_ringbuffer *rb);
+/*
+ * adreno_switch_to_unsecure_mode - Execute a zap shader
+ * @adreno_dev: An Adreno GPU handle
+ * @rb: The ringbuffer to execute on
+ *
+ * Execute the zap shader from the CP to take the GPU out of secure mode.
+ * Return: 0 on success or negative on failure
+ */
+int adreno_switch_to_unsecure_mode(struct adreno_device *adreno_dev,
+				struct adreno_ringbuffer *rb);
+
 void adreno_spin_idle_debug(struct adreno_device *adreno_dev, const char *str);
 int adreno_spin_idle(struct adreno_device *device, unsigned int timeout);
 int adreno_idle(struct kgsl_device *device);
@@ -955,10 +960,9 @@ long adreno_ioctl_perfcounter_get(struct kgsl_device_private *dev_priv,
 long adreno_ioctl_perfcounter_put(struct kgsl_device_private *dev_priv,
 	unsigned int cmd, void *data);
 
-int adreno_efuse_map(struct adreno_device *adreno_dev);
-int adreno_efuse_read_u32(struct adreno_device *adreno_dev, unsigned int offset,
-		unsigned int *val);
-void adreno_efuse_unmap(struct adreno_device *adreno_dev);
+int adreno_efuse_map(struct platform_device *pdev);
+int adreno_efuse_read_u32(unsigned int offset, unsigned int *val);
+void adreno_efuse_unmap(void);
 
 bool adreno_is_cx_dbgc_register(struct kgsl_device *device,
 		unsigned int offset);
diff --git a/drivers/gpu/msm/adreno_a3xx.c b/drivers/gpu/msm/adreno_a3xx.c
index 0275dcc80f04..858520271d42 100644
--- a/drivers/gpu/msm/adreno_a3xx.c
+++ b/drivers/gpu/msm/adreno_a3xx.c
@@ -135,7 +135,7 @@ static void a3xx_efuse_speed_bin(struct adreno_device *adreno_dev)
 		"qcom,gpu-speed-bin", speed_bin, 3))
 		return;
 
-	adreno_efuse_read_u32(adreno_dev, speed_bin[0], &val);
+	adreno_efuse_read_u32(speed_bin[0], &val);
 
 	adreno_dev->speed_bin = (val & speed_bin[1]) >> speed_bin[2];
 }
@@ -151,7 +151,7 @@ static void a3xx_check_features(struct adreno_device *adreno_dev)
 {
 	unsigned int i;
 
-	if (adreno_efuse_map(adreno_dev))
+	if (adreno_efuse_map(KGSL_DEVICE(adreno_dev)->pdev))
 		return;
 
 	for (i = 0; i < ARRAY_SIZE(a3xx_efuse_funcs); i++) {
@@ -159,7 +159,7 @@ static void a3xx_check_features(struct adreno_device *adreno_dev)
 			a3xx_efuse_funcs[i].func(adreno_dev);
 	}
 
-	adreno_efuse_unmap(adreno_dev);
+	adreno_efuse_unmap();
 }
 
 /**
diff --git a/drivers/gpu/msm/adreno_a5xx.c b/drivers/gpu/msm/adreno_a5xx.c
index 732d8be6e8f7..30febffd8859 100644
--- a/drivers/gpu/msm/adreno_a5xx.c
+++ b/drivers/gpu/msm/adreno_a5xx.c
@@ -49,11 +49,9 @@ static void a530_efuse_leakage(struct adreno_device *adreno_dev)
 	unsigned int row0, row2;
 	unsigned int multiplier, gfx_active, leakage_pwr_on, coeff;
 
-	adreno_efuse_read_u32(adreno_dev,
-		A530_QFPROM_RAW_PTE_ROW0_MSB, &row0);
+	adreno_efuse_read_u32(A530_QFPROM_RAW_PTE_ROW0_MSB, &row0);
 
-	adreno_efuse_read_u32(adreno_dev,
-		A530_QFPROM_RAW_PTE_ROW2_MSB, &row2);
+	adreno_efuse_read_u32(A530_QFPROM_RAW_PTE_ROW2_MSB, &row2);
 
 	multiplier = (row0 >> 1) & 0x3;
 	gfx_active = (row2 >> 2) & 0xFF;
@@ -78,7 +76,7 @@ static void a530_efuse_speed_bin(struct adreno_device *adreno_dev)
 		"qcom,gpu-speed-bin", speed_bin, 3))
 		return;
 
-	adreno_efuse_read_u32(adreno_dev, speed_bin[0], &val);
+	adreno_efuse_read_u32(speed_bin[0], &val);
 
 	adreno_dev->speed_bin = (val & speed_bin[1]) >> speed_bin[2];
 }
@@ -98,7 +96,7 @@ static void a5xx_check_features(struct adreno_device *adreno_dev)
 {
 	unsigned int i;
 
-	if (adreno_efuse_map(adreno_dev))
+	if (adreno_efuse_map(KGSL_DEVICE(adreno_dev)->pdev))
 		return;
 
 	for (i = 0; i < ARRAY_SIZE(a5xx_efuse_funcs); i++) {
@@ -106,7 +104,7 @@ static void a5xx_check_features(struct adreno_device *adreno_dev)
 			a5xx_efuse_funcs[i].func(adreno_dev);
 	}
 
-	adreno_efuse_unmap(adreno_dev);
+	adreno_efuse_unmap();
 }
 
 static void a5xx_platform_setup(struct adreno_device *adreno_dev)
@@ -1811,7 +1809,7 @@ static void _set_ordinals(struct adreno_device *adreno_dev,
 		*cmds++ = 0x0;
 }
 
-int a5xx_critical_packet_submit(struct adreno_device *adreno_dev,
+static int a5xx_critical_packet_submit(struct adreno_device *adreno_dev,
 					struct adreno_ringbuffer *rb)
 {
 	unsigned int *cmds;
@@ -1907,8 +1905,22 @@ static int a5xx_rb_start(struct adreno_device *adreno_dev)
 	if (ret)
 		return ret;
 
-	/* GPU comes up in secured mode, make it unsecured by default */
-	ret = adreno_set_unsecured_mode(adreno_dev, rb);
+	/* Run the critical packets if we need to */
+	if (ADRENO_QUIRK(adreno_dev, ADRENO_QUIRK_CRITICAL_PACKETS)) {
+		ret = a5xx_critical_packet_submit(adreno_dev, rb);
+		if (ret)
+			return ret;
+	}
+
+	/*
+	 * Try to execute the zap shader if it exists, otherwise just try
+	 * directly writing to the control register
+	 */
+	if (!adreno_dev->zap_loaded)
+		kgsl_regwrite(device, A5XX_RBBM_SECVID_TRUST_CNTL, 0);
+	else
+		ret = adreno_switch_to_unsecure_mode(adreno_dev, rb);
+
 	if (ret)
 		return ret;
 
@@ -2433,10 +2445,6 @@ static unsigned int a5xx_register_offsets[ADRENO_REG_REGISTER_MAX] = {
 				A5XX_RBBM_PERFCTR_LOAD_VALUE_LO),
 	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_PERFCTR_LOAD_VALUE_HI,
 				A5XX_RBBM_PERFCTR_LOAD_VALUE_HI),
-	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_SECVID_TRUST_CONTROL,
-				A5XX_RBBM_SECVID_TRUST_CNTL),
-	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_SECVID_TRUST_CONFIG,
-				A5XX_RBBM_SECVID_TRUST_CONFIG),
 	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_SECVID_TSB_CONTROL,
 				A5XX_RBBM_SECVID_TSB_CNTL),
 	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_SECVID_TSB_TRUSTED_BASE,
diff --git a/drivers/gpu/msm/adreno_a6xx.c b/drivers/gpu/msm/adreno_a6xx.c
index 2576593b517b..58f383c345af 100644
--- a/drivers/gpu/msm/adreno_a6xx.c
+++ b/drivers/gpu/msm/adreno_a6xx.c
@@ -816,8 +816,15 @@ static int a6xx_rb_start(struct adreno_device *adreno_dev)
 	if (ret)
 		return ret;
 
-	/* GPU comes up in secured mode, make it unsecured by default */
-	ret = adreno_set_unsecured_mode(adreno_dev, rb);
+	/*
+	 * Take the GPU out of secure mode. Try the zap shader if it is loaded,
+	 * otherwise just try to write directly to the secure control register
+	 */
+	if (!adreno_dev->zap_loaded)
+		kgsl_regwrite(device, A6XX_RBBM_SECVID_TRUST_CNTL, 0);
+	else
+		ret = adreno_switch_to_unsecure_mode(adreno_dev, rb);
+
 	if (ret)
 		return ret;
 
@@ -2168,7 +2175,7 @@ static void a6xx_efuse_speed_bin(struct adreno_device *adreno_dev)
 		"qcom,gpu-speed-bin", speed_bin, 3))
 		return;
 
-	adreno_efuse_read_u32(adreno_dev, speed_bin[0], &val);
+	adreno_efuse_read_u32(speed_bin[0], &val);
 
 	adreno_dev->speed_bin = (val & speed_bin[1]) >> speed_bin[2];
 }
@@ -2185,14 +2192,14 @@ static void a6xx_check_features(struct adreno_device *adreno_dev)
 {
 	unsigned int i;
 
-	if (adreno_efuse_map(adreno_dev))
+	if (adreno_efuse_map(KGSL_DEVICE(adreno_dev)->pdev))
 		return;
 	for (i = 0; i < ARRAY_SIZE(a6xx_efuse_funcs); i++) {
 		if (a6xx_efuse_funcs[i].check(adreno_dev))
 			a6xx_efuse_funcs[i].func(adreno_dev);
 	}
 
-	adreno_efuse_unmap(adreno_dev);
+	adreno_efuse_unmap();
 }
 static void a6xx_platform_setup(struct adreno_device *adreno_dev)
 {
@@ -2370,8 +2377,6 @@ static unsigned int a6xx_register_offsets[ADRENO_REG_REGISTER_MAX] = {
 				A6XX_GMU_CM3_CFG),
 	ADRENO_REG_DEFINE(ADRENO_REG_GMU_RBBM_INT_UNMASKED_STATUS,
 				A6XX_GMU_RBBM_INT_UNMASKED_STATUS),
-	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_SECVID_TRUST_CONTROL,
-				A6XX_RBBM_SECVID_TRUST_CNTL),
 	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_SECVID_TSB_TRUSTED_BASE,
 				A6XX_RBBM_SECVID_TSB_TRUSTED_BASE_LO),
 	ADRENO_REG_DEFINE(ADRENO_REG_RBBM_SECVID_TSB_TRUSTED_BASE_HI,
diff --git a/drivers/gpu/msm/kgsl.c b/drivers/gpu/msm/kgsl.c
index 18767bbdbd12..b20597d7555d 100644
--- a/drivers/gpu/msm/kgsl.c
+++ b/drivers/gpu/msm/kgsl.c
@@ -21,6 +21,7 @@
 #include "kgsl_debugfs.h"
 #include "kgsl_device.h"
 #include "kgsl_mmu.h"
+#include "kgsl_pool.h"
 #include "kgsl_sync.h"
 #include "kgsl_trace.h"
 
diff --git a/drivers/gpu/msm/kgsl_pool.c b/drivers/gpu/msm/kgsl_pool.c
index 7077d5d7445c..7fca6d8b8b4a 100644
--- a/drivers/gpu/msm/kgsl_pool.c
+++ b/drivers/gpu/msm/kgsl_pool.c
@@ -12,15 +12,10 @@
 #include "kgsl_pool.h"
 #include "kgsl_sharedmem.h"
 
-#define KGSL_MAX_POOLS 4
-#define KGSL_MAX_POOL_ORDER 8
-#define KGSL_MAX_RESERVED_PAGES 4096
-
 /**
  * struct kgsl_page_pool - Structure to hold information for the pool
  * @pool_order: Page order describing the size of the page
  * @page_count: Number of pages currently present in the pool
- * @reserved_pages: Number of pages reserved at init for the pool
  * @allocation_allowed: Tells if reserved pool gets exhausted, can we allocate
  * from system memory
  * @list_lock: Spinlock for page list in the pool
@@ -29,29 +24,37 @@
 struct kgsl_page_pool {
 	unsigned int pool_order;
 	int page_count;
-	unsigned int reserved_pages;
 	bool allocation_allowed;
 	spinlock_t list_lock;
 	struct list_head page_list;
 };
 
-static struct kgsl_page_pool kgsl_pools[KGSL_MAX_POOLS];
+static struct kgsl_page_pool kgsl_pools[4];
 static int kgsl_num_pools;
 static int kgsl_pool_max_pages;
 
+static void kgsl_pool_free_page(struct page *page);
 
-/* Returns KGSL pool corresponding to input page order*/
-static struct kgsl_page_pool *
-_kgsl_get_pool_from_order(unsigned int order)
+/* Return the index of the pool for the specified order */
+static int kgsl_get_pool_index(int order)
 {
 	int i;
 
 	for (i = 0; i < kgsl_num_pools; i++) {
 		if (kgsl_pools[i].pool_order == order)
-			return &kgsl_pools[i];
+			return i;
 	}
 
-	return NULL;
+	return -EINVAL;
+}
+
+/* Returns KGSL pool corresponding to input page order*/
+static struct kgsl_page_pool *
+_kgsl_get_pool_from_order(int order)
+{
+	int index = kgsl_get_pool_index(order);
+
+	return index >= 0 ? &kgsl_pools[index] : NULL;
 }
 
 /* Map the page into kernel and zero it out */
@@ -74,6 +77,9 @@ _kgsl_pool_zero_page(struct page *p, unsigned int pool_order)
 static void
 _kgsl_pool_add_page(struct kgsl_page_pool *pool, struct page *p)
 {
+	if (!p)
+		return;
+
 	_kgsl_pool_zero_page(p, pool->pool_order);
 
 	spin_lock(&pool->list_lock);
@@ -102,27 +108,20 @@ _kgsl_pool_get_page(struct kgsl_page_pool *pool)
 	return p;
 }
 
-/* Returns the number of pages in specified pool */
-static int
-kgsl_pool_size(struct kgsl_page_pool *kgsl_pool)
-{
-	int size;
-
-	spin_lock(&kgsl_pool->list_lock);
-	size = kgsl_pool->page_count * (1 << kgsl_pool->pool_order);
-	spin_unlock(&kgsl_pool->list_lock);
-
-	return size;
-}
-
 /* Returns the number of pages in all kgsl page pools */
 static int kgsl_pool_size_total(void)
 {
 	int i;
 	int total = 0;
 
-	for (i = 0; i < kgsl_num_pools; i++)
-		total += kgsl_pool_size(&kgsl_pools[i]);
+	for (i = 0; i < kgsl_num_pools; i++) {
+		struct kgsl_page_pool *kgsl_pool = &kgsl_pools[i];
+
+		spin_lock(&kgsl_pool->list_lock);
+		total += kgsl_pool->page_count * (1 << kgsl_pool->pool_order);
+		spin_unlock(&kgsl_pool->list_lock);
+	}
+
 	return total;
 }
 
@@ -142,13 +141,11 @@ _kgsl_pool_shrink(struct kgsl_page_pool *pool, int num_pages)
 	for (j = 0; j < num_pages >> pool->pool_order; j++) {
 		struct page *page = _kgsl_pool_get_page(pool);
 
-		if (page != NULL) {
-			__free_pages(page, pool->pool_order);
-			pcount += (1 << pool->pool_order);
-		} else {
-			/* Break as this pool is empty */
+		if (!page)
 			break;
-		}
+
+		__free_pages(page, pool->pool_order);
+		pcount += (1 << pool->pool_order);
 	}
 
 	return pcount;
@@ -238,19 +235,11 @@ void kgsl_pool_free_sgt(struct sg_table *sgt)
 	}
 }
 
-/**
- * kgsl_pool_free_pages() - Free pages in the pages array
- * @pages: pointer of the pages array
- *
- * Free the pages by collapsing any physical adjacent pages.
- * Pages are added back to the pool, if pool has sufficient space
- * otherwise they are given back to system.
- */
 void kgsl_pool_free_pages(struct page **pages, unsigned int pcount)
 {
 	int i;
 
-	if (pages == NULL || pcount == 0)
+	if (!pages)
 		return;
 
 	if (WARN(!kern_addr_valid((unsigned long)pages),
@@ -271,16 +260,6 @@ void kgsl_pool_free_pages(struct page **pages, unsigned int pcount)
 		kgsl_pool_free_page(p);
 	}
 }
-static int kgsl_pool_idx_lookup(unsigned int order)
-{
-	int i;
-
-	for (i = 0; i < kgsl_num_pools; i++)
-		if (order == kgsl_pools[i].pool_order)
-			return i;
-
-	return -ENOMEM;
-}
 
 static int kgsl_pool_get_retry_order(unsigned int order)
 {
@@ -293,9 +272,9 @@ static int kgsl_pool_get_retry_order(unsigned int order)
 	return 0;
 }
 
-static unsigned int kgsl_gfp_mask(unsigned int page_order)
+static gfp_t kgsl_gfp_mask(int page_order)
 {
-	unsigned int gfp_mask = __GFP_HIGHMEM;
+	gfp_t gfp_mask = __GFP_HIGHMEM;
 
 	if (page_order > 0) {
 		gfp_mask |= __GFP_COMP | __GFP_NORETRY | __GFP_NOWARN;
@@ -309,7 +288,36 @@ static unsigned int kgsl_gfp_mask(unsigned int page_order)
 	return gfp_mask;
 }
 
-/**
+/*
+ * Return true if the pool of specified page size is supported
+ * or no pools are supported otherwise return false.
+ */
+static bool kgsl_pool_available(unsigned int page_size)
+{
+	int order = ilog2(page_size >> PAGE_SHIFT);
+
+	if (!kgsl_num_pools)
+		return true;
+
+	return (kgsl_get_pool_index(order) >= 0);
+}
+
+static int kgsl_get_page_size(size_t size, unsigned int align)
+{
+	if (align >= ilog2(SZ_1M) && size >= SZ_1M &&
+		kgsl_pool_available(SZ_1M))
+		return SZ_1M;
+	else if (align >= ilog2(SZ_64K) && size >= SZ_64K &&
+		kgsl_pool_available(SZ_64K))
+		return SZ_64K;
+	else if (align >= ilog2(SZ_8K) && size >= SZ_8K &&
+		kgsl_pool_available(SZ_8K))
+		return SZ_8K;
+
+	return PAGE_SIZE;
+}
+
+/*
  * kgsl_pool_alloc_page() - Allocate a page of requested size
  * @page_size: Size of the page to be allocated
  * @pages: pointer to hold list of pages, should be big enough to hold
@@ -318,7 +326,7 @@ static unsigned int kgsl_gfp_mask(unsigned int page_order)
  *
  * Return total page count on success and negative value on failure
  */
-int kgsl_pool_alloc_page(int *page_size, struct page **pages,
+static int kgsl_pool_alloc_page(int *page_size, struct page **pages,
 			unsigned int pages_len, unsigned int *align)
 {
 	int j;
@@ -372,7 +380,7 @@ int kgsl_pool_alloc_page(int *page_size, struct page **pages,
 		}
 	}
 
-	pool_idx = kgsl_pool_idx_lookup(order);
+	pool_idx = kgsl_get_pool_index(order);
 	page = _kgsl_pool_get_page(pool);
 
 	/* Allocate a new page if not allocated from pool */
@@ -413,13 +421,65 @@ done:
 	return pcount;
 
 eagain:
-	*page_size = kgsl_get_page_size(size,
-			ilog2(size));
+	*page_size = kgsl_get_page_size(size, ilog2(size));
 	*align = ilog2(*page_size);
 	return -EAGAIN;
 }
 
-void kgsl_pool_free_page(struct page *page)
+int kgsl_pool_alloc_pages(u64 size, struct page ***pages)
+{
+	int count = 0;
+	int npages = size >> PAGE_SHIFT;
+	struct page **local = kvcalloc(npages, sizeof(*local), GFP_KERNEL);
+	u32 page_size, align;
+	u64 len = size;
+
+	if (!local)
+		return -ENOMEM;
+
+	/* Start with 1MB alignment to get the biggest page we can */
+	align = ilog2(SZ_1M);
+
+	page_size = kgsl_get_page_size(len, align);
+
+	while (len) {
+		int ret = kgsl_pool_alloc_page(&page_size, &local[count],
+			npages, &align);
+
+		if (ret == -EAGAIN)
+			continue;
+		else if (ret <= 0) {
+			int i;
+
+			for (i = 0; i < count; ) {
+				int n = 1 << compound_order(local[i]);
+
+				kgsl_pool_free_page(local[i]);
+				i += n;
+			}
+			kvfree(local);
+
+			if (!kgsl_sharedmem_get_noretry()) {
+				pr_err_ratelimited("kgsl: out of memory: only allocated %lldKb of %lldKb requested\n",
+					(size - len) >> 10, size >> 10);
+
+				return -ENOMEM;
+			}
+		}
+
+		count += ret;
+		npages -= ret;
+		len -= page_size;
+
+		page_size = kgsl_get_page_size(len, align);
+	}
+
+	*pages = local;
+
+	return count;
+}
+
+static void kgsl_pool_free_page(struct page *page)
 {
 	struct kgsl_page_pool *pool;
 	int page_order;
@@ -445,42 +505,6 @@ void kgsl_pool_free_page(struct page *page)
 	__free_pages(page, page_order);
 }
 
-/*
- * Return true if the pool of specified page size is supported
- * or no pools are supported otherwise return false.
- */
-bool kgsl_pool_avaialable(int page_size)
-{
-	int i;
-
-	if (!kgsl_num_pools)
-		return true;
-
-	for (i = 0; i < kgsl_num_pools; i++)
-		if (ilog2(page_size >> PAGE_SHIFT) == kgsl_pools[i].pool_order)
-			return true;
-
-	return false;
-}
-
-static void kgsl_pool_reserve_pages(void)
-{
-	int i, j;
-
-	for (i = 0; i < kgsl_num_pools; i++) {
-		struct page *page;
-
-		for (j = 0; j < kgsl_pools[i].reserved_pages; j++) {
-			int order = kgsl_pools[i].pool_order;
-			gfp_t gfp_mask = kgsl_gfp_mask(order);
-
-			page = alloc_pages(gfp_mask, order);
-			if (page != NULL)
-				_kgsl_pool_add_page(&kgsl_pools[i], page);
-		}
-	}
-}
-
 /* Functions for the shrinker */
 
 static unsigned long
@@ -514,79 +538,86 @@ static struct shrinker kgsl_pool_shrinker = {
 	.batch = 0,
 };
 
-static void kgsl_pool_config(unsigned int order, unsigned int reserved_pages,
-		bool allocation_allowed)
+static void kgsl_pool_reserve_pages(struct kgsl_page_pool *pool,
+		struct device_node *node)
 {
-#ifdef CONFIG_ALLOC_BUFFERS_IN_4K_CHUNKS
-	if (order > 0) {
-		pr_err("kgsl: pool order:%d not supprted\n", order);
-		return;
-	}
-#endif
-	if ((order > KGSL_MAX_POOL_ORDER) ||
-			(reserved_pages > KGSL_MAX_RESERVED_PAGES))
-		return;
+	u32 reserved = 0;
+	int i;
 
-	kgsl_pools[kgsl_num_pools].pool_order = order;
-	kgsl_pools[kgsl_num_pools].reserved_pages = reserved_pages;
-	kgsl_pools[kgsl_num_pools].allocation_allowed = allocation_allowed;
-	spin_lock_init(&kgsl_pools[kgsl_num_pools].list_lock);
-	INIT_LIST_HEAD(&kgsl_pools[kgsl_num_pools].page_list);
-	kgsl_num_pools++;
+	of_property_read_u32(node, "qcom,mempool-reserved", &reserved);
+
+	/* Limit the total number of reserved pages to 4096 */
+	reserved = min_t(u32, reserved, 4096);
+
+	for (i = 0; i < reserved; i++) {
+		gfp_t gfp_mask = kgsl_gfp_mask(pool->pool_order);
+		struct page *page;
+
+		page = alloc_pages(gfp_mask, pool->pool_order);
+		_kgsl_pool_add_page(pool, page);
+	}
 }
 
-static void kgsl_of_parse_mempools(struct device_node *node)
+static int kgsl_of_parse_mempool(struct kgsl_page_pool *pool,
+		struct device_node *node)
 {
-	struct device_node *child;
-	unsigned int page_size, reserved_pages = 0;
-	bool allocation_allowed;
+	u32 size;
+	int order;
 
-	for_each_child_of_node(node, child) {
-		unsigned int index;
+	if (of_property_read_u32(node, "qcom,mempool-page-size", &size))
+		return -EINVAL;
 
-		if (of_property_read_u32(child, "reg", &index))
-			return;
+	order = ilog2(size >> PAGE_SHIFT);
 
-		if (index >= KGSL_MAX_POOLS)
-			continue;
-
-		if (of_property_read_u32(child, "qcom,mempool-page-size",
-					&page_size))
-			return;
-
-		of_property_read_u32(child, "qcom,mempool-reserved",
-				&reserved_pages);
-
-		allocation_allowed = of_property_read_bool(child,
-				"qcom,mempool-allocate");
-
-		kgsl_pool_config(ilog2(page_size >> PAGE_SHIFT), reserved_pages,
-				allocation_allowed);
+	if (order > 8) {
+		pr_err("kgsl: %pOF: pool order %d is too big\n", node, order);
+		return -EINVAL;
 	}
+
+	pool->pool_order = order;
+	pool->allocation_allowed = of_property_read_bool(node,
+		"qcom,mempool-allocate");
+
+	spin_lock_init(&pool->list_lock);
+	INIT_LIST_HEAD(&pool->page_list);
+
+	kgsl_pool_reserve_pages(pool, node);
+
+	return 0;
 }
 
 static void kgsl_of_get_mempools(struct device_node *parent)
 {
-	struct device_node *node;
+	struct device_node *node, *child;
+	int index = 0;
 
 	node = of_find_compatible_node(parent, NULL, "qcom,gpu-mempools");
-	if (node != NULL) {
-		/* Get Max pages limit for mempool */
-		of_property_read_u32(node, "qcom,mempool-max-pages",
-				&kgsl_pool_max_pages);
-		kgsl_of_parse_mempools(node);
+	if (!node)
+		return;
+
+	/* Get Max pages limit for mempool */
+	of_property_read_u32(node, "qcom,mempool-max-pages",
+			&kgsl_pool_max_pages);
+
+	for_each_child_of_node(node, child) {
+		if (!kgsl_of_parse_mempool(&kgsl_pools[index], child))
+			index++;
+
+		if (index == ARRAY_SIZE(kgsl_pools)) {
+			of_node_put(child);
+			break;
+		}
 	}
+
+	kgsl_num_pools = index;
+	of_node_put(node);
 }
 
 void kgsl_init_page_pools(struct platform_device *pdev)
 {
-
 	/* Get GPU mempools data and configure pools */
 	kgsl_of_get_mempools(pdev->dev.of_node);
 
-	/* Reserve the appropriate number of pages for each pool */
-	kgsl_pool_reserve_pages();
-
 	/* Initialize shrinker */
 	register_shrinker(&kgsl_pool_shrinker);
 }
diff --git a/drivers/gpu/msm/kgsl_pool.h b/drivers/gpu/msm/kgsl_pool.h
index 1bfa3233a654..dbddced7f277 100644
--- a/drivers/gpu/msm/kgsl_pool.h
+++ b/drivers/gpu/msm/kgsl_pool.h
@@ -6,12 +6,29 @@
 #define __KGSL_POOL_H
 
 void kgsl_pool_free_sgt(struct sg_table *sgt);
+
+/**
+ * kgsl_pool_alloc_pages - Allocate an array of pages from the pool
+ * @size: Size of the allocation
+ * @pages: Pointer to an array of pages
+ *
+ * Allocate a list of pages and store it in the pointer pointed to by @pages.
+ * Return: The number of entries in the array pointed to by @page or negative
+ * on error.
+ */
+int kgsl_pool_alloc_pages(u64 size, struct page ***pages);
+
+/**
+ * kgsl_pool_free_pages - Free pages in an pages array
+ * @pages: pointer to an array of page structs
+ * @page_count: Number of entries in @pages
+ *
+ * Free the pages by collapsing any physical adjacent pages.
+ * Pages are added back to the pool, if pool has sufficient space
+ * otherwise they are given back to system.
+ */
 void kgsl_pool_free_pages(struct page **pages, unsigned int page_count);
 void kgsl_init_page_pools(struct platform_device *pdev);
 void kgsl_exit_page_pools(void);
-int kgsl_pool_alloc_page(int *page_size, struct page **pages,
-			unsigned int pages_len, unsigned int *align);
-void kgsl_pool_free_page(struct page *p);
-bool kgsl_pool_avaialable(int size);
 #endif /* __KGSL_POOL_H */
 
diff --git a/drivers/gpu/msm/kgsl_sharedmem.c b/drivers/gpu/msm/kgsl_sharedmem.c
index fa3f7fdb8950..37d4399d4f3e 100644
--- a/drivers/gpu/msm/kgsl_sharedmem.c
+++ b/drivers/gpu/msm/kgsl_sharedmem.c
@@ -10,6 +10,7 @@
 #include <linux/random.h>
 
 #include "kgsl_device.h"
+#include "kgsl_pool.h"
 #include "kgsl_sharedmem.h"
 
 /*
@@ -966,59 +967,6 @@ int kgsl_memdesc_sg_dma(struct kgsl_memdesc *memdesc,
 	return 0;
 }
 
-static struct page **kgsl_pool_alloc_pages(u64 size, u32 *pages_count)
-{
-	int count = 0;
-	int npages = size >> PAGE_SHIFT;
-	struct page **pages = kvcalloc(npages, sizeof(*pages), GFP_KERNEL);
-	u32 page_size;
-	u64 len = size;
-	u32 align = ilog2(SZ_1M);
-
-	if (!pages)
-		return ERR_PTR(-ENOMEM);
-
-	/*
-	 * Start with an 1M alignment to get the largest supported pagesize we
-	 * can
-	 */
-	page_size = kgsl_get_page_size(len, align);
-
-	while (len) {
-		int ret = kgsl_pool_alloc_page(&page_size, &pages[count],
-			npages, &align);
-
-		if (ret == -EAGAIN)
-			continue;
-		else if (ret <= 0) {
-			int i;
-
-			for (i = 0; i < count; ) {
-				int n = 1 << compound_order(pages[i]);
-
-				kgsl_pool_free_page(pages[i]);
-				i += n;
-			}
-			kvfree(pages);
-
-			if (!sharedmem_noretry_flag)
-				pr_err_ratelimited("kgsl: out of memory: only allocated %lldKb of %lldKb requested\n",
-					(size - len) >> 10, size >> 10);
-
-			return ERR_PTR(-ENOMEM);
-		}
-
-		count += ret;
-		npages -= ret;
-		len -= page_size;
-
-		page_size = kgsl_get_page_size(len, align);
-	}
-
-	*pages_count = count;
-	return pages;
-}
-
 static void _kgsl_contiguous_free(struct kgsl_memdesc *memdesc)
 {
 	dma_free_attrs(memdesc->dev, memdesc->size,
@@ -1126,10 +1074,10 @@ static int kgsl_alloc_secure_pages(struct kgsl_device *device,
 	memdesc->priv |= priv;
 
 	memdesc->ops = &kgsl_secure_pool_ops;
-	pages = kgsl_pool_alloc_pages(size, &count);
+	count = kgsl_pool_alloc_pages(size, &pages);
 
-	if (IS_ERR(pages))
-		return PTR_ERR(pages);
+	if (count < 0)
+		return count;
 
 	sgt = kzalloc(sizeof(*sgt), GFP_KERNEL);
 	if (!sgt) {
@@ -1181,10 +1129,10 @@ static int kgsl_alloc_pages(struct kgsl_device *device,
 	memdesc->priv |= priv;
 
 	memdesc->ops = &kgsl_pool_ops;
-	pages = kgsl_pool_alloc_pages(size, &count);
+	count = kgsl_pool_alloc_pages(size, &pages);
 
-	if (IS_ERR(pages))
-		return PTR_ERR(pages);
+	if (count < 0)
+		return count;
 
 	memdesc->pages = pages;
 	memdesc->size = size;
diff --git a/drivers/gpu/msm/kgsl_sharedmem.h b/drivers/gpu/msm/kgsl_sharedmem.h
index 3f609b0d537b..0a1b807c3d34 100644
--- a/drivers/gpu/msm/kgsl_sharedmem.h
+++ b/drivers/gpu/msm/kgsl_sharedmem.h
@@ -350,35 +350,4 @@ static inline void kgsl_free_sgt(struct sg_table *sgt)
 	}
 }
 
-#include "kgsl_pool.h"
-
-/**
- * kgsl_get_page_size() - Get supported pagesize
- * @size: Size of the page
- * @align: Desired alignment of the size
- *
- * Return supported pagesize
- */
-#ifndef CONFIG_ALLOC_BUFFERS_IN_4K_CHUNKS
-static inline int kgsl_get_page_size(size_t size, unsigned int align)
-{
-	if (align >= ilog2(SZ_1M) && size >= SZ_1M &&
-		kgsl_pool_avaialable(SZ_1M))
-		return SZ_1M;
-	else if (align >= ilog2(SZ_64K) && size >= SZ_64K &&
-		kgsl_pool_avaialable(SZ_64K))
-		return SZ_64K;
-	else if (align >= ilog2(SZ_8K) && size >= SZ_8K &&
-		kgsl_pool_avaialable(SZ_8K))
-		return SZ_8K;
-	else
-		return PAGE_SIZE;
-}
-#else
-static inline int kgsl_get_page_size(size_t size, unsigned int align)
-{
-	return PAGE_SIZE;
-}
-#endif
-
 #endif /* __KGSL_SHAREDMEM_H */