disp: msm: add func to parse pll_codes from dfps_data_region

Add function to parse pll_codes from dfps_data_region, and the
pll_codes are used as trim_codes for RFI.

Change-Id: Ic81529cd685f17012809fb68cefc4b36cb1172ca
Signed-off-by: Satya Rama Aditya Pinapala <[email protected]>

msm/dsi/dsi_pll.c

@@ -11,6 +11,7 @@
 #include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/iopoll.h>
+#include <linux/of_address.h>
 #include "dsi_pll.h"
 
 static int dsi_pll_clock_register(struct platform_device *pdev,
@@ -51,6 +52,67 @@ static inline int dsi_pll_get_ioresources(struct platform_device *pdev,
 	return rc;
 }
 
+static void dsi_pll_free_bootmem(u32 mem_addr, u32 size)
+{
+	unsigned long pfn_start, pfn_end, pfn_idx;
+
+	pfn_start = mem_addr >> PAGE_SHIFT;
+	pfn_end = (mem_addr + size) >> PAGE_SHIFT;
+	for (pfn_idx = pfn_start; pfn_idx < pfn_end; pfn_idx++)
+		free_reserved_page(pfn_to_page(pfn_idx));
+}
+
+static void dsi_pll_parse_dfps(struct platform_device *pdev,
+				struct dsi_pll_resource *pll_res)
+{
+	struct device_node *pnode = NULL;
+	const u32 *addr;
+	void *trim_codes = NULL;
+	u64 size;
+	u32 offsets[2];
+
+	pnode = of_parse_phandle(pdev->dev.of_node, "memory-region", 0);
+	if (IS_ERR_OR_NULL(pnode)) {
+		DSI_PLL_INFO(pll_res, "of_parse_phandle failed\n");
+		goto node_err;
+	}
+
+	addr = of_get_address(pnode, 0, &size, NULL);
+	if (!addr) {
+		DSI_PLL_ERR(pll_res,
+			"failed to parse the dfps memory address\n");
+		goto node_err;
+	}
+
+	/* maintain compatibility for 32/64 bit */
+	offsets[0] = (u32) of_read_ulong(addr, 2);
+	offsets[1] = (u32) size;
+
+	trim_codes = memremap(offsets[0], offsets[1], MEMREMAP_WB);
+	if (!trim_codes)
+		goto mem_err;
+
+	pll_res->dfps = kzalloc(sizeof(struct dfps_info), GFP_KERNEL);
+	if (IS_ERR_OR_NULL(pll_res->dfps)) {
+		DSI_PLL_ERR(pll_res, "pll_res->dfps allocate failed\n");
+		goto mem_err;
+	}
+
+	/* memcopy complete dfps structure from kernel virtual memory */
+	memcpy_fromio(pll_res->dfps, trim_codes, sizeof(struct dfps_info));
+
+mem_err:
+	if (trim_codes)
+		memunmap(trim_codes);
+
+	/* free the dfps memory here */
+	dsi_pll_free_bootmem(offsets[0], offsets[1]);
+
+node_err:
+	if (pnode)
+		of_node_put(pnode);
+}
+
 int dsi_pll_init(struct platform_device *pdev, struct dsi_pll_resource **pll)
 {
 	int rc = 0;
@@ -145,6 +207,9 @@ int dsi_pll_init(struct platform_device *pdev, struct dsi_pll_resource **pll)
 		return -EINVAL;
 	}
 
+	if (!(pll_res->index))
+		dsi_pll_parse_dfps(pdev, pll_res);
+
 	return rc;
 
 }