1 files changed, 100 insertions, 0 deletions

diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 4eeeb2d7f6ea..5b6b0728be21 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -565,6 +565,96 @@ time_out:
         return ret;
 }
 
+static int m25p80_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
+{
+        struct m25p *flash = mtd_to_m25p(mtd);
+        uint32_t offset = ofs;
+        uint8_t status_old, status_new;
+        int res = 0;
+
+        mutex_lock(&flash->lock);
+        /* Wait until finished previous command */
+        if (wait_till_ready(flash)) {
+                res = 1;
+                goto err;
+        }
+
+        status_old = read_sr(flash);
+
+        if (offset < flash->mtd.size-(flash->mtd.size/2))
+                status_new = status_old | SR_BP2 | SR_BP1 | SR_BP0;
+        else if (offset < flash->mtd.size-(flash->mtd.size/4))
+                status_new = (status_old & ~SR_BP0) | SR_BP2 | SR_BP1;
+        else if (offset < flash->mtd.size-(flash->mtd.size/8))
+                status_new = (status_old & ~SR_BP1) | SR_BP2 | SR_BP0;
+        else if (offset < flash->mtd.size-(flash->mtd.size/16))
+                status_new = (status_old & ~(SR_BP0|SR_BP1)) | SR_BP2;
+        else if (offset < flash->mtd.size-(flash->mtd.size/32))
+                status_new = (status_old & ~SR_BP2) | SR_BP1 | SR_BP0;
+        else if (offset < flash->mtd.size-(flash->mtd.size/64))
+                status_new = (status_old & ~(SR_BP2|SR_BP0)) | SR_BP1;
+        else
+                status_new = (status_old & ~(SR_BP2|SR_BP1)) | SR_BP0;
+
+        /* Only modify protection if it will not unlock other areas */
+        if ((status_new&(SR_BP2|SR_BP1|SR_BP0)) >
+                                        (status_old&(SR_BP2|SR_BP1|SR_BP0))) {
+                write_enable(flash);
+                if (write_sr(flash, status_new) < 0) {
+                        res = 1;
+                        goto err;
+                }
+        }
+
+err:    mutex_unlock(&flash->lock);
+        return res;
+}
+
+static int m25p80_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
+{
+        struct m25p *flash = mtd_to_m25p(mtd);
+        uint32_t offset = ofs;
+        uint8_t status_old, status_new;
+        int res = 0;
+
+        mutex_lock(&flash->lock);
+        /* Wait until finished previous command */
+        if (wait_till_ready(flash)) {
+                res = 1;
+                goto err;
+        }
+
+        status_old = read_sr(flash);
+
+        if (offset+len > flash->mtd.size-(flash->mtd.size/64))
+                status_new = status_old & ~(SR_BP2|SR_BP1|SR_BP0);
+        else if (offset+len > flash->mtd.size-(flash->mtd.size/32))
+                status_new = (status_old & ~(SR_BP2|SR_BP1)) | SR_BP0;
+        else if (offset+len > flash->mtd.size-(flash->mtd.size/16))
+                status_new = (status_old & ~(SR_BP2|SR_BP0)) | SR_BP1;
+        else if (offset+len > flash->mtd.size-(flash->mtd.size/8))
+                status_new = (status_old & ~SR_BP2) | SR_BP1 | SR_BP0;
+        else if (offset+len > flash->mtd.size-(flash->mtd.size/4))
+                status_new = (status_old & ~(SR_BP0|SR_BP1)) | SR_BP2;
+        else if (offset+len > flash->mtd.size-(flash->mtd.size/2))
+                status_new = (status_old & ~SR_BP1) | SR_BP2 | SR_BP0;
+        else
+                status_new = (status_old & ~SR_BP0) | SR_BP2 | SR_BP1;
+
+        /* Only modify protection if it will not lock other areas */
+        if ((status_new&(SR_BP2|SR_BP1|SR_BP0)) <
+                                        (status_old&(SR_BP2|SR_BP1|SR_BP0))) {
+                write_enable(flash);
+                if (write_sr(flash, status_new) < 0) {
+                        res = 1;
+                        goto err;
+                }
+        }
+
+err:    mutex_unlock(&flash->lock);
+        return res;
+}
+
 /****************************************************************************/
 
 /*
@@ -642,6 +732,10 @@ static const struct spi_device_id m25p_ids[] = {
         /* Everspin */
         { "mr25h256", CAT25_INFO(  32 * 1024, 1, 256, 2) },
 
+        /* GigaDevice */
+        { "gd25q32", INFO(0xc84016, 0, 64 * 1024,  64, SECT_4K) },
+        { "gd25q64", INFO(0xc84017, 0, 64 * 1024, 128, SECT_4K) },
+
         /* Intel/Numonyx -- xxxs33b */
         { "160s33b",  INFO(0x898911, 0, 64 * 1024,  32, 0) },
         { "320s33b",  INFO(0x898912, 0, 64 * 1024,  64, 0) },
@@ -899,6 +993,12 @@ static int m25p_probe(struct spi_device *spi)
         flash->mtd._erase = m25p80_erase;
         flash->mtd._read = m25p80_read;
 
+        /* flash protection support for STmicro chips */
+        if (JEDEC_MFR(info->jedec_id) == CFI_MFR_ST) {
+                flash->mtd._lock = m25p80_lock;
+                flash->mtd._unlock = m25p80_unlock;
+        }
+
         /* sst flash chips use AAI word program */
         if (JEDEC_MFR(info->jedec_id) == CFI_MFR_SST)
                 flash->mtd._write = sst_write;