mtd: spi-nor: swp: Create helpers for building the SR register

The status register contains 3 or 4 BP (Block Protect) bits, 0 or 1
TB (Top/Bottom) bit, soon 0 or 1 CMP (Complement) bit. The last BP bit
and the TB bit locations change between vendors. The whole logic of
buildling the content of the status register based on some input
conditions is used two times and soon will be used 4 times.

Create dedicated helpers for these steps.

Signed-off-by: Miquel Raynal <miquel.raynal@bootlin.com>
Signed-off-by: Pratyush Yadav <pratyush@kernel.org>

diff --git a/drivers/mtd/spi-nor/swp.c b/drivers/mtd/spi-nor/swp.c
index 540cd221c455bd13e4c8eb7c2d8fb76cec3e3b6b..8a75cd59cb100297f8fa38a3590df94a8ab4940d 100644 (file)
--- a/drivers/mtd/spi-nor/swp.c
+++ b/drivers/mtd/spi-nor/swp.c
@@ -125,6 +125,43 @@ static bool spi_nor_is_unlocked_sr(struct spi_nor *nor, loff_t ofs, u64 len,
        return spi_nor_check_lock_status_sr(nor, ofs, len, sr, false);
 }
 
+static int spi_nor_sr_set_bp_mask(struct spi_nor *nor, u8 *sr, u8 pow)
+{
+       u8 mask = spi_nor_get_sr_bp_mask(nor);
+       u8 val = pow << SR_BP_SHIFT;
+
+       if (nor->flags & SNOR_F_HAS_SR_BP3_BIT6 && val & SR_BP3)
+               val = (val & ~SR_BP3) | SR_BP3_BIT6;
+
+       if (val & ~mask)
+               return -EINVAL;
+
+       sr[0] |= val;
+
+       return 0;
+}
+
+static int spi_nor_build_sr(struct spi_nor *nor, const u8 *old_sr, u8 *new_sr,
+                           u8 pow, bool use_top)
+{
+       u8 bp_mask = spi_nor_get_sr_bp_mask(nor);
+       u8 tb_mask = spi_nor_get_sr_tb_mask(nor);
+       int ret;
+
+       new_sr[0] = old_sr[0] & ~bp_mask & ~tb_mask;
+
+       /* Build BP field */
+       ret = spi_nor_sr_set_bp_mask(nor, &new_sr[0], pow);
+       if (ret)
+               return ret;
+
+       /* Build TB field */
+       if (!use_top)
+               new_sr[0] |= tb_mask;
+
+       return 0;
+}
+
 /*
  * Lock a region of the flash. Compatible with ST Micro and similar flash.
  * Supports the block protection bits BP{0,1,2}/BP{0,1,2,3} in the status
@@ -164,11 +201,10 @@ static int spi_nor_sr_lock(struct spi_nor *nor, loff_t ofs, u64 len)
        int ret;
        u8 status_old[1] = {}, status_new[1] = {};
        u8 bp_mask = spi_nor_get_sr_bp_mask(nor);
-       u8 tb_mask = spi_nor_get_sr_tb_mask(nor);
-       u8 pow, val;
        loff_t lock_len;
        bool can_be_top = true, can_be_bottom = nor->flags & SNOR_F_HAS_SR_TB;
        bool use_top;
+       u8 pow;
 
        ret = spi_nor_read_sr(nor, nor->bouncebuf);
        if (ret)
@@ -202,24 +238,19 @@ static int spi_nor_sr_lock(struct spi_nor *nor, loff_t ofs, u64 len)
                lock_len = ofs + len;
 
        if (lock_len == nor->params->size) {
-               val = bp_mask;
+               pow = (nor->flags & SNOR_F_HAS_4BIT_BP) ? GENMASK(3, 0) : GENMASK(2, 0);
        } else {
                min_prot_len = spi_nor_get_min_prot_length_sr(nor);
                pow = ilog2(lock_len) - ilog2(min_prot_len) + 1;
-               val = pow << SR_BP_SHIFT;
-
-               if (nor->flags & SNOR_F_HAS_SR_BP3_BIT6 && val & SR_BP3)
-                       val = (val & ~SR_BP3) | SR_BP3_BIT6;
-
-               if (val & ~bp_mask)
-                       return -EINVAL;
-
-               /* Don't "lock" with no region! */
-               if (!(val & bp_mask))
-                       return -EINVAL;
        }
 
-       status_new[0] = (status_old[0] & ~bp_mask & ~tb_mask) | val;
+       ret = spi_nor_build_sr(nor, status_old, status_new, pow, use_top);
+       if (ret)
+               return ret;
+
+       /* Don't "lock" with no region! */
+       if (!(status_new[0] & bp_mask))
+               return -EINVAL;
 
        /*
         * Disallow further writes if WP# pin is neither left floating nor
@@ -229,9 +260,6 @@ static int spi_nor_sr_lock(struct spi_nor *nor, loff_t ofs, u64 len)
        if (!(nor->flags & SNOR_F_NO_WP))
                status_new[0] |= SR_SRWD;
 
-       if (!use_top)
-               status_new[0] |= tb_mask;
-
        /* Don't bother if they're the same */
        if (status_new[0] == status_old[0])
                return 0;
@@ -254,11 +282,10 @@ static int spi_nor_sr_unlock(struct spi_nor *nor, loff_t ofs, u64 len)
        int ret;
        u8 status_old[1], status_new[1];
        u8 bp_mask = spi_nor_get_sr_bp_mask(nor);
-       u8 tb_mask = spi_nor_get_sr_tb_mask(nor);
-       u8 pow, val;
        loff_t lock_len;
        bool can_be_top = true, can_be_bottom = nor->flags & SNOR_F_HAS_SR_TB;
        bool use_top;
+       u8 pow;
 
        ret = spi_nor_read_sr(nor, nor->bouncebuf);
        if (ret)
@@ -299,29 +326,21 @@ static int spi_nor_sr_unlock(struct spi_nor *nor, loff_t ofs, u64 len)
                lock_len = ofs;
 
        if (lock_len == 0) {
-               val = 0; /* fully unlocked */
+               pow = 0; /* fully unlocked */
        } else {
                min_prot_len = spi_nor_get_min_prot_length_sr(nor);
                pow = ilog2(lock_len) - ilog2(min_prot_len) + 1;
-               val = pow << SR_BP_SHIFT;
-
-               if (nor->flags & SNOR_F_HAS_SR_BP3_BIT6 && val & SR_BP3)
-                       val = (val & ~SR_BP3) | SR_BP3_BIT6;
 
-               /* Some power-of-two sizes may not be supported */
-               if (val & ~bp_mask)
-                       return -EINVAL;
        }
 
-       status_new[0] = (status_old[0] & ~bp_mask & ~tb_mask) | val;
+       ret = spi_nor_build_sr(nor, status_old, status_new, pow, use_top);
+       if (ret)
+               return ret;
 
        /* Don't protect status register if we're fully unlocked */
        if (lock_len == 0)
                status_new[0] &= ~SR_SRWD;
 
-       if (!use_top)
-               status_new[0] |= tb_mask;
-
        /* Don't bother if they're the same */
        if (status_new[0] == status_old[0])
                return 0;