diff --git a/drivers/otp/c2k_otp.c b/drivers/otp/c2k_otp.c
index 74218a4..3642588 100644
--- a/drivers/otp/c2k_otp.c
+++ b/drivers/otp/c2k_otp.c
@@ -7,8 +7,8 @@
 #include <mach/clkcore.h>
 #include <clock.h>
 
-#define NP1	4
-#define NP2	16
+#define NP1	4       /* Number of initial programming pulses */
+#define NP2	12      /* Maximum number of additional programming pulses */
 
 extern u32 HAL_get_axi_clk(void);
 
@@ -88,7 +88,7 @@
 		return RETCODE_ERROR;
 
 	/* Setting up counters to program */
-	pgm2cpump_counter = axi_clk & 0x7FF ; 				/* 1 uSec */
+	pgm2cpump_counter = axi_clk & 0x7FF ;				/* 1 uSec */
 	cpump2web_counter = (axi_clk*3) & 0x7FF ;			/* 3 uSec */
 	web_counter = (axi_clk*5) & 0x7FF ;					/* 5 uSec */
 	web2cpump_counter = (axi_clk*2) & 0x7FF ;			/* 2 uSec */
@@ -119,18 +119,17 @@
 
 	for(i = 0 ; i < size ; i++) {
 
+		/* Skip bits that are 0 because 0 is the default value */
 		if (!prog_data[i]) {
-			/* Skip bits that are 0 because 0 is the default value.
-			 * */
 			continue;
 		}
 
 		for(k = 0 ; k < NP1-1 ; k++)
-			otp_smart_write_sequence(offset + i, prog_data[i]);
+			otp_smart_write_sequence(offset + i, 1);
 
 		for(k = 0 ; k < NP2+1 ; k++) {
 			ndelay(100);
-			otp_smart_write_sequence(offset + i, prog_data[i]);
+			otp_smart_write_sequence(offset + i, 1);
 
 			/* Verify Data */
 			read_data = readl(OTP_DATA_OUTPUT);
@@ -138,18 +137,19 @@
 			/* Adjust bit offset */
 			read_data = ((read_data >> ((offset+i) & 0x7)) & 0x1);
 
-			if(read_data == prog_data[i])
+			if(read_data)
 				break;
 		}
-		if(read_data != prog_data[i]) {
+		if(!read_data) {
 			printf("Warning : failed to write OTP value at bit %d (%d attempts) !\n",
 					offset + i, NP1 + NP2);
-			/* Not returrning Error here for debug */
+			return -1;
 		}
 	}
 
 	return 0;
 }
+EXPORT_SYMBOL(otp_write)
 
 /*
  * Reads from the OTP.
@@ -225,6 +225,99 @@
 }
 EXPORT_SYMBOL(otp_read);
 
+void otp_lock(void)
+{
+	u32 i;
+	u32 pgm2cpump_counter, cpump2web_counter, web_counter, web2cpump_counter;
+	u32 cpump2pgm_counter, dataout_counter;
+	u32 axi_clk = HAL_get_axi_clk();
+
+	/* Setting up counters to program */
+	pgm2cpump_counter = axi_clk & 0x7FF ;               /* 1 uSec */
+	cpump2web_counter = (axi_clk*3) & 0x7FF ;           /* 3 uSec */
+	web_counter = (axi_clk*5) & 0x7FF ;                 /* 5 uSec */
+	web2cpump_counter = (axi_clk*2) & 0x7FF ;           /* 2 uSec */
+	cpump2pgm_counter = axi_clk & 0x7FF ;               /* 1 uSec */
+	dataout_counter = ((axi_clk * 7 + 99) / 100) & 0x1FF ;  /* 70 nSec */
+
+	/* program the counters */
+	writel(pgm2cpump_counter, OTP_PGM2CPUMP_COUNTER);
+	writel(cpump2web_counter, OTP_CPUMP2WEB_COUNTER);
+	writel(web_counter, OTP_WEB_COUNTER);
+	writel(web2cpump_counter, OTP_WEB2CPUMP_COUNTER);
+	writel(cpump2pgm_counter, OTP_CPUMP2PGM_COUNTER);
+	writel(dataout_counter, OTP_DATA_OUT_COUNTER);
+
+	write_protect_unlock();
+	udelay(1);
+
+	/* rstb drive 0 */
+	writel(0x0, OTP_RSTB_INPUT);
+	ndelay(20);
+	/* rstb drive 1 to have pulse  */
+	writel(0x1, OTP_RSTB_INPUT);
+	udelay(1);
+
+	/* address input */
+	writel(0x8, OTP_ADDR_INPUT);
+	udelay(1);
+
+	/* CLE drive  "1" */
+	writel(0x1, OTP_CLE_INPUT);
+	/* Wait for at least 20nsec */
+	ndelay(20);
+
+	/* WEB drive  "0" */
+	writel(0x0, OTP_WEB_INPUT);
+	/* Wait for at least 20nsec */
+	ndelay(20);
+
+	/* WEB drive  "1" */
+	writel(0x1, OTP_WEB_INPUT);
+	/* Wait for at least 20nsec */
+	ndelay(20);
+
+	/* CLE drive  "0" */
+	writel(0x0, OTP_CLE_INPUT);
+
+	for( i = 0 ; i < 2 ; i++ )
+	{
+		/* PGMEN drive  "1" */
+		writel(0x1, OTP_PGMEN_INPUT);
+		udelay(100);
+
+		/* WEB drive  "0" */
+		writel(0x0, OTP_WEB_INPUT);
+		/* Wait for at least 20nsec */
+		udelay(100);
+
+		/* WEB drive  "1" */
+		writel(0x1, OTP_WEB_INPUT);
+		/* Wait for at least 20nsec */
+		udelay(100);
+	}
+
+	/* rstb drive 0 */
+	writel(0x0, OTP_RSTB_INPUT);
+	ndelay(20);
+	/* rstb drive 1 to have pulse  */
+	writel(0x1, OTP_RSTB_INPUT);
+	udelay(1);
+
+	/* read and check lock status register */
+	for (i = 0 ; i < 12 ; i++) {
+		if (readl(OTP_SECURE_LOCK_OUTPUT) & 1)
+			break;
+		udelay(100);
+	}
+
+	if (!(readl(OTP_SECURE_LOCK_OUTPUT) & 1))
+		printk("Timeout waiting for Security Lock Status Going high \n ");
+
+	return;
+}
+EXPORT_SYMBOL(otp_lock);
+
 static int c2k_otp_probe(struct device_d *pdev)
 {
 	printf("c2k_otp_probe.\n");
diff --git a/include/c2k_otp.h b/include/c2k_otp.h
index 40d75ff..6485f71 100644
--- a/include/c2k_otp.h
+++ b/include/c2k_otp.h
@@ -5,5 +5,6 @@
 
 int otp_read(u32 s_addr, u8 *read_data, int size) ;
 int otp_write(u32 offset, u8 *prog_data, int size);
+void otp_lock(void);
 
 #endif