1 /*
2 * Copyright (c) 2021 Huawei Device Co., Ltd.
3 *
4 * HDF is dual licensed: you can use it either under the terms of
5 * the GPL, or the BSD license, at your option.
6 * See the LICENSE file in the root of this repository for complete details.
7 */
8
9 #include "mtd_core.h"
10 #include "securec.h"
11
12 #include "hdf_log.h"
13 #include "mtd_block.h"
14 #include "mtd_char.h"
15 #include "platform_core.h"
16
MtdDeviceCheckParms(struct MtdDevice * mtdDevice)17 static int32_t MtdDeviceCheckParms(struct MtdDevice *mtdDevice)
18 {
19 if (mtdDevice->index < 0 || mtdDevice->index >= MTD_DEVICE_NUM_MAX) {
20 HDF_LOGE("%s: invalid index: %d", __func__, mtdDevice->index);
21 return HDF_ERR_INVALID_OBJECT;
22 }
23
24 if (mtdDevice->name == NULL) {
25 HDF_LOGE("%s: name is NULL", __func__);
26 return HDF_ERR_INVALID_OBJECT;
27 }
28
29 if (mtdDevice->type >= MTD_TYPE_MAX) {
30 HDF_LOGE("%s: invalid mtd type:%d", __func__, mtdDevice->type);
31 return HDF_ERR_INVALID_OBJECT;
32 }
33
34 if (mtdDevice->idLen > MTD_FLASH_ID_LEN_MAX) {
35 HDF_LOGE("%s: invalid idLen:%u", __func__, mtdDevice->idLen);
36 return HDF_ERR_INVALID_OBJECT;
37 }
38
39 if (mtdDevice->idLen > MTD_FLASH_ID_LEN_MAX) {
40 HDF_LOGE("%s: invalid idLen:%u", __func__, mtdDevice->idLen);
41 return HDF_ERR_INVALID_OBJECT;
42 }
43
44 if (mtdDevice->capacity == 0) {
45 HDF_LOGE("%s: invalid capacity:%zu", __func__, mtdDevice->capacity);
46 return HDF_ERR_INVALID_OBJECT;
47 }
48
49 if (mtdDevice->eraseSize == 0) {
50 HDF_LOGE("%s: invalid erase size:%zu", __func__, mtdDevice->eraseSize);
51 return HDF_ERR_INVALID_OBJECT;
52 }
53
54 if (mtdDevice->writeSize == 0) {
55 HDF_LOGE("%s: invalid write size:%zu", __func__, mtdDevice->writeSize);
56 return HDF_ERR_INVALID_OBJECT;
57 }
58
59 if (mtdDevice->readSize == 0) {
60 HDF_LOGE("%s: invalid read size:%zu", __func__, mtdDevice->readSize);
61 return HDF_ERR_INVALID_OBJECT;
62 }
63
64 return HDF_SUCCESS;
65 }
66
MtdDeviceDumpDefault(struct MtdDevice * mtdDevice)67 static void MtdDeviceDumpDefault(struct MtdDevice *mtdDevice)
68 {
69 if (mtdDevice != NULL) {
70 MTD_DEVICE_DUMP(mtdDevice);
71 }
72 return;
73 }
74
MtdDeviceDump(struct MtdDevice * mtdDevice)75 static void MtdDeviceDump(struct MtdDevice *mtdDevice)
76 {
77 if (mtdDevice != NULL && mtdDevice->ops != NULL && mtdDevice->ops->dump != NULL) {
78 mtdDevice->ops->dump(mtdDevice);
79 }
80 return;
81 }
82
MtdDeviceLockDefault(struct MtdDevice * mtdDevice)83 static int32_t MtdDeviceLockDefault(struct MtdDevice *mtdDevice)
84 {
85 if (mtdDevice == NULL) {
86 return HDF_ERR_INVALID_OBJECT;
87 }
88 return OsalMutexLock(&mtdDevice->lock);
89 }
90
MtdDeviceUnlockDefault(struct MtdDevice * mtdDevice)91 static void MtdDeviceUnlockDefault(struct MtdDevice *mtdDevice)
92 {
93 if (mtdDevice != NULL) {
94 OsalMutexUnlock(&mtdDevice->lock);
95 }
96 return;
97 }
98
MtdManagerGet(void)99 struct PlatformManager *MtdManagerGet(void)
100 {
101 int32_t ret;
102 static struct PlatformManager *g_mtdManager = NULL;
103
104 if (g_mtdManager == NULL) {
105 ret = PlatformManagerCreate("STORAGE_MTD", &g_mtdManager);
106 if (ret != HDF_SUCCESS) {
107 HDF_LOGE("MtdManagerGet: create manager failed:%d", ret);
108 }
109 }
110 return g_mtdManager;
111 }
112
MtdDeviceAdd(struct MtdDevice * mtdDevice)113 int32_t MtdDeviceAdd(struct MtdDevice *mtdDevice)
114 {
115 int32_t ret;
116
117 if (mtdDevice == NULL || mtdDevice->ops == NULL) {
118 return HDF_ERR_INVALID_OBJECT;
119 }
120
121 ret = MtdDeviceCheckParms(mtdDevice);
122 if (ret != HDF_SUCCESS) {
123 return HDF_ERR_INVALID_OBJECT;
124 }
125
126 ret = OsalMutexInit(&mtdDevice->lock);
127 if (ret != HDF_SUCCESS) {
128 HDF_LOGE("%s: mutex init failed, ret=%d", __func__, ret);
129 return ret;
130 }
131
132 if (mtdDevice->ops->dump == NULL) {
133 mtdDevice->ops->dump = MtdDeviceDumpDefault;
134 }
135
136 if (mtdDevice->ops->lock == NULL || mtdDevice->ops->unlock == NULL) {
137 mtdDevice->ops->lock = MtdDeviceLockDefault;
138 mtdDevice->ops->unlock = MtdDeviceUnlockDefault;
139 }
140
141 mtdDevice->device.manager = MtdManagerGet();
142 mtdDevice->device.number = mtdDevice->index;
143 ret = PlatformDeviceAdd(&mtdDevice->device);
144 if (ret != HDF_SUCCESS) {
145 HDF_LOGE("%s: mtd device add fail", __func__);
146 return ret;
147 }
148
149 MtdDeviceDump(mtdDevice);
150
151 ret = MtdCharInit(mtdDevice);
152 if (ret != HDF_SUCCESS) {
153 PlatformDeviceDel(&mtdDevice->device);
154 return ret;
155 }
156
157 ret = MtdBlockInit(mtdDevice);
158 if (ret != HDF_SUCCESS) {
159 PlatformDeviceDel(&mtdDevice->device);
160 return ret;
161 }
162
163 return HDF_SUCCESS;
164 }
165
MtdDeviceDel(struct MtdDevice * mtdDevice)166 void MtdDeviceDel(struct MtdDevice *mtdDevice)
167 {
168 if (mtdDevice != NULL) {
169 MtdCharUninit(mtdDevice);
170 MtdBlockUninit(mtdDevice);
171 PlatformDeviceDel(&mtdDevice->device);
172 (void)OsalMutexDestroy(&mtdDevice->lock);
173 }
174 }
175
MtdDeviceLock(struct MtdDevice * mtdDevice)176 int32_t MtdDeviceLock(struct MtdDevice *mtdDevice)
177 {
178 int32_t ret;
179
180 if (mtdDevice == NULL || mtdDevice->ops == NULL) {
181 return HDF_ERR_INVALID_OBJECT;
182 }
183
184 if (mtdDevice->ops->lock == NULL) {
185 return HDF_ERR_NOT_SUPPORT;
186 }
187
188 ret = mtdDevice->ops->lock(mtdDevice);
189 if (ret != HDF_SUCCESS) {
190 HDF_LOGE("%s: lock mtd device failed, ret=%d", __func__, ret);
191 }
192 return ret;
193 }
194
MtdDeviceUnlock(struct MtdDevice * mtdDevice)195 void MtdDeviceUnlock(struct MtdDevice *mtdDevice)
196 {
197 if (mtdDevice != NULL && mtdDevice->ops != NULL && mtdDevice->ops->unlock != NULL) {
198 mtdDevice->ops->unlock(mtdDevice);
199 }
200 return;
201 }
202
MtdDumpBuf(uint8_t * buf,size_t len)203 static void MtdDumpBuf(uint8_t *buf, size_t len)
204 {
205 int ret;
206 size_t i;
207 size_t idx;
208 size_t lidx;
209 size_t line;
210 #define MTD_DUMP_SIGLE_WIDTH 2
211 #define MTD_DUMP_LINE_LEN 32
212 #define MTD_DUMP_BUF_LEN (MTD_DUMP_LINE_LEN * MTD_DUMP_SIGLE_WIDTH + 1)
213 char lineBuf[MTD_DUMP_BUF_LEN];
214 idx = 0;
215 while (idx < len) {
216 line = (MTD_DUMP_LINE_LEN <= (len - idx)) ? MTD_DUMP_LINE_LEN : (len - idx);
217 for (i = 0, lidx = 0; i < line; i++, lidx += MTD_DUMP_SIGLE_WIDTH, buf++) {
218 ret = snprintf_s(lineBuf + lidx, MTD_DUMP_SIGLE_WIDTH + 1, MTD_DUMP_SIGLE_WIDTH, "%02x", *buf);
219 if (ret < 0) {
220 HDF_LOGD("%s: format string failed, ret=%d", __func__, ret);
221 return;
222 }
223 }
224 HDF_LOGD("0x%08zx : %s", idx, lineBuf);
225 idx += line;
226 }
227 return;
228 }
229
MtdDeviceEraseUnlock(struct MtdDevice * mtdDevice,off_t addr,size_t len,off_t * faddr)230 static int32_t MtdDeviceEraseUnlock(struct MtdDevice *mtdDevice, off_t addr, size_t len, off_t *faddr)
231 {
232 int32_t ret;
233
234 if (mtdDevice == NULL || mtdDevice->ops == NULL) {
235 return HDF_ERR_INVALID_OBJECT;
236 }
237
238 if (mtdDevice->ops->erase == NULL) {
239 return HDF_ERR_NOT_SUPPORT;
240 }
241
242 ret = mtdDevice->ops->erase(mtdDevice, addr, len, faddr);
243 if (ret != HDF_SUCCESS) {
244 HDF_LOGE("%s: erase mtd device failed, addr=%jx, ret=%d", __func__, addr, ret);
245 }
246 return ret;
247 }
248
MtdDeviceErase(struct MtdDevice * mtdDevice,off_t addr,size_t len,off_t * failAddr)249 ssize_t MtdDeviceErase(struct MtdDevice *mtdDevice, off_t addr, size_t len, off_t *failAddr)
250 {
251 int32_t ret;
252
253 if ((ret = MtdDeviceLock(mtdDevice)) != HDF_SUCCESS) {
254 return ret;
255 }
256 ret = MtdDeviceEraseUnlock(mtdDevice, addr, len, failAddr);
257 MtdDeviceUnlock(mtdDevice);
258 return ret;
259 }
260
MtdDeviceIsBadBlockUnlocked(struct MtdDevice * mtdDevice,off_t addr)261 static bool MtdDeviceIsBadBlockUnlocked(struct MtdDevice *mtdDevice, off_t addr)
262 {
263 if (mtdDevice != NULL && mtdDevice->ops != NULL && mtdDevice->ops->isBadBlock != NULL) {
264 return mtdDevice->ops->isBadBlock(mtdDevice, addr);
265 }
266 return false;
267 }
268
MtdDeviceMarkBadBlockUnlocked(struct MtdDevice * mtdDevice,off_t addr)269 static int32_t MtdDeviceMarkBadBlockUnlocked(struct MtdDevice *mtdDevice, off_t addr)
270 {
271 int32_t ret;
272
273 if (mtdDevice == NULL) {
274 return HDF_ERR_INVALID_OBJECT;
275 }
276 if (mtdDevice->ops == NULL || mtdDevice->ops->markBadBlock == NULL) {
277 return HDF_ERR_NOT_SUPPORT;
278 }
279 ret = mtdDevice->ops->markBadBlock(mtdDevice, addr);
280 if (ret != HDF_SUCCESS) {
281 HDF_LOGE("%s: mark bad block failed, addr=%jx, ret=%d", __func__, addr, ret);
282 }
283 return ret;
284 }
285
MtdDeviceIsBadBlock(struct MtdDevice * mtdDevice,off_t addr)286 bool MtdDeviceIsBadBlock(struct MtdDevice *mtdDevice, off_t addr)
287 {
288 bool ret = false;
289
290 if (MtdDeviceLock(mtdDevice) != HDF_SUCCESS) {
291 return false;
292 }
293 ret = MtdDeviceIsBadBlockUnlocked(mtdDevice, addr);
294 MtdDeviceUnlock(mtdDevice);
295 return ret;
296 }
297
MtdDeviceMarkBadBlock(struct MtdDevice * mtdDevice,off_t addr)298 int32_t MtdDeviceMarkBadBlock(struct MtdDevice *mtdDevice, off_t addr)
299 {
300 int32_t ret;
301
302 if ((ret = MtdDeviceLock(mtdDevice)) != HDF_SUCCESS) {
303 return ret;
304 }
305 ret = MtdDeviceMarkBadBlockUnlocked(mtdDevice, addr);
306 MtdDeviceUnlock(mtdDevice);
307 return ret;
308 }
309
MtdDevicePageTransferUnlocked(struct MtdDevice * mtdDevice,struct MtdPage * mtdPage)310 static int32_t MtdDevicePageTransferUnlocked(struct MtdDevice *mtdDevice, struct MtdPage *mtdPage)
311 {
312 int32_t ret;
313
314 if (mtdDevice == NULL) {
315 return HDF_ERR_INVALID_OBJECT;
316 }
317
318 if (mtdDevice->ops == NULL || mtdDevice->ops->pageTransfer == NULL) {
319 return HDF_ERR_NOT_SUPPORT;
320 }
321
322 #ifdef MTD_DEBUG
323 HDF_LOGD("%s: mtdPage-> type=%d, addr=0x%jx, databuf=%p, datalen=%zu, oobbuf=%p, ooblen=%zu", __func__,
324 mtdPage->type, mtdPage->addr, mtdPage->dataBuf, mtdPage->dataLen, mtdPage->oobBuf, mtdPage->oobLen);
325 #endif
326
327 ret = mtdDevice->ops->pageTransfer(mtdDevice, mtdPage);
328 if (ret != HDF_SUCCESS) {
329 HDF_LOGE("%s: do page transfer failed, ret = %d, addr = 0x%jx", __func__, ret, mtdPage->addr);
330 }
331 return ret;
332 }
333
MtdDeviceCheckMsg(struct MtdDevice * mtdDevice,struct MtdMsg * msg)334 static int32_t MtdDeviceCheckMsg(struct MtdDevice *mtdDevice, struct MtdMsg *msg)
335 {
336 size_t oobSize;
337
338 if (mtdDevice == NULL) {
339 return HDF_ERR_INVALID_OBJECT;
340 }
341
342 if (msg == NULL) {
343 return HDF_ERR_INVALID_PARAM;
344 }
345
346 if (msg->buf == NULL) {
347 return HDF_ERR_INVALID_PARAM;
348 }
349
350 if ((msg->addr + msg->len) > mtdDevice->capacity) {
351 HDF_LOGE("%s: over range, addr=%jx, len=%zu", __func__, msg->addr, msg->len);
352 return HDF_ERR_INVALID_PARAM;
353 }
354
355 if (msg->type == MTD_MSG_TYPE_ERASE) {
356 if ((msg->addr % mtdDevice->eraseSize) != 0) {
357 HDF_LOGE("%s: not erase size aligned, addr=%jd, erase size=%zu", __func__,
358 msg->addr, mtdDevice->eraseSize);
359 return HDF_ERR_INVALID_PARAM;
360 }
361 return HDF_SUCCESS;
362 }
363
364 oobSize = (msg->withOob) ? mtdDevice->oobSize : 0;
365 if ((msg->addr % mtdDevice->writeSize) != 0 || (msg->len % (mtdDevice->writeSize + oobSize)) != 0) {
366 if (msg->type != MTD_MSG_TYPE_READ || msg->withOob) {
367 HDF_LOGE("%s: not page aligned, addr=%jd, type=%d, withOob=%d", __func__,
368 msg->addr, msg->type, msg->withOob);
369 return HDF_ERR_INVALID_PARAM;
370 }
371 return HDF_SUCCESS;
372 }
373
374 return HDF_SUCCESS;
375 }
376
MtdDeviceWriteReadByPageUnlock(struct MtdDevice * mtdDevice,struct MtdMsg * msg)377 static int32_t MtdDeviceWriteReadByPageUnlock(struct MtdDevice *mtdDevice, struct MtdMsg *msg)
378 {
379 int32_t ret;
380 off_t addr;
381 uint8_t *buf = NULL;
382 size_t dataLenLeft;
383 size_t blockSize;
384 off_t eraseOffset;
385 struct MtdPage mtdPage;
386
387 dataLenLeft = msg->withOob ?
388 (msg->len / (mtdDevice->writeSize + mtdDevice->oobSize)) * mtdDevice->writeSize : msg->len;
389 for (addr = msg->addr, buf = msg->buf; (dataLenLeft > 0) && addr < mtdDevice->capacity;) {
390 if (MtdDeviceIsBadBlockUnlocked(mtdDevice, addr)) {
391 if (!msg->skipBad) {
392 HDF_LOGE("%s: failed on bad block @0x%jx", __func__, addr);
393 return HDF_ERR_IO;
394 }
395 HDF_LOGW("%s: skip bad block @0x%jx", __func__, addr);
396 addr = (addr & ~(mtdDevice->eraseSize - 1)) + mtdDevice->eraseSize;
397 continue;
398 }
399 eraseOffset = addr & (mtdDevice->eraseSize - 1);
400 blockSize = (dataLenLeft < (mtdDevice->eraseSize - eraseOffset)) ?
401 dataLenLeft : (mtdDevice->eraseSize - eraseOffset);
402 // no more than one block at once
403 mtdPage.type = msg->type;
404 while (blockSize > 0) {
405 mtdPage.addr = addr;
406 mtdPage.dataBuf = (uint8_t *)buf;
407 mtdPage.dataLen = mtdDevice->writeSize - (addr & (mtdDevice->writeSize - 1));
408 if (mtdPage.dataLen > blockSize) {
409 mtdPage.dataLen = blockSize;
410 }
411 mtdPage.oobBuf = msg->withOob ? (buf + mtdPage.dataLen) : NULL;
412 mtdPage.oobLen = msg->withOob ? mtdDevice->oobSize : 0;
413 ret = MtdDevicePageTransferUnlocked(mtdDevice, &mtdPage);
414 if (ret != HDF_SUCCESS) {
415 MtdDumpBuf(mtdPage.dataBuf, mtdPage.dataLen + mtdPage.oobLen);
416 return ret;
417 }
418 buf += mtdPage.dataLen + mtdPage.oobLen;
419 addr += mtdPage.dataLen;
420 blockSize -= mtdPage.dataLen;
421 dataLenLeft -= mtdPage.dataLen;
422 }
423 }
424
425 if (dataLenLeft > 0) {
426 HDF_LOGE("%s: no enough space, dataLenLeft=%zu, addr=0x%jx", __func__, dataLenLeft, addr);
427 return HDF_ERR_IO;
428 }
429 return HDF_SUCCESS;
430 }
431
MtdDeviceRequest(struct MtdDevice * mtdDevice,struct MtdMsg * msg)432 static int32_t MtdDeviceRequest(struct MtdDevice *mtdDevice, struct MtdMsg *msg)
433 {
434 int32_t ret;
435
436 ret = MtdDeviceCheckMsg(mtdDevice, msg);
437 if (ret != HDF_SUCCESS) {
438 return ret;
439 }
440
441 if ((ret = MtdDeviceLock(mtdDevice)) != HDF_SUCCESS) {
442 return ret;
443 }
444
445 switch (msg->type) {
446 case MTD_MSG_TYPE_READ:
447 case MTD_MSG_TYPE_WRITE:
448 ret = MtdDeviceWriteReadByPageUnlock(mtdDevice, msg);
449 break;
450 case MTD_MSG_TYPE_ERASE:
451 ret = MtdDeviceEraseUnlock(mtdDevice, msg->addr, msg->len, &msg->faddr);
452 break;
453 default:
454 ret = HDF_ERR_NOT_SUPPORT;
455 break;
456 }
457
458 MtdDeviceUnlock(mtdDevice);
459 return ret;
460 }
461
MtdDeviceWrite(struct MtdDevice * mtdDevice,off_t to,size_t len,const uint8_t * buf)462 ssize_t MtdDeviceWrite(struct MtdDevice *mtdDevice, off_t to, size_t len, const uint8_t *buf)
463 {
464 int32_t ret;
465 struct MtdMsg msg;
466
467 if (mtdDevice == NULL) {
468 return HDF_ERR_INVALID_OBJECT;
469 }
470
471 if (mtdDevice->ops == NULL) {
472 return HDF_ERR_NOT_SUPPORT;
473 }
474
475 if (mtdDevice->ops->write != NULL) {
476 if ((ret = MtdDeviceLock(mtdDevice)) != HDF_SUCCESS) {
477 return ret;
478 }
479 ret = mtdDevice->ops->write(mtdDevice, to, len, buf);
480 MtdDeviceUnlock(mtdDevice);
481 } else {
482 msg.type = MTD_MSG_TYPE_WRITE;
483 msg.addr = to;
484 msg.buf = (uint8_t *)buf;
485 msg.len = len;
486 msg.withOob = false;
487 msg.skipBad = true;
488 ret = MtdDeviceRequest(mtdDevice, &msg);
489 }
490
491 return (ret == HDF_SUCCESS) ? len : ret;
492 }
493
MtdDeviceRead(struct MtdDevice * mtdDevice,off_t from,size_t len,uint8_t * buf)494 ssize_t MtdDeviceRead(struct MtdDevice *mtdDevice, off_t from, size_t len, uint8_t *buf)
495 {
496 int32_t ret;
497 struct MtdMsg msg;
498
499 if (mtdDevice == NULL) {
500 return HDF_ERR_INVALID_OBJECT;
501 }
502
503 if (mtdDevice->ops == NULL) {
504 return HDF_ERR_NOT_SUPPORT;
505 }
506
507 if (mtdDevice->ops->read != NULL) {
508 if ((ret = MtdDeviceLock(mtdDevice)) != HDF_SUCCESS) {
509 return ret;
510 }
511 ret = mtdDevice->ops->read(mtdDevice, from, len, buf);
512 MtdDeviceUnlock(mtdDevice);
513 } else {
514 msg.type = MTD_MSG_TYPE_READ;
515 msg.addr = from;
516 msg.buf = (uint8_t *)buf;
517 msg.len = len;
518 msg.withOob = false;
519 msg.skipBad = true;
520 ret = MtdDeviceRequest(mtdDevice, &msg);
521 }
522
523 return (ret == HDF_SUCCESS) ? len : ret;
524 }
525
MtdDeviceWriteWithOob(struct MtdDevice * mtdDevice,off_t to,size_t len,const uint8_t * buf)526 ssize_t MtdDeviceWriteWithOob(struct MtdDevice *mtdDevice, off_t to, size_t len, const uint8_t *buf)
527 {
528 int32_t ret;
529 struct MtdMsg msg;
530
531 msg.type = MTD_MSG_TYPE_WRITE;
532 msg.addr = to;
533 msg.buf = (uint8_t *)buf;
534 msg.len = len;
535 msg.withOob = true;
536 msg.skipBad = true;
537 ret = MtdDeviceRequest(mtdDevice, &msg);
538 return (ret == HDF_SUCCESS) ? len : ret;
539 }
540
MtdDeviceReadWithOob(struct MtdDevice * mtdDevice,off_t from,size_t len,uint8_t * buf)541 ssize_t MtdDeviceReadWithOob(struct MtdDevice *mtdDevice, off_t from, size_t len, uint8_t *buf)
542 {
543 int32_t ret;
544 struct MtdMsg msg;
545
546 msg.type = MTD_MSG_TYPE_READ;
547 msg.addr = from;
548 msg.buf = (uint8_t *)buf;
549 msg.len = len;
550 msg.withOob = true;
551 msg.skipBad = true;
552 ret = MtdDeviceRequest(mtdDevice, &msg);
553 return (ret == HDF_SUCCESS) ? len : ret;
554 }
555
556
557 #define MTD_FFS_SHIFT_16BIT 16
558 #define MTD_FFS_SHIFT_8BIT 8
559 #define MTD_FFS_SHIFT_4BIT 4
560 #define MTD_FFS_SHIFT_2BIT 2
561
MtdFfs(int x)562 int MtdFfs(int x)
563 {
564 int r = 1;
565 unsigned int f = (unsigned int)x;
566
567 if (f == 0) {
568 return 0;
569 }
570
571 if ((f & 0xffff) == 0) {
572 f >>= MTD_FFS_SHIFT_16BIT;
573 r += MTD_FFS_SHIFT_16BIT;
574 }
575
576 if ((f & 0xff) == 0) {
577 f >>= MTD_FFS_SHIFT_8BIT;
578 r += MTD_FFS_SHIFT_8BIT;
579 }
580
581 if ((f & 0xf) == 0) {
582 f >>= MTD_FFS_SHIFT_4BIT;
583 r += MTD_FFS_SHIFT_4BIT;
584 }
585
586 if ((f & 0x3) == 0) {
587 f >>= MTD_FFS_SHIFT_2BIT;
588 r += MTD_FFS_SHIFT_2BIT;
589 }
590
591 if ((f & 0x1) == 0) {
592 r += 1;
593 }
594
595 return r;
596 }
597