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