target/linux/mediatek/patches-5.10/330-mtk-bmt-support.patch

   1 --- a/drivers/mtd/nand/Kconfig
   2 +++ b/drivers/mtd/nand/Kconfig
   3 @@ -15,6 +15,10 @@ config MTD_NAND_ECC
   4         bool
   5         depends on MTD_NAND_CORE
   6
   7 +config MTD_NAND_MTK_BMT
   8 +       bool "Support MediaTek NAND Bad-block Management Table"
   9 +       default n
  10 +
  11  endmenu
  12
  13  endmenu
  14 --- a/drivers/mtd/nand/Makefile
  15 +++ b/drivers/mtd/nand/Makefile
  16 @@ -2,6 +2,7 @@
  17
  18  nandcore-objs := core.o bbt.o
  19  obj-$(CONFIG_MTD_NAND_CORE) += nandcore.o
  20 +obj-$(CONFIG_MTD_NAND_MTK_BMT) += mtk_bmt.o
  21
  22  obj-y  += onenand/
  23  obj-y  += raw/
  24 --- /dev/null
  25 +++ b/drivers/mtd/nand/mtk_bmt.c
  26 @@ -0,0 +1,788 @@
  27 +/*
  28 + * Copyright (c) 2017 MediaTek Inc.
  29 + * Author: Xiangsheng Hou <xiangsheng.hou@mediatek.com>
  30 + * Copyright (c) 2020 Felix Fietkau <nbd@nbd.name>
  31 + *
  32 + * This program is free software; you can redistribute it and/or modify
  33 + * it under the terms of the GNU General Public License version 2 as
  34 + * published by the Free Software Foundation.
  35 + *
  36 + * This program is distributed in the hope that it will be useful,
  37 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
  38 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  39 + * GNU General Public License for more details.
  40 + */
  41 +
  42 +#include <linux/slab.h>
  43 +#include <linux/gfp.h>
  44 +#include <linux/kernel.h>
  45 +#include <linux/of.h>
  46 +#include <linux/mtd/mtd.h>
  47 +#include <linux/mtd/partitions.h>
  48 +#include <linux/mtd/mtk_bmt.h>
  49 +#include <linux/module.h>
  50 +#include <linux/debugfs.h>
  51 +
  52 +#define MAIN_SIGNATURE_OFFSET   0
  53 +#define OOB_SIGNATURE_OFFSET    1
  54 +#define BBPOOL_RATIO           2
  55 +
  56 +#define BBT_LOG(fmt, ...) pr_debug("[BBT][%s|%d] "fmt"\n", __func__, __LINE__, ##__VA_ARGS__)
  57 +
  58 +/* Maximum 8k blocks */
  59 +#define BB_TABLE_MAX   bmtd.table_size
  60 +#define BMT_TABLE_MAX  (BB_TABLE_MAX * BBPOOL_RATIO / 100)
  61 +#define BMT_TBL_DEF_VAL        0x0
  62 +
  63 +/*
  64 + * Burner Bad Block Table
  65 + * --------- Only support SLC Nand Chips!!!!!!!!!!! ----------
  66 + */
  67 +
  68 +struct bbbt {
  69 +       char signature[3];
  70 +       /* This version is used to distinguish the legacy and new algorithm */
  71 +#define BBMT_VERSION           2
  72 +       unsigned char version;
  73 +       /* Below 2 tables will be written in SLC */
  74 +       u16 bb_tbl[];
  75 +};
  76 +
  77 +struct bbmt {
  78 +       u16 block;
  79 +#define NO_MAPPED              0
  80 +#define NORMAL_MAPPED  1
  81 +#define BMT_MAPPED             2
  82 +       u16 mapped;
  83 +};
  84 +
  85 +static struct bmt_desc {
  86 +       struct mtd_info *mtd;
  87 +
  88 +       int (*_read_oob) (struct mtd_info *mtd, loff_t from,
  89 +                         struct mtd_oob_ops *ops);
  90 +       int (*_write_oob) (struct mtd_info *mtd, loff_t to,
  91 +                          struct mtd_oob_ops *ops);
  92 +       int (*_erase) (struct mtd_info *mtd, struct erase_info *instr);
  93 +       int (*_block_isbad) (struct mtd_info *mtd, loff_t ofs);
  94 +       int (*_block_markbad) (struct mtd_info *mtd, loff_t ofs);
  95 +
  96 +       struct bbbt *bbt;
  97 +
  98 +       struct dentry *debugfs_dir;
  99 +
 100 +       u32 table_size;
 101 +       u32 pg_size;
 102 +       u32 blk_size;
 103 +       u16 pg_shift;
 104 +       u16 blk_shift;
 105 +       /* bbt logical address */
 106 +       u16 pool_lba;
 107 +       /* bbt physical address */
 108 +       u16 pool_pba;
 109 +       /* Maximum count of bad blocks that the vendor guaranteed */
 110 +       u16 bb_max;
 111 +       /* Total blocks of the Nand Chip */
 112 +       u16 total_blks;
 113 +       /* The block(n) BMT is located at (bmt_tbl[n]) */
 114 +       u16 bmt_blk_idx;
 115 +       /* How many pages needs to store 'struct bbbt' */
 116 +       u32 bmt_pgs;
 117 +
 118 +       /* to compensate for driver level remapping */
 119 +       u8 oob_offset;
 120 +} bmtd = {0};
 121 +
 122 +static unsigned char *nand_bbt_buf;
 123 +static unsigned char *nand_data_buf;
 124 +
 125 +/* -------- Unit conversions -------- */
 126 +static inline u32 blk_pg(u16 block)
 127 +{
 128 +       return (u32)(block << (bmtd.blk_shift - bmtd.pg_shift));
 129 +}
 130 +
 131 +/* -------- Nand operations wrapper -------- */
 132 +static inline int
 133 +bbt_nand_read(u32 page, unsigned char *dat, int dat_len,
 134 +             unsigned char *fdm, int fdm_len)
 135 +{
 136 +       struct mtd_oob_ops ops = {
 137 +               .mode = MTD_OPS_PLACE_OOB,
 138 +               .ooboffs = bmtd.oob_offset,
 139 +               .oobbuf = fdm,
 140 +               .ooblen = fdm_len,
 141 +               .datbuf = dat,
 142 +               .len = dat_len,
 143 +       };
 144 +
 145 +       return bmtd._read_oob(bmtd.mtd, page << bmtd.pg_shift, &ops);
 146 +}
 147 +
 148 +static inline int bbt_nand_erase(u16 block)
 149 +{
 150 +       struct mtd_info *mtd = bmtd.mtd;
 151 +       struct erase_info instr = {
 152 +               .addr = (loff_t)block << bmtd.blk_shift,
 153 +               .len = bmtd.blk_size,
 154 +       };
 155 +
 156 +       return bmtd._erase(mtd, &instr);
 157 +}
 158 +
 159 +/* -------- Bad Blocks Management -------- */
 160 +static inline struct bbmt *bmt_tbl(struct bbbt *bbbt)
 161 +{
 162 +       return (struct bbmt *)&bbbt->bb_tbl[bmtd.table_size];
 163 +}
 164 +
 165 +static int
 166 +read_bmt(u16 block, unsigned char *dat, unsigned char *fdm, int fdm_len)
 167 +{
 168 +       u32 len = bmtd.bmt_pgs << bmtd.pg_shift;
 169 +
 170 +       return bbt_nand_read(blk_pg(block), dat, len, fdm, fdm_len);
 171 +}
 172 +
 173 +static int write_bmt(u16 block, unsigned char *dat)
 174 +{
 175 +       struct mtd_oob_ops ops = {
 176 +               .mode = MTD_OPS_PLACE_OOB,
 177 +               .ooboffs = OOB_SIGNATURE_OFFSET + bmtd.oob_offset,
 178 +               .oobbuf = "bmt",
 179 +               .ooblen = 3,
 180 +               .datbuf = dat,
 181 +               .len = bmtd.bmt_pgs << bmtd.pg_shift,
 182 +       };
 183 +       loff_t addr = (loff_t)block << bmtd.blk_shift;
 184 +
 185 +       return bmtd._write_oob(bmtd.mtd, addr, &ops);
 186 +}
 187 +
 188 +static u16 find_valid_block(u16 block)
 189 +{
 190 +       u8 fdm[4];
 191 +       int ret;
 192 +       int loop = 0;
 193 +
 194 +retry:
 195 +       if (block >= bmtd.total_blks)
 196 +               return 0;
 197 +
 198 +       ret = bbt_nand_read(blk_pg(block), nand_data_buf, bmtd.pg_size,
 199 +                           fdm, sizeof(fdm));
 200 +       /* Read the 1st byte of FDM to judge whether it's a bad
 201 +        * or not
 202 +        */
 203 +       if (ret || fdm[0] != 0xff) {
 204 +               pr_info("nand: found bad block 0x%x\n", block);
 205 +               if (loop >= bmtd.bb_max) {
 206 +                       pr_info("nand: FATAL ERR: too many bad blocks!!\n");
 207 +                       return 0;
 208 +               }
 209 +
 210 +               loop++;
 211 +               block++;
 212 +               goto retry;
 213 +       }
 214 +
 215 +       return block;
 216 +}
 217 +
 218 +/* Find out all bad blocks, and fill in the mapping table */
 219 +static int scan_bad_blocks(struct bbbt *bbt)
 220 +{
 221 +       int i;
 222 +       u16 block = 0;
 223 +
 224 +       /* First time download, the block0 MUST NOT be a bad block,
 225 +        * this is guaranteed by vendor
 226 +        */
 227 +       bbt->bb_tbl[0] = 0;
 228 +
 229 +       /*
 230 +        * Construct the mapping table of Normal data area(non-PMT/BMTPOOL)
 231 +        * G - Good block; B - Bad block
 232 +        *                      ---------------------------
 233 +        * physical |G|G|B|G|B|B|G|G|G|G|B|G|B|
 234 +        *                      ---------------------------
 235 +        * What bb_tbl[i] looks like:
 236 +        *   physical block(i):
 237 +        *                       0 1 2 3 4 5 6 7 8 9 a b c
 238 +        *   mapped block(bb_tbl[i]):
 239 +        *                       0 1 3 6 7 8 9 b ......
 240 +        * ATTENTION:
 241 +        *              If new bad block ocurred(n), search bmt_tbl to find
 242 +        *              a available block(x), and fill in the bb_tbl[n] = x;
 243 +        */
 244 +       for (i = 1; i < bmtd.pool_lba; i++) {
 245 +               bbt->bb_tbl[i] = find_valid_block(bbt->bb_tbl[i - 1] + 1);
 246 +               BBT_LOG("bb_tbl[0x%x] = 0x%x", i, bbt->bb_tbl[i]);
 247 +               if (bbt->bb_tbl[i] == 0)
 248 +                       return -1;
 249 +       }
 250 +
 251 +       /* Physical Block start Address of BMT pool */
 252 +       bmtd.pool_pba = bbt->bb_tbl[i - 1] + 1;
 253 +       if (bmtd.pool_pba >= bmtd.total_blks - 2) {
 254 +               pr_info("nand: FATAL ERR: Too many bad blocks!!\n");
 255 +               return -1;
 256 +       }
 257 +
 258 +       BBT_LOG("pool_pba=0x%x", bmtd.pool_pba);
 259 +       i = 0;
 260 +       block = bmtd.pool_pba;
 261 +       /*
 262 +        * The bmt table is used for runtime bad block mapping
 263 +        * G - Good block; B - Bad block
 264 +        *                      ---------------------------
 265 +        * physical |G|G|B|G|B|B|G|G|G|G|B|G|B|
 266 +        *                      ---------------------------
 267 +        *   block:      0 1 2 3 4 5 6 7 8 9 a b c
 268 +        * What bmt_tbl[i] looks like in initial state:
 269 +        *   i:
 270 +        *                       0 1 2 3 4 5 6 7
 271 +        *   bmt_tbl[i].block:
 272 +        *                       0 1 3 6 7 8 9 b
 273 +        *   bmt_tbl[i].mapped:
 274 +        *                       N N N N N N N B
 275 +        *              N - Not mapped(Available)
 276 +        *              M - Mapped
 277 +        *              B - BMT
 278 +        * ATTENTION:
 279 +        *              BMT always in the last valid block in pool
 280 +        */
 281 +       while ((block = find_valid_block(block)) != 0) {
 282 +               bmt_tbl(bbt)[i].block = block;
 283 +               bmt_tbl(bbt)[i].mapped = NO_MAPPED;
 284 +               BBT_LOG("bmt_tbl[%d].block = 0x%x", i, block);
 285 +               block++;
 286 +               i++;
 287 +       }
 288 +
 289 +       /* i - How many available blocks in pool, which is the length of bmt_tbl[]
 290 +        * bmtd.bmt_blk_idx - bmt_tbl[bmtd.bmt_blk_idx].block => the BMT block
 291 +        */
 292 +       bmtd.bmt_blk_idx = i - 1;
 293 +       bmt_tbl(bbt)[bmtd.bmt_blk_idx].mapped = BMT_MAPPED;
 294 +
 295 +       if (i < 1) {
 296 +               pr_info("nand: FATAL ERR: no space to store BMT!!\n");
 297 +               return -1;
 298 +       }
 299 +
 300 +       pr_info("[BBT] %d available blocks in BMT pool\n", i);
 301 +
 302 +       return 0;
 303 +}
 304 +
 305 +static bool is_valid_bmt(unsigned char *buf, unsigned char *fdm)
 306 +{
 307 +       struct bbbt *bbt = (struct bbbt *)buf;
 308 +       u8 *sig = (u8*)bbt->signature + MAIN_SIGNATURE_OFFSET;
 309 +
 310 +
 311 +       if (memcmp(bbt->signature + MAIN_SIGNATURE_OFFSET, "BMT", 3) == 0 &&
 312 +               memcmp(fdm + OOB_SIGNATURE_OFFSET, "bmt", 3) == 0) {
 313 +               if (bbt->version == BBMT_VERSION)
 314 +                       return true;
 315 +       }
 316 +       BBT_LOG("[BBT] BMT Version not match,upgrage preloader and uboot please! sig=%02x%02x%02x, fdm=%02x%02x%02x",
 317 +               sig[0], sig[1], sig[2],
 318 +               fdm[1], fdm[2], fdm[3]);
 319 +       return false;
 320 +}
 321 +
 322 +static u16 get_bmt_index(struct bbmt *bmt)
 323 +{
 324 +       int i = 0;
 325 +
 326 +       while (bmt[i].block != BMT_TBL_DEF_VAL) {
 327 +               if (bmt[i].mapped == BMT_MAPPED)
 328 +                       return i;
 329 +               i++;
 330 +       }
 331 +       return 0;
 332 +}
 333 +
 334 +static struct bbbt *scan_bmt(u16 block)
 335 +{
 336 +       u8 fdm[4];
 337 +
 338 +       if (block < bmtd.pool_lba)
 339 +               return NULL;
 340 +
 341 +       if (read_bmt(block, nand_bbt_buf, fdm, sizeof(fdm)))
 342 +               return scan_bmt(block - 1);
 343 +
 344 +       if (is_valid_bmt(nand_bbt_buf, fdm)) {
 345 +               bmtd.bmt_blk_idx = get_bmt_index(bmt_tbl((struct bbbt *)nand_bbt_buf));
 346 +               if (bmtd.bmt_blk_idx == 0) {
 347 +                       pr_info("[BBT] FATAL ERR: bmt block index is wrong!\n");
 348 +                       return NULL;
 349 +               }
 350 +               pr_info("[BBT] BMT.v2 is found at 0x%x\n", block);
 351 +               return (struct bbbt *)nand_bbt_buf;
 352 +       } else
 353 +               return scan_bmt(block - 1);
 354 +}
 355 +
 356 +/* Write the Burner Bad Block Table to Nand Flash
 357 + * n - write BMT to bmt_tbl[n]
 358 + */
 359 +static u16 upload_bmt(struct bbbt *bbt, int n)
 360 +{
 361 +       u16 block;
 362 +
 363 +retry:
 364 +       if (n < 0 || bmt_tbl(bbt)[n].mapped == NORMAL_MAPPED) {
 365 +               pr_info("nand: FATAL ERR: no space to store BMT!\n");
 366 +               return (u16)-1;
 367 +       }
 368 +
 369 +       block = bmt_tbl(bbt)[n].block;
 370 +       BBT_LOG("n = 0x%x, block = 0x%x", n, block);
 371 +       if (bbt_nand_erase(block)) {
 372 +               bmt_tbl(bbt)[n].block = 0;
 373 +               /* erase failed, try the previous block: bmt_tbl[n - 1].block */
 374 +               n--;
 375 +               goto retry;
 376 +       }
 377 +
 378 +       /* The signature offset is fixed set to 0,
 379 +        * oob signature offset is fixed set to 1
 380 +        */
 381 +       memcpy(bbt->signature + MAIN_SIGNATURE_OFFSET, "BMT", 3);
 382 +       bbt->version = BBMT_VERSION;
 383 +
 384 +       if (write_bmt(block, (unsigned char *)bbt)) {
 385 +               bmt_tbl(bbt)[n].block = 0;
 386 +
 387 +               /* write failed, try the previous block in bmt_tbl[n - 1] */
 388 +               n--;
 389 +               goto retry;
 390 +       }
 391 +
 392 +       /* Return the current index(n) of BMT pool (bmt_tbl[n]) */
 393 +       return n;
 394 +}
 395 +
 396 +static u16 find_valid_block_in_pool(struct bbbt *bbt)
 397 +{
 398 +       int i;
 399 +
 400 +       if (bmtd.bmt_blk_idx == 0)
 401 +               goto error;
 402 +
 403 +       for (i = 0; i < bmtd.bmt_blk_idx; i++) {
 404 +               if (bmt_tbl(bbt)[i].block != 0 && bmt_tbl(bbt)[i].mapped == NO_MAPPED) {
 405 +                       bmt_tbl(bbt)[i].mapped = NORMAL_MAPPED;
 406 +                       return bmt_tbl(bbt)[i].block;
 407 +               }
 408 +       }
 409 +
 410 +error:
 411 +       pr_info("nand: FATAL ERR: BMT pool is run out!\n");
 412 +       return 0;
 413 +}
 414 +
 415 +/* We met a bad block, mark it as bad and map it to a valid block in pool,
 416 + * if it's a write failure, we need to write the data to mapped block
 417 + */
 418 +static bool update_bmt(u16 block)
 419 +{
 420 +       u16 mapped_blk;
 421 +       struct bbbt *bbt;
 422 +
 423 +       bbt = bmtd.bbt;
 424 +       mapped_blk = find_valid_block_in_pool(bbt);
 425 +       if (mapped_blk == 0)
 426 +               return false;
 427 +
 428 +       /* Map new bad block to available block in pool */
 429 +       bbt->bb_tbl[block] = mapped_blk;
 430 +       bmtd.bmt_blk_idx = upload_bmt(bbt, bmtd.bmt_blk_idx);
 431 +
 432 +       return true;
 433 +}
 434 +
 435 +u16 get_mapping_block_index(int block)
 436 +{
 437 +       int mapping_block;
 438 +
 439 +       if (block < bmtd.pool_lba)
 440 +               mapping_block = bmtd.bbt->bb_tbl[block];
 441 +       else
 442 +               mapping_block = block;
 443 +       BBT_LOG("0x%x mapped to 0x%x", block, mapping_block);
 444 +
 445 +       return mapping_block;
 446 +}
 447 +
 448 +static int
 449 +mtk_bmt_read(struct mtd_info *mtd, loff_t from,
 450 +            struct mtd_oob_ops *ops)
 451 +{
 452 +       struct mtd_oob_ops cur_ops = *ops;
 453 +       int retry_count = 0;
 454 +       loff_t cur_from;
 455 +       int ret;
 456 +
 457 +       ops->retlen = 0;
 458 +       ops->oobretlen = 0;
 459 +
 460 +       while (ops->retlen < ops->len || ops->oobretlen < ops->ooblen) {
 461 +               u32 offset = from & (bmtd.blk_size - 1);
 462 +               u32 block = from >> bmtd.blk_shift;
 463 +               u32 cur_block;
 464 +
 465 +               cur_block = get_mapping_block_index(block);
 466 +               cur_from = ((loff_t)cur_block << bmtd.blk_shift) + offset;
 467 +
 468 +               cur_ops.oobretlen = 0;
 469 +               cur_ops.retlen = 0;
 470 +               cur_ops.len = min_t(u32, mtd->erasesize - offset,
 471 +                                        ops->len - ops->retlen);
 472 +               ret = bmtd._read_oob(mtd, cur_from, &cur_ops);
 473 +               if (ret < 0) {
 474 +                       update_bmt(block);
 475 +                       if (retry_count++ < 10)
 476 +                               continue;
 477 +
 478 +                       return ret;
 479 +               }
 480 +
 481 +               ops->retlen += cur_ops.retlen;
 482 +               ops->oobretlen += cur_ops.oobretlen;
 483 +
 484 +               cur_ops.ooboffs = 0;
 485 +               cur_ops.datbuf += cur_ops.retlen;
 486 +               cur_ops.oobbuf += cur_ops.oobretlen;
 487 +               cur_ops.ooblen -= cur_ops.oobretlen;
 488 +
 489 +               if (!cur_ops.len)
 490 +                       cur_ops.len = mtd->erasesize - offset;
 491 +
 492 +               from += cur_ops.len;
 493 +               retry_count = 0;
 494 +       }
 495 +
 496 +       return 0;
 497 +}
 498 +
 499 +static int
 500 +mtk_bmt_write(struct mtd_info *mtd, loff_t to,
 501 +             struct mtd_oob_ops *ops)
 502 +{
 503 +       struct mtd_oob_ops cur_ops = *ops;
 504 +       int retry_count = 0;
 505 +       loff_t cur_to;
 506 +       int ret;
 507 +
 508 +       ops->retlen = 0;
 509 +       ops->oobretlen = 0;
 510 +
 511 +       while (ops->retlen < ops->len || ops->oobretlen < ops->ooblen) {
 512 +               u32 offset = to & (bmtd.blk_size - 1);
 513 +               u32 block = to >> bmtd.blk_shift;
 514 +               u32 cur_block;
 515 +
 516 +               cur_block = get_mapping_block_index(block);
 517 +               cur_to = ((loff_t)cur_block << bmtd.blk_shift) + offset;
 518 +
 519 +               cur_ops.oobretlen = 0;
 520 +               cur_ops.retlen = 0;
 521 +               cur_ops.len = min_t(u32, bmtd.blk_size - offset,
 522 +                                        ops->len - ops->retlen);
 523 +               ret = bmtd._write_oob(mtd, cur_to, &cur_ops);
 524 +               if (ret < 0) {
 525 +                       update_bmt(block);
 526 +                       if (retry_count++ < 10)
 527 +                               continue;
 528 +
 529 +                       return ret;
 530 +               }
 531 +
 532 +               ops->retlen += cur_ops.retlen;
 533 +               ops->oobretlen += cur_ops.oobretlen;
 534 +
 535 +               cur_ops.ooboffs = 0;
 536 +               cur_ops.datbuf += cur_ops.retlen;
 537 +               cur_ops.oobbuf += cur_ops.oobretlen;
 538 +               cur_ops.ooblen -= cur_ops.oobretlen;
 539 +
 540 +               if (!cur_ops.len)
 541 +                       cur_ops.len = mtd->erasesize - offset;
 542 +
 543 +               to += cur_ops.len;
 544 +               retry_count = 0;
 545 +       }
 546 +
 547 +       return 0;
 548 +}
 549 +
 550 +static int
 551 +mtk_bmt_mtd_erase(struct mtd_info *mtd, struct erase_info *instr)
 552 +{
 553 +       struct erase_info mapped_instr = {
 554 +               .len = bmtd.blk_size,
 555 +       };
 556 +       int retry_count = 0;
 557 +       u64 start_addr, end_addr;
 558 +       int ret;
 559 +       u16 orig_block, block;
 560 +
 561 +       start_addr = instr->addr & (~mtd->erasesize_mask);
 562 +       end_addr = instr->addr + instr->len;
 563 +
 564 +       while (start_addr < end_addr) {
 565 +               orig_block = start_addr >> bmtd.blk_shift;
 566 +               block = get_mapping_block_index(orig_block);
 567 +               mapped_instr.addr = (loff_t)block << bmtd.blk_shift;
 568 +               ret = bmtd._erase(mtd, &mapped_instr);
 569 +               if (ret) {
 570 +                       update_bmt(orig_block);
 571 +                       if (retry_count++ < 10)
 572 +                               continue;
 573 +                       instr->fail_addr = start_addr;
 574 +                       break;
 575 +               }
 576 +               start_addr += mtd->erasesize;
 577 +               retry_count = 0;
 578 +       }
 579 +
 580 +       return ret;
 581 +}
 582 +static int
 583 +mtk_bmt_block_isbad(struct mtd_info *mtd, loff_t ofs)
 584 +{
 585 +       int retry_count = 0;
 586 +       u16 orig_block = ofs >> bmtd.blk_shift;
 587 +       u16 block;
 588 +       int ret;
 589 +
 590 +retry:
 591 +       block = get_mapping_block_index(orig_block);
 592 +       ret = bmtd._block_isbad(mtd, (loff_t)block << bmtd.blk_shift);
 593 +       if (ret) {
 594 +               update_bmt(orig_block);
 595 +               if (retry_count++ < 10)
 596 +                       goto retry;
 597 +       }
 598 +       return ret;
 599 +}
 600 +
 601 +static int
 602 +mtk_bmt_block_markbad(struct mtd_info *mtd, loff_t ofs)
 603 +{
 604 +       u16 orig_block = ofs >> bmtd.blk_shift;
 605 +       u16 block = get_mapping_block_index(orig_block);
 606 +       update_bmt(orig_block);
 607 +       return bmtd._block_markbad(mtd, (loff_t)block << bmtd.blk_shift);
 608 +}
 609 +
 610 +static void
 611 +mtk_bmt_replace_ops(struct mtd_info *mtd)
 612 +{
 613 +       bmtd._read_oob = mtd->_read_oob;
 614 +       bmtd._write_oob = mtd->_write_oob;
 615 +       bmtd._erase = mtd->_erase;
 616 +       bmtd._block_isbad = mtd->_block_isbad;
 617 +       bmtd._block_markbad = mtd->_block_markbad;
 618 +
 619 +       mtd->_read_oob = mtk_bmt_read;
 620 +       mtd->_write_oob = mtk_bmt_write;
 621 +       mtd->_erase = mtk_bmt_mtd_erase;
 622 +       mtd->_block_isbad = mtk_bmt_block_isbad;
 623 +       mtd->_block_markbad = mtk_bmt_block_markbad;
 624 +}
 625 +
 626 +static int mtk_bmt_debug_mark_good(void *data, u64 val)
 627 +{
 628 +       u32 block = val >> bmtd.blk_shift;
 629 +
 630 +       bmtd.bbt->bb_tbl[block] = block;
 631 +       bmtd.bmt_blk_idx = upload_bmt(bmtd.bbt, bmtd.bmt_blk_idx);
 632 +
 633 +       return 0;
 634 +}
 635 +
 636 +static int mtk_bmt_debug_mark_bad(void *data, u64 val)
 637 +{
 638 +       u32 block = val >> bmtd.blk_shift;
 639 +
 640 +       update_bmt(block);
 641 +
 642 +       return 0;
 643 +}
 644 +
 645 +DEFINE_DEBUGFS_ATTRIBUTE(fops_mark_good, NULL, mtk_bmt_debug_mark_good, "%llu\n");
 646 +DEFINE_DEBUGFS_ATTRIBUTE(fops_mark_bad, NULL, mtk_bmt_debug_mark_bad, "%llu\n");
 647 +
 648 +static void
 649 +mtk_bmt_add_debugfs(void)
 650 +{
 651 +       struct dentry *dir;
 652 +
 653 +       dir = bmtd.debugfs_dir = debugfs_create_dir("mtk-bmt", NULL);
 654 +       if (!dir)
 655 +               return;
 656 +
 657 +       debugfs_create_file_unsafe("mark_good", S_IWUSR, dir, NULL, &fops_mark_good);
 658 +       debugfs_create_file_unsafe("mark_bad", S_IWUSR, dir, NULL, &fops_mark_bad);
 659 +}
 660 +
 661 +void mtk_bmt_detach(struct mtd_info *mtd)
 662 +{
 663 +       if (bmtd.mtd != mtd)
 664 +               return;
 665 +
 666 +       if (bmtd.debugfs_dir)
 667 +               debugfs_remove_recursive(bmtd.debugfs_dir);
 668 +       bmtd.debugfs_dir = NULL;
 669 +
 670 +       kfree(nand_bbt_buf);
 671 +       kfree(nand_data_buf);
 672 +
 673 +       mtd->_read_oob = bmtd._read_oob;
 674 +       mtd->_write_oob = bmtd._write_oob;
 675 +       mtd->_erase = bmtd._erase;
 676 +       mtd->_block_isbad = bmtd._block_isbad;
 677 +       mtd->_block_markbad = bmtd._block_markbad;
 678 +       mtd->size = bmtd.total_blks << bmtd.blk_shift;
 679 +
 680 +       memset(&bmtd, 0, sizeof(bmtd));
 681 +}
 682 +
 683 +/* total_blocks - The total count of blocks that the Nand Chip has */
 684 +int mtk_bmt_attach(struct mtd_info *mtd)
 685 +{
 686 +       struct device_node *np;
 687 +       struct bbbt *bbt;
 688 +       u32 bufsz;
 689 +       u32 block;
 690 +       u16 total_blocks, pmt_block;
 691 +       int ret = 0;
 692 +       u32 bmt_pool_size, bmt_table_size;
 693 +
 694 +       if (bmtd.mtd)
 695 +               return -ENOSPC;
 696 +
 697 +       np = mtd_get_of_node(mtd);
 698 +       if (!np)
 699 +               return 0;
 700 +
 701 +       if (!of_property_read_bool(np, "mediatek,bmt-v2"))
 702 +               return 0;
 703 +
 704 +       if (of_property_read_u32(np, "mediatek,bmt-pool-size",
 705 +                                &bmt_pool_size) != 0)
 706 +               bmt_pool_size = 80;
 707 +
 708 +       if (of_property_read_u8(np, "mediatek,bmt-oob-offset",
 709 +                                &bmtd.oob_offset) != 0)
 710 +               bmtd.oob_offset = 0;
 711 +
 712 +       if (of_property_read_u32(np, "mediatek,bmt-table-size",
 713 +                                &bmt_table_size) != 0)
 714 +               bmt_table_size = 0x2000U;
 715 +
 716 +       bmtd.mtd = mtd;
 717 +       mtk_bmt_replace_ops(mtd);
 718 +
 719 +       bmtd.table_size = bmt_table_size;
 720 +       bmtd.blk_size = mtd->erasesize;
 721 +       bmtd.blk_shift = ffs(bmtd.blk_size) - 1;
 722 +       bmtd.pg_size = mtd->writesize;
 723 +       bmtd.pg_shift = ffs(bmtd.pg_size) - 1;
 724 +       total_blocks = mtd->size >> bmtd.blk_shift;
 725 +       pmt_block = total_blocks - bmt_pool_size - 2;
 726 +
 727 +       mtd->size = pmt_block << bmtd.blk_shift;
 728 +
 729 +       /*
 730 +        *  ---------------------------------------
 731 +        * | PMT(2blks) | BMT POOL(totalblks * 2%) |
 732 +        *  ---------------------------------------
 733 +        * ^            ^
 734 +        * |            |
 735 +        * pmt_block    pmt_block + 2blocks(pool_lba)
 736 +        *
 737 +        * ATTETION!!!!!!
 738 +        *     The blocks ahead of the boundary block are stored in bb_tbl
 739 +        *     and blocks behind are stored in bmt_tbl
 740 +        */
 741 +
 742 +       bmtd.pool_lba = (u16)(pmt_block + 2);
 743 +       bmtd.total_blks = total_blocks;
 744 +       bmtd.bb_max = bmtd.total_blks * BBPOOL_RATIO / 100;
 745 +
 746 +       /* 3 buffers we need */
 747 +       bufsz = round_up(sizeof(struct bbbt) +
 748 +                        bmt_table_size * sizeof(struct bbmt), bmtd.pg_size);
 749 +       bmtd.bmt_pgs = bufsz >> bmtd.pg_shift;
 750 +
 751 +       nand_bbt_buf = kzalloc(bufsz, GFP_KERNEL);
 752 +       nand_data_buf = kzalloc(bmtd.pg_size, GFP_KERNEL);
 753 +
 754 +       if (!nand_bbt_buf || !nand_data_buf) {
 755 +               pr_info("nand: FATAL ERR: allocate buffer failed!\n");
 756 +               ret = -1;
 757 +               goto error;
 758 +       }
 759 +
 760 +       memset(nand_bbt_buf, 0xff, bufsz);
 761 +       memset(nand_data_buf, 0xff, bmtd.pg_size);
 762 +
 763 +       BBT_LOG("bbtbuf=0x%p(0x%x) dat=0x%p(0x%x)",
 764 +                       nand_bbt_buf, bufsz, nand_data_buf, bmtd.pg_size);
 765 +       BBT_LOG("pool_lba=0x%x total_blks=0x%x bb_max=0x%x",
 766 +                       bmtd.pool_lba, bmtd.total_blks, bmtd.bb_max);
 767 +
 768 +       /* Scanning start from the first page of the last block
 769 +        * of whole flash
 770 +        */
 771 +       bbt = scan_bmt(bmtd.total_blks - 1);
 772 +       if (!bbt) {
 773 +               /* BMT not found */
 774 +               if (bmtd.total_blks > BB_TABLE_MAX + BMT_TABLE_MAX) {
 775 +                       pr_info("nand: FATAL: Too many blocks, can not support!\n");
 776 +                       ret = -1;
 777 +                       goto error;
 778 +               }
 779 +
 780 +               bbt = (struct bbbt *)nand_bbt_buf;
 781 +               memset(bmt_tbl(bbt), BMT_TBL_DEF_VAL, bmtd.table_size * sizeof(struct bbmt));
 782 +
 783 +               if (scan_bad_blocks(bbt)) {
 784 +                       ret = -1;
 785 +                       goto error;
 786 +               }
 787 +
 788 +               /* BMT always in the last valid block in pool */
 789 +               bmtd.bmt_blk_idx = upload_bmt(bbt, bmtd.bmt_blk_idx);
 790 +               block = bmt_tbl(bbt)[bmtd.bmt_blk_idx].block;
 791 +               pr_notice("[BBT] BMT.v2 is written into PBA:0x%x\n", block);
 792 +
 793 +               if (bmtd.bmt_blk_idx == 0)
 794 +                       pr_info("nand: Warning: no available block in BMT pool!\n");
 795 +               else if (bmtd.bmt_blk_idx == (u16)-1) {
 796 +                       ret = -1;
 797 +                       goto error;
 798 +               }
 799 +       }
 800 +       mtk_bmt_add_debugfs();
 801 +
 802 +       bmtd.bbt = bbt;
 803 +       return 0;
 804 +
 805 +error:
 806 +       mtk_bmt_detach(mtd);
 807 +       return ret;
 808 +}
 809 +
 810 +
 811 +MODULE_LICENSE("GPL");
 812 +MODULE_AUTHOR("Xiangsheng Hou <xiangsheng.hou@mediatek.com>, Felix Fietkau <nbd@nbd.name>");
 813 +MODULE_DESCRIPTION("Bad Block mapping management v2 for MediaTek NAND Flash Driver");
 814 +
 815 --- /dev/null
 816 +++ b/include/linux/mtd/mtk_bmt.h
 817 @@ -0,0 +1,18 @@
 818 +#ifndef __MTK_BMT_H
 819 +#define __MTK_BMT_H
 820 +
 821 +#ifdef CONFIG_MTD_NAND_MTK_BMT
 822 +int mtk_bmt_attach(struct mtd_info *mtd);
 823 +void mtk_bmt_detach(struct mtd_info *mtd);
 824 +#else
 825 +static inline int mtk_bmt_attach(struct mtd_info *mtd)
 826 +{
 827 +       return 0;
 828 +}
 829 +
 830 +static inline void mtk_bmt_detach(struct mtd_info *mtd)
 831 +{
 832 +}
 833 +#endif
 834 +
 835 +#endif
 836 --- a/drivers/mtd/mtk-snand/mtk-snand-mtd.c
 837 +++ b/drivers/mtd/mtk-snand/mtk-snand-mtd.c
 838 @@ -16,6 +16,7 @@
 839  #include <linux/dma-mapping.h>
 840  #include <linux/wait.h>
 841  #include <linux/mtd/mtd.h>
 842 +#include <linux/mtd/mtk_bmt.h>
 843  #include <linux/mtd/partitions.h>
 844  #include <linux/of_platform.h>
 845
 846 @@ -612,6 +613,8 @@ static int mtk_snand_probe(struct platfo
 847         mtd->_block_isbad = mtk_snand_mtd_block_isbad;
 848         mtd->_block_markbad = mtk_snand_mtd_block_markbad;
 849
 850 +       mtk_bmt_attach(mtd);
 851 +
 852         ret = mtd_device_register(mtd, NULL, 0);
 853         if (ret) {
 854                 dev_err(msm->pdev.dev, "failed to register mtd partition\n");
 855 @@ -623,6 +626,7 @@ static int mtk_snand_probe(struct platfo
 856         return 0;
 857
 858  errout4:
 859 +       mtk_bmt_detach(mtd);
 860         devm_kfree(msm->pdev.dev, msm->page_cache);
 861
 862  errout3:
 863 @@ -650,6 +654,8 @@ static int mtk_snand_remove(struct platf
 864         if (ret)
 865                 return ret;
 866
 867 +       mtk_bmt_detach(mtd);
 868 +
 869         mtk_snand_cleanup(msm->snf);
 870
 871         if (msm->irq >= 0)