1 files changed, 58 insertions, 7 deletions

diff --git a/drivers/misc/fsl_law.c b/drivers/misc/fsl_law.c
index dca6a4da4a0..48ece4f090b 100644
--- a/drivers/misc/fsl_law.c
+++ b/drivers/misc/fsl_law.c
@@ -27,8 +27,22 @@
 #include <asm/fsl_law.h>
 #include <asm/io.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 #define LAWAR_EN	0x80000000
-#define FSL_HW_NUM_LAWS 10	/* number of LAWs in the hw implementation */
+/* number of LAWs in the hw implementation */
+#if defined(CONFIG_MPC8540) || defined(CONFIG_MPC8541) || \
+    defined(CONFIG_MPC8560) || defined(CONFIG_MPC8555)
+#define FSL_HW_NUM_LAWS 8
+#elif defined(CONFIG_MPC8548) || defined(CONFIG_MPC8544) || \
+      defined(CONFIG_MPC8568) || \
+      defined(CONFIG_MPC8641) || defined(CONFIG_MPC8610)
+#define FSL_HW_NUM_LAWS 10
+#elif defined(CONFIG_MPC8572)
+#define FSL_HW_NUM_LAWS 12
+#else
+#error FSL_HW_NUM_LAWS not defined for this platform
+#endif
 
 void set_law(u8 idx, phys_addr_t addr, enum law_size sz, enum law_trgt_if id)
 {
@@ -36,18 +50,53 @@ void set_law(u8 idx, phys_addr_t addr, enum law_size sz, enum law_trgt_if id)
 	volatile u32 *lawbar = base + 8 * idx;
 	volatile u32 *lawar = base + 8 * idx + 2;
 
+	gd->used_laws |= (1 << idx);
+
 	out_be32(lawbar, addr >> 12);
 	out_be32(lawar, LAWAR_EN | ((u32)id << 20) | (u32)sz);
 
 	return ;
 }
 
+int set_next_law(phys_addr_t addr, enum law_size sz, enum law_trgt_if id)
+{
+	u32 idx = ffz(gd->used_laws);
+
+	if (idx >= FSL_HW_NUM_LAWS)
+		return -1;
+
+	set_law(idx, addr, sz, id);
+
+	return idx;
+}
+
+int set_last_law(phys_addr_t addr, enum law_size sz, enum law_trgt_if id)
+{
+	u32 idx;
+
+	/* we have no LAWs free */
+	if (gd->used_laws == -1)
+		return -1;
+
+	/* grab the last free law */
+	idx = __ilog2(~(gd->used_laws));
+
+	if (idx >= FSL_HW_NUM_LAWS)
+		return -1;
+
+	set_law(idx, addr, sz, id);
+
+	return idx;
+}
+
 void disable_law(u8 idx)
 {
 	volatile u32 *base = (volatile u32 *)(CFG_IMMR + 0xc08);
 	volatile u32 *lawbar = base + 8 * idx;
 	volatile u32 *lawar = base + 8 * idx + 2;
 
+	gd->used_laws &= ~(1 << idx);
+
 	out_be32(lawar, 0);
 	out_be32(lawbar, 0);
 
@@ -75,14 +124,16 @@ void print_laws(void)
 void init_laws(void)
 {
 	int i;
-	u8 law_idx = 0;
 
-	for (i = 0; i < num_law_entries; i++) {
-		if (law_table[i].index != -1)
-			law_idx = law_table[i].index;
+	gd->used_laws = ~((1 << FSL_HW_NUM_LAWS) - 1);
 
-		set_law(law_idx++, law_table[i].addr,
-			law_table[i].size, law_table[i].trgt_id);
+	for (i = 0; i < num_law_entries; i++) {
+		if (law_table[i].index == -1)
+			set_next_law(law_table[i].addr, law_table[i].size,
+					law_table[i].trgt_id);
+		else
+			set_law(law_table[i].index, law_table[i].addr,
+				law_table[i].size, law_table[i].trgt_id);
 	}
 
 	return ;