From b9f23ba8c6e88989c8b5dc36e6dbafd92197fb6f Mon Sep 17 00:00:00 2001 From: danielzhang Date: Tue, 14 Nov 2023 19:09:30 +0800 Subject: [PATCH] update SPI NAND block device driver --- .../include/SPINAND/SPINANDBlockDevice.h | 42 ++ .../COMPONENT_SPINAND/include/SPINAND/bch.h | 43 ++ .../source/SPINANDBlockDevice.cpp | 588 ++++++++++++++++-- .../COMPONENT_SPINAND/source/bch.c | 452 ++++++++++++++ 4 files changed, 1085 insertions(+), 40 deletions(-) create mode 100644 storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/bch.h create mode 100644 storage/blockdevice/COMPONENT_SPINAND/source/bch.c diff --git a/storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/SPINANDBlockDevice.h b/storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/SPINANDBlockDevice.h index b310f53962c..9267af1b8a9 100644 --- a/storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/SPINANDBlockDevice.h +++ b/storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/SPINANDBlockDevice.h @@ -20,6 +20,7 @@ #include "drivers/QSPI.h" #include "blockdevice/BlockDevice.h" #include "platform/Callback.h" +#include "bch.h" #ifndef MBED_CONF_SPINAND_QSPI_IO0 #define MBED_CONF_SPINAND_QSPI_IO0 NC @@ -237,6 +238,10 @@ class SPINANDBlockDevice : public mbed::BlockDevice { */ virtual const char *get_type() const; + virtual bool is_bad_block(uint16_t blk_idx); + + virtual int mark_bad_block(uint16_t blk_idx); + private: /********************************/ /* Different Device Csel Mgmt */ @@ -258,6 +263,9 @@ class SPINANDBlockDevice : public mbed::BlockDevice { // Send Read command to Driver qspi_status_t _qspi_send_read_command(mbed::qspi_inst_t read_instruction, void *buffer, mbed::bd_addr_t addr, mbed::bd_size_t size); + // Send Continuous Read command to Driver + qspi_status_t _qspi_send_continuous_read_command(mbed::qspi_inst_t read_instruction, void *buffer, mbed::bd_addr_t addr, mbed::bd_size_t size); + // Send Erase Instruction using command_transfer command to Driver qspi_status_t _qspi_send_erase_command(mbed::qspi_inst_t erase_instruction, mbed::bd_addr_t addr, mbed::bd_size_t size); @@ -272,6 +280,13 @@ class SPINANDBlockDevice : public mbed::BlockDevice { /* Flash Configuration Functions */ /*********************************/ + // Read OTP ONFI parameters + bool _read_otp_onfi(); + + int _read_oob(void *buffer, bd_addr_t addr, bd_size_t size); + + int _program_oob(const void *buffer, bd_addr_t addr, bd_size_t size); + // Quad Enable in Security Register int _set_quad_enable(); @@ -281,9 +296,19 @@ class SPINANDBlockDevice : public mbed::BlockDevice { // Configure Write Enable in Status Register int _set_write_enable(); + int _set_conti_read_enable(); + + int _set_conti_read_disable(); + + int _conti_read_exit(); + // Wait on status register until write not-in-progress bool _is_mem_ready(); + void _bch_init(uint8_t ecc_bits); + void _bch_free(); + int _bch_calculate_ecc(unsigned char *buf, unsigned char *code); + int _bch_correct_data(unsigned char *buf, unsigned char *read_ecc, unsigned char *calc_ecc); private: // QSPI Driver Object @@ -320,6 +345,23 @@ class SPINANDBlockDevice : public mbed::BlockDevice { uint32_t _init_ref_count; bool _is_initialized; + char _name[32]; + uint32_t _page_size, _block_size, _flash_size; + uint8_t _page_shift, _block_shift; + uint16_t _block_num, _page_num, _oob_size; + uint8_t _ecc_bits, _ecc_bytes, _ecc_steps, _ecc_layout_pos; + uint32_t _ecc_size; + uint8_t *_ecc_calc; + uint8_t *_ecc_code; + uint8_t *_page_buf; + uint8_t _continuous_read; + + struct nand_bch_control { + struct bch_code *bch; + unsigned int *errloc; + unsigned char *eccmask; + }; + struct nand_bch_control _nbc; }; #endif diff --git a/storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/bch.h b/storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/bch.h new file mode 100644 index 00000000000..0a40effb05e --- /dev/null +++ b/storage/blockdevice/COMPONENT_SPINAND/include/SPINAND/bch.h @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2022 Macronix International Co., Ltd. + * SPDX-License-Identifier: Apache-2.0 + */ + +#ifndef _BCH_H +#define _BCH_H +#ifdef __cplusplus +extern "C" { +#endif +#include + +#define DIV_ROUND_UP(n, d) (((n) + (d) - 1) / (d)) + +struct bch_code { + unsigned int m; + unsigned int n; + unsigned int t; + unsigned int ecc_bits; + unsigned int ecc_words; + unsigned int len; + unsigned int *a_pow; + unsigned int *a_log; + unsigned int *mod_tab; + unsigned int *ecc; + unsigned int *syn; + unsigned int *elp; + unsigned int *buf; + unsigned int *buf2; + unsigned char *input_data; + unsigned int endian; +}; + +struct bch_code *bch_init(unsigned int m, unsigned int t); +void bch_free(struct bch_code *bch); +void bch_encode(struct bch_code *bch, unsigned char *data, unsigned int *ecc); +int bch_decode(struct bch_code *bch, unsigned char *data, unsigned int *ecc); +int fls(int x); + +#ifdef __cplusplus +} +#endif +#endif diff --git a/storage/blockdevice/COMPONENT_SPINAND/source/SPINANDBlockDevice.cpp b/storage/blockdevice/COMPONENT_SPINAND/source/SPINANDBlockDevice.cpp index 6f11d8b1546..9a509fd2f58 100644 --- a/storage/blockdevice/COMPONENT_SPINAND/source/SPINANDBlockDevice.cpp +++ b/storage/blockdevice/COMPONENT_SPINAND/source/SPINANDBlockDevice.cpp @@ -19,6 +19,7 @@ #include "SPINANDBlockDevice.h" #include #include "rtos/ThisThread.h" +#include "bch.h" #ifndef MBED_CONF_MBED_TRACE_ENABLE #define MBED_CONF_MBED_TRACE_ENABLE 0 @@ -50,11 +51,12 @@ using namespace mbed; #define SPINAND_STATUS_BIT_PROGRAM_FAIL 0x8 // Program failed #define SPINAND_STATUS_BIT_ECC_STATUS_MASK 0x30 // ECC status #define SPINAND_STATUS_ECC_STATUS_NO_ERR 0x00 -#define SPINAND_STATUS_ECC_STATUS_ERR_COR 0x00 -#define SPINAND_STATUS_ECC_STATUS_ERR_NO_COR 0x00 +#define SPINAND_STATUS_ECC_STATUS_ERR_COR 0x10 +#define SPINAND_STATUS_ECC_STATUS_ERR_NO_COR 0x20 // Secure OTP Register Bits #define SPINAND_SECURE_BIT_QE 0x01 // Quad enable +#define SPINAND_SECURE_BIT_CONT 0x04 // continuous read enable #define SPINAND_SECURE_BIT_ECC_EN 0x10 // On-die ECC enable #define SPINAND_SECURE_BIT_OTP_EN 0x40 // #define SPINAND_SECURE_BIT_OTP_PROT 0x80 // @@ -82,6 +84,7 @@ using namespace mbed; #define SPINAND_INST_READ_CACHE 0x03 // Read data from cache #define SPINAND_INST_READ_CACHE2 0x3B #define SPINAND_INST_READ_CACHE4 0x6B +#define SPINAND_INST_READ_CACHE144 0xEB #define SPINAND_INST_READ_CACHE_SEQ 0x31 #define SPINAND_INST_READ_CACHE_END 0x3F @@ -99,18 +102,22 @@ using namespace mbed; #define SPINAND_INST_RESET 0xFF #define SPINAND_INST_ECC_STAT_READ 0x7C +#define SPINAND_INST_EXIT_CONTI_READ 0x63 + // Default read/legacy erase instructions //#define SPINAND_INST_READ_DEFAULT SPINAND_INST_READ_CACHE //#define SPINAND_INST_READ_DEFAULT SPINAND_INST_READ_CACHE2 #define SPINAND_INST_READ_DEFAULT SPINAND_INST_READ_CACHE4 +//#define SPINAND_INST_READ_DEFAULT SPINAND_INST_READ_CACHE144 //#define SPINAND_INST_PROGRAM_DEFAULT SPINAND_INST_PP_LOAD #define SPINAND_INST_PROGRAM_DEFAULT SPINAND_INST_4PP_LOAD #define SPINAND_BLOCK_OFFSET 0x40000 #define SPINAND_PAGE_OFFSET 0x1000 +#define SPINAND_PAGE_MASK 0xFFFFF000 -#define SPI_NAND_ROW_ADDR_SIZE QSPI_CFG_ADDR_SIZE_16 -#define SPI_NAND_COLUMN_ADDR_SIZE QSPI_CFG_ADDR_SIZE_24 +#define SPI_NAND_COLUMN_ADDR_SIZE QSPI_CFG_ADDR_SIZE_16 +#define SPI_NAND_ROW_ADDR_SIZE QSPI_CFG_ADDR_SIZE_24 /* Init function to initialize Different Devices CS static list */ static PinName *generate_initialized_active_spinand_csel_arr(); @@ -189,12 +196,18 @@ int SPINANDBlockDevice::init() _alt_size = 0; _dummy_cycles = 8; + _page_shift = 12; + _ecc_bits = 0; if (QSPI_STATUS_OK != _qspi_set_frequency(_freq)) { tr_error("QSPI Set Frequency Failed"); status = SPINAND_BD_ERROR_DEVICE_ERROR; goto exit_point; } + if (!_read_otp_onfi()) { + return SPINAND_BD_ERROR_READY_FAILED; + } + // Synchronize Device if (false == _is_mem_ready()) { tr_error("Init - _is_mem_ready Failed"); @@ -208,7 +221,7 @@ int SPINANDBlockDevice::init() goto exit_point; } - if (_read_instruction == SPINAND_INST_READ_CACHE4) { + if ((_read_instruction == SPINAND_INST_READ_CACHE4) || (_program_instruction == SPINAND_INST_4PP_LOAD)) { if (QSPI_STATUS_OK != _set_quad_enable()) { tr_error("SPI NAND Set Quad enable Failed"); status = SPINAND_BD_ERROR_DEVICE_ERROR; @@ -250,6 +263,10 @@ int SPINANDBlockDevice::deinit() result = SPINAND_BD_ERROR_DEVICE_ERROR; } + if (_ecc_bits > 0) { + _bch_free(); + } + _is_initialized = false; _mutex.unlock(); @@ -264,6 +281,7 @@ int SPINANDBlockDevice::deinit() int SPINANDBlockDevice::read(void *buffer, bd_addr_t addr, bd_size_t size) { int status = SPINAND_BD_ERROR_OK; + bool read_failed = false; uint32_t offset = 0; uint32_t chunk = 0; bd_size_t read_bytes = 0; @@ -272,15 +290,77 @@ int SPINANDBlockDevice::read(void *buffer, bd_addr_t addr, bd_size_t size) while (size > 0) { // Read on _page_size_bytes boundaries (Default 2048 bytes a page) - offset = addr % MBED_CONF_SPINAND_SPINAND_PAGE_SIZE; - chunk = (offset + size < MBED_CONF_SPINAND_SPINAND_PAGE_SIZE) ? size : (MBED_CONF_SPINAND_SPINAND_PAGE_SIZE - offset); + offset = addr % _page_size; + chunk = (offset + size < _page_size) ? size : (_page_size - offset); read_bytes = chunk; _mutex.lock(); - if (QSPI_STATUS_OK != _qspi_send_read_command(_read_instruction, buffer, addr, read_bytes)) { - tr_error("Read Command failed"); - status = SPINAND_BD_ERROR_DEVICE_ERROR; + if (_ecc_bits == 0) { + if (_continuous_read) { + if (QSPI_STATUS_OK != _qspi_send_continuous_read_command(_read_instruction, buffer, addr, size)) { + tr_error("Read Command failed"); + read_failed = true; + status = SPINAND_BD_ERROR_DEVICE_ERROR; + goto exit_point; + } + } else { + if (QSPI_STATUS_OK != _qspi_send_read_command(_read_instruction, buffer, addr, read_bytes)) { + tr_error("Read Command failed"); + read_failed = true; + status = SPINAND_BD_ERROR_DEVICE_ERROR; + goto exit_point; + } + } + + uint8_t status_reg; + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_GET_FEATURE, FEATURES_ADDR_STATUS, + NULL, 0, (char *) &status_reg, 1)) { + tr_error("Reading Status Register failed"); + read_failed = true; + status = SPINAND_BD_ERROR_DEVICE_ERROR; + goto exit_point; + } + + if ((status_reg & SPINAND_STATUS_BIT_ECC_STATUS_MASK) == SPINAND_STATUS_ECC_STATUS_ERR_NO_COR) { + tr_error("Reading data failed"); + status = SPINAND_BD_ERROR_DEVICE_ERROR; + read_failed = true; + goto exit_point; + } + + if (_continuous_read) { + return status; + } + } else { + uint8_t ecc_steps = _ecc_steps; + uint8_t *p = (uint8_t *)_page_buf; + + if (QSPI_STATUS_OK != _qspi_send_read_command(_read_instruction, (void *)_page_buf, addr & SPINAND_PAGE_MASK, _page_size + _oob_size)) { + tr_error("Read Command failed"); + read_failed = true; + status = SPINAND_BD_ERROR_DEVICE_ERROR; + goto exit_point; + } + + memcpy(_ecc_code, _page_buf + _page_size + _ecc_layout_pos, _ecc_bytes * _ecc_steps); + + p = (uint8_t *)_page_buf; + ecc_steps = _ecc_steps; + for (uint8_t i = 0 ; ecc_steps; ecc_steps--, i += _ecc_bytes, p += _ecc_size) { + memset(_nbc.bch->input_data, 0x0, (1 << _nbc.bch->m) / 8); + memcpy(_nbc.bch->input_data + _ecc_bytes, p, _ecc_size); + + int res = bch_decode(_nbc.bch, _nbc.bch->input_data, (unsigned int *)(_ecc_code + i)); + if (res < 0) { + tr_error("Reading data failed"); + status = SPINAND_BD_ERROR_DEVICE_ERROR; + read_failed = true; + goto exit_point; + } + memcpy(p, _nbc.bch->input_data + _ecc_bytes, _ecc_size); + } + memcpy(buffer, _page_buf + offset, read_bytes); } buffer = static_cast< uint8_t *>(buffer) + chunk; @@ -290,6 +370,11 @@ int SPINANDBlockDevice::read(void *buffer, bd_addr_t addr, bd_size_t size) _mutex.unlock(); } +exit_point: + if (read_failed) { + _mutex.unlock(); + } + return status; } @@ -306,8 +391,8 @@ int SPINANDBlockDevice::program(const void *buffer, bd_addr_t addr, bd_size_t si while (size > 0) { // Write on _page_size_bytes boundaries (Default 2048 bytes a page) - offset = addr % MBED_CONF_SPINAND_SPINAND_PAGE_SIZE; - chunk = (offset + size < MBED_CONF_SPINAND_SPINAND_PAGE_SIZE) ? size : (MBED_CONF_SPINAND_SPINAND_PAGE_SIZE - offset); + offset = addr % _page_size; + chunk = (offset + size < _page_size) ? size : (_page_size - offset); written_bytes = chunk; _mutex.lock(); @@ -320,12 +405,47 @@ int SPINANDBlockDevice::program(const void *buffer, bd_addr_t addr, bd_size_t si goto exit_point; } - result = _qspi_send_program_command(_program_instruction, buffer, addr, &written_bytes); - if ((result != QSPI_STATUS_OK) || (chunk != written_bytes)) { - tr_error("Write failed"); - program_failed = true; - status = SPINAND_BD_ERROR_DEVICE_ERROR; - goto exit_point; + if (_ecc_bits == 0) { + result = _qspi_send_program_command(_program_instruction, buffer, addr, &written_bytes); + if ((result != QSPI_STATUS_OK) || (chunk != written_bytes)) { + tr_error("Write failed"); + program_failed = true; + status = SPINAND_BD_ERROR_DEVICE_ERROR; + goto exit_point; + } + } else { + uint8_t *p = (uint8_t *)_page_buf; + uint8_t ecc_steps = _ecc_steps; + + if (size < _page_size) { + tr_error("Write failed"); + program_failed = true; + status = SPINAND_BD_ERROR_DEVICE_ERROR; + goto exit_point; + } + + // prepare data + memset(_page_buf, 0xff, _page_size + _oob_size); + memcpy(_page_buf + offset, (uint8_t *)buffer, written_bytes); + + // calculate the software ECC + for (uint8_t i = 0; ecc_steps; ecc_steps--, i += _ecc_bytes, p += _ecc_size) { + memset(_nbc.bch->input_data, 0x0, (1 << _nbc.bch->m) / 8); + memcpy(_nbc.bch->input_data + _ecc_bytes, p, _ecc_size); + _bch_calculate_ecc(_nbc.bch->input_data, _ecc_calc + i); + } + + // prepare ECC code + memcpy(_page_buf + _page_size + _ecc_layout_pos, _ecc_calc, _ecc_bytes * _ecc_steps); + + written_bytes = _page_size + _oob_size; + result = _qspi_send_program_command(_program_instruction, (void *)_page_buf, addr & SPINAND_PAGE_MASK, &written_bytes); + if ((result != QSPI_STATUS_OK)) { + tr_error("Write failed"); + program_failed = true; + status = SPINAND_BD_ERROR_DEVICE_ERROR; + goto exit_point; + } } buffer = static_cast(buffer) + chunk; @@ -356,7 +476,7 @@ int SPINANDBlockDevice::erase(bd_addr_t addr, bd_size_t size) tr_debug("Erase - addr: %llu, size: %llu", addr, size); - if ((addr + size) > MBED_CONF_SPINAND_SPINAND_FLASH_SIZE) { + if ((addr + size) > _flash_size) { tr_error("Erase exceeds flash device size"); return SPINAND_BD_ERROR_INVALID_ERASE_PARAMS; } @@ -385,8 +505,8 @@ int SPINANDBlockDevice::erase(bd_addr_t addr, bd_size_t size) } addr += SPINAND_BLOCK_OFFSET; - if (size > MBED_CONF_SPINAND_SPINAND_BLOCK_SIZE) { - size -= MBED_CONF_SPINAND_SPINAND_BLOCK_SIZE; + if (size > _block_size) { + size -= _block_size; } else { size = 0; } @@ -409,26 +529,55 @@ int SPINANDBlockDevice::erase(bd_addr_t addr, bd_size_t size) return status; } +bool SPINANDBlockDevice::is_bad_block(uint16_t blk_idx) +{ + mbed::bd_addr_t addr; + uint8_t mark[2]; + + addr = (blk_idx << _block_shift) + _page_size; + if (QSPI_STATUS_OK != _read_oob(mark, addr, sizeof(mark))) { + tr_error("Read Command failed"); + return 0; + } + return (mark[0] != 0xff || mark[1] != 0xff) ? 1 : 0; +} + +int SPINANDBlockDevice::mark_bad_block(uint16_t blk_idx) +{ + int status = SPINAND_BD_ERROR_OK; + mbed::bd_addr_t addr; + uint8_t mark[2]; + + mark[0] = 0x00; + mark[1] = 0x00; + addr = (blk_idx << _block_shift) + _page_size; + if (QSPI_STATUS_OK != _program_oob(mark, addr, sizeof(mark))) { + tr_error("Program Command failed"); + status = SPINAND_BD_ERROR_DEVICE_ERROR; + } + return status; +} + bd_size_t SPINANDBlockDevice::get_read_size() const { // Return minimum read size in bytes for the device - return MBED_CONF_SPINAND_SPINAND_MIN_READ_SIZE; + return _page_size; } bd_size_t SPINANDBlockDevice::get_program_size() const { // Return minimum program/write size in bytes for the device - return MBED_CONF_SPINAND_SPINAND_MIN_PROG_SIZE; + return _page_size; } bd_size_t SPINANDBlockDevice::get_erase_size() const { - return MBED_CONF_SPINAND_SPINAND_BLOCK_SIZE; + return _block_size; } bd_size_t SPINANDBlockDevice::get_erase_size(bd_addr_t addr) const { - return MBED_CONF_SPINAND_SPINAND_BLOCK_SIZE; + return _block_size; } const char *SPINANDBlockDevice::get_type() const @@ -438,7 +587,7 @@ const char *SPINANDBlockDevice::get_type() const bd_size_t SPINANDBlockDevice::size() const { - return MBED_CONF_SPINAND_SPINAND_FLASH_SIZE; + return _flash_size; } int SPINANDBlockDevice::get_erase_value() const @@ -508,6 +657,148 @@ int SPINANDBlockDevice::remove_csel_instance(PinName csel) return status; } +bool SPINANDBlockDevice::_read_otp_onfi() +{ + uint8_t secur_reg = 0, onfi_table[256]; + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_GET_FEATURE, FEATURES_ADDR_SECURE_OTP, + NULL, 0, (char *) &secur_reg, 1)) { + tr_error("Reading Register failed"); + } + + secur_reg |= SPINAND_SECURE_BIT_OTP_EN; + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_SET_FEATURE, FEATURES_ADDR_SECURE_OTP, + (char *) &secur_reg, 1, NULL, 0)) { + tr_error("Writing Security Register failed"); + return 0; + } + if (QSPI_STATUS_OK != _qspi_send_read_command(SPINAND_INST_READ_CACHE, onfi_table, 1 << _page_shift, sizeof(onfi_table))) { + tr_error("Writing Security Register failed"); + return 0; + } + if (onfi_table[0] == 'O' && onfi_table[1] == 'N' && onfi_table[2] == 'F' && onfi_table[3] == 'I') { + tr_info("ONFI table found\n"); + memcpy(_name, &onfi_table[32], sizeof(_name)); + _name[31] = 0; + _page_size = onfi_table[80] + (onfi_table[81] << 8) + (onfi_table[82] << 16); + _oob_size = onfi_table[84] + (onfi_table[85] << 8); + _page_num = onfi_table[92] + (onfi_table[93] << 8); + _block_num = onfi_table[96] + (onfi_table[97] << 8); + _block_size = _page_size * _page_num; + switch (_page_size) { + case 2048 : + _page_shift = 12; + break; + case 4096 : + _page_shift = 13; + break; + } + switch (_page_num) { + case 64 : + _block_shift = _page_shift + 6; + break; + case 128 : + _block_shift = _page_shift + 7; + break; + case 256 : + _block_shift = _page_shift + 8; + break; + } + _flash_size = _block_size * _block_num; + _ecc_bits = onfi_table[112]; + if (_ecc_bits > 0) { + _bch_init(_ecc_bits); + secur_reg &= ~SPINAND_SECURE_BIT_ECC_EN; + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_SET_FEATURE, FEATURES_ADDR_SECURE_OTP, + (char *) &secur_reg, 1, NULL, 0)) { + tr_error("Writing Register failed"); + } + } else { + secur_reg |= SPINAND_SECURE_BIT_ECC_EN; + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_SET_FEATURE, FEATURES_ADDR_SECURE_OTP, + (char *) &secur_reg, 1, NULL, 0)) { + tr_error("Writing Register failed"); + } + } + + if (onfi_table[168] & 0x02) { + _continuous_read = true; + + if (QSPI_STATUS_OK != _set_conti_read_enable()) { + tr_error("SPI NAND Set continuous read enable Failed"); + return 0; + } + } else { + _continuous_read = false; + } + } else { + tr_error("ONFI table not found"); + return 0; + } + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_GET_FEATURE, FEATURES_ADDR_SECURE_OTP, + NULL, 0, (char *) &secur_reg, 1)) { + tr_error("Reading Register failed"); + } + + secur_reg &= ~SPINAND_SECURE_BIT_OTP_EN; + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_SET_FEATURE, FEATURES_ADDR_SECURE_OTP, + (char *) &secur_reg, 1, NULL, 0)) { + tr_error("Writing Register failed"); + } + return 1; +} + +int SPINANDBlockDevice::_read_oob(void *buffer, bd_addr_t addr, bd_size_t size) +{ + int status = SPINAND_BD_ERROR_OK; + + _mutex.lock(); + + if (QSPI_STATUS_OK != _qspi_send_read_command(_read_instruction, buffer, addr, size)) { + tr_error("Read Command failed"); + status = SPINAND_BD_ERROR_DEVICE_ERROR; + } + + _mutex.unlock(); + + return status; +} + +int SPINANDBlockDevice::_program_oob(const void *buffer, bd_addr_t addr, bd_size_t size) +{ + qspi_status_t result = QSPI_STATUS_OK; + bool program_failed = false; + int status = SPINAND_BD_ERROR_OK; + + _mutex.lock(); + + //Send WREN + if (_set_write_enable() != 0) { + tr_error("Write Enable failed"); + status = SPINAND_BD_ERROR_WREN_FAILED; + goto exit_point; + } + + result = _qspi_send_program_command(_program_instruction, buffer, addr, &size); + if (result != QSPI_STATUS_OK) { + tr_error("Write failed"); + status = SPINAND_BD_ERROR_DEVICE_ERROR; + } + + _mutex.unlock(); + +exit_point: + if (program_failed) { + _mutex.unlock(); + } + + return status; +} + int SPINANDBlockDevice::_set_quad_enable() { uint8_t secur_reg = 0; @@ -606,6 +897,79 @@ int SPINANDBlockDevice::_set_write_enable() return status; } +int SPINANDBlockDevice::_set_conti_read_enable() +{ + uint8_t secur_reg = 0; + + if (false == _is_mem_ready()) { + tr_error("Device not ready, set quad enable failed"); + return -1; + } + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_GET_FEATURE, FEATURES_ADDR_SECURE_OTP, + NULL, 0, (char *) &secur_reg, 1)) { + tr_error("Reading Security Register failed"); + } + + secur_reg |= SPINAND_SECURE_BIT_CONT; + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_SET_FEATURE, FEATURES_ADDR_SECURE_OTP, + (char *) &secur_reg, 1, NULL, 0)) { + tr_error("Writing Security Register failed"); + } + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_GET_FEATURE, FEATURES_ADDR_SECURE_OTP, + NULL, 0, (char *) &secur_reg, 1)) { + tr_error("Reading Security Register failed"); + } + if (false == _is_mem_ready()) { + tr_error("Device not ready, set quad enable failed"); + return -1; + } + + return 0; +} + +int SPINANDBlockDevice::_set_conti_read_disable() +{ + uint8_t secur_reg = 0; + + if (false == _is_mem_ready()) { + tr_error("Device not ready, set quad enable failed"); + return -1; + } + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_GET_FEATURE, FEATURES_ADDR_SECURE_OTP, + NULL, 0, (char *) &secur_reg, 1)) { + tr_error("Reading Security Register failed"); + } + + secur_reg &= ~SPINAND_SECURE_BIT_CONT; + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_SET_FEATURE, FEATURES_ADDR_SECURE_OTP, + (char *) &secur_reg, 1, NULL, 0)) { + tr_error("Writing Security Register failed"); + } + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_GET_FEATURE, FEATURES_ADDR_SECURE_OTP, + NULL, 0, (char *) &secur_reg, 1)) { + tr_error("Reading Security Register failed"); + } + if (false == _is_mem_ready()) { + tr_error("Device not ready, set quad enable failed"); + return -1; + } + + return 0; +} + +int SPINANDBlockDevice::_conti_read_exit() +{ + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_RESET, QSPI_NO_ADDRESS_COMMAND, NULL, 0, NULL, 0)) { + tr_error("Sending WREN command FAILED"); + } + + return 0; +} + bool SPINANDBlockDevice::_is_mem_ready() { // Check Status Register Busy Bit to Verify the Device isn't Busy @@ -651,20 +1015,20 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_read_command(qspi_inst_t read_inst, data_width = QSPI_CFG_BUS_SINGLE; } else if (read_inst == SPINAND_INST_READ_CACHE2) { data_width = QSPI_CFG_BUS_DUAL; - } else if (read_inst == SPINAND_INST_READ_CACHE4) { - data_width = QSPI_CFG_BUS_QUAD; + } else { + data_width = QSPI_CFG_BUS_QUAD; //read_inst == SPINAND_INST_READ_CACHE4 } // Send read command to device driver // Read commands use the best bus mode supported by the part - qspi_status_t status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_COLUMN_ADDR_SIZE, // Alt width should be the same as address width + qspi_status_t status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_ROW_ADDR_SIZE, // Alt width should be the same as address width _address_width, _alt_size, _data_width, 0); if (QSPI_STATUS_OK != status) { tr_error("_qspi_configure_format failed"); return status; } - if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_PAGE_READ, addr >> 12, NULL, 0, NULL, 0)) { + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_PAGE_READ, addr >> _page_shift, NULL, 0, NULL, 0)) { tr_error("Read page from array failed"); } @@ -680,7 +1044,7 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_read_command(qspi_inst_t read_inst, return QSPI_STATUS_ERROR; } - status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_ROW_ADDR_SIZE, _address_width, // Alt width should be the same as address width + status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_COLUMN_ADDR_SIZE, _address_width, // Alt width should be the same as address width _alt_size, data_width, _dummy_cycles); if (QSPI_STATUS_OK != status) { tr_error("_qspi_configure_format failed"); @@ -706,6 +1070,78 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_read_command(qspi_inst_t read_inst, return QSPI_STATUS_OK; } +qspi_status_t SPINANDBlockDevice::_qspi_send_continuous_read_command(qspi_inst_t read_inst, void *buffer, + bd_addr_t addr, bd_size_t size) +{ + tr_debug("Inst: 0x%xh, addr: %llu, size: %llu", read_inst, addr, size); + + size_t buf_len = size; + + qspi_bus_width_t data_width; + if (read_inst == SPINAND_INST_READ_CACHE) { + data_width = QSPI_CFG_BUS_SINGLE; + } else if (read_inst == SPINAND_INST_READ_CACHE2) { + data_width = QSPI_CFG_BUS_DUAL; + } else { + data_width = QSPI_CFG_BUS_QUAD; //read_inst == SPINAND_INST_READ_CACHE4 + } + + // Send read command to device driver + // Read commands use the best bus mode supported by the part + qspi_status_t status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_ROW_ADDR_SIZE, // Alt width should be the same as address width + _address_width, _alt_size, _data_width, 0); + if (QSPI_STATUS_OK != status) { + tr_error("_qspi_configure_format failed"); + return status; + } + + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_PAGE_READ, addr >> _page_shift, NULL, 0, NULL, 0)) { + tr_error("Read page from array failed"); + } + + status = _qspi.configure_format(_inst_width, _address_width, _address_size, _address_width, // Alt width should be the same as address width + _alt_size, _data_width, 0); + if (QSPI_STATUS_OK != status) { + tr_error("_qspi_configure_format failed"); + return status; + } + + if (false == _is_mem_ready()) { + tr_error("Device not ready, clearing block protection failed"); + return QSPI_STATUS_ERROR; + } + + status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_ROW_ADDR_SIZE, _address_width, // Alt width should be the same as address width + _alt_size, data_width, 0); + if (QSPI_STATUS_OK != status) { + tr_error("_qspi_configure_format failed"); + return status; + } + + // Don't check the read status until after we've configured the format back to 1-1-1, to avoid leaving the interface in an + // incorrect state if the read fails. + status = _qspi.read(read_inst, (_alt_size == 0) ? -1 : QSPI_ALT_DEFAULT_VALUE, (unsigned int)(addr >> _page_shift), (char *)buffer, &buf_len); + + // All commands other than Read use default 1-1-1 bus mode (Program/Erase are constrained by flash memory performance more than bus performance) + qspi_status_t format_status = _qspi.configure_format(QSPI_CFG_BUS_SINGLE, QSPI_CFG_BUS_SINGLE, _address_size, QSPI_CFG_BUS_SINGLE, 0, QSPI_CFG_BUS_SINGLE, 0); + if (QSPI_STATUS_OK != format_status) { + tr_error("_qspi_configure_format failed"); + return format_status; + } + + if (QSPI_STATUS_OK != status) { + tr_error("QSPI Read failed"); + return status; + } + + if (QSPI_STATUS_OK != _conti_read_exit()) { + tr_error("SPI NAND exit continuous read Failed"); + return QSPI_STATUS_ERROR; + } + + return QSPI_STATUS_OK; +} + qspi_status_t SPINANDBlockDevice::_qspi_send_program_command(qspi_inst_t prog_inst, const void *buffer, bd_addr_t addr, bd_size_t *size) { @@ -715,12 +1151,12 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_program_command(qspi_inst_t prog_in if (prog_inst == SPINAND_INST_PP_LOAD) { data_width = QSPI_CFG_BUS_SINGLE; - } else if (prog_inst == SPINAND_INST_4PP_LOAD) { - data_width = QSPI_CFG_BUS_QUAD; + } else { + data_width = QSPI_CFG_BUS_QUAD; //prog_inst == SPINAND_INST_4PP_LOAD } - // Program load commands need 16 bit row address - qspi_status_t status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_ROW_ADDR_SIZE, // Alt width should be the same as address width + // Program load commands need 16 bit column address + qspi_status_t status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_COLUMN_ADDR_SIZE, // Alt width should be the same as address width _address_width, _alt_size, data_width, 0); if (QSPI_STATUS_OK != status) { tr_error("_qspi_configure_format failed"); @@ -734,8 +1170,8 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_program_command(qspi_inst_t prog_in return status; } - // Program execute command need 24 bit column address - qspi_status_t format_status = _qspi.configure_format(QSPI_CFG_BUS_SINGLE, QSPI_CFG_BUS_SINGLE, SPI_NAND_COLUMN_ADDR_SIZE, QSPI_CFG_BUS_SINGLE, + // Program execute command need 24 bit row address + qspi_status_t format_status = _qspi.configure_format(QSPI_CFG_BUS_SINGLE, QSPI_CFG_BUS_SINGLE, SPI_NAND_ROW_ADDR_SIZE, QSPI_CFG_BUS_SINGLE, 0, QSPI_CFG_BUS_SINGLE, 0); if (QSPI_STATUS_OK != format_status) { tr_error("_qspi_configure_format failed"); @@ -743,7 +1179,7 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_program_command(qspi_inst_t prog_in } // Program execute command - if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_PROGRAM_EXEC, addr >> 12, NULL, 0, NULL, 0)) { + if (QSPI_STATUS_OK != _qspi_send_general_command(SPINAND_INST_PROGRAM_EXEC, addr >> _page_shift, NULL, 0, NULL, 0)) { tr_error("Read page from array failed"); } @@ -766,7 +1202,7 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_erase_command(qspi_inst_t erase_ins { tr_debug("Inst: 0x%xh, addr: %llu, size: %llu", erase_inst, addr, size); - qspi_status_t status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_COLUMN_ADDR_SIZE,// Alt width should be the same as address width + qspi_status_t status = _qspi.configure_format(_inst_width, _address_width, SPI_NAND_ROW_ADDR_SIZE,// Alt width should be the same as address width _address_width, _alt_size, _data_width, 0); if (QSPI_STATUS_OK != status) { tr_error("_qspi_configure_format failed"); @@ -774,7 +1210,7 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_erase_command(qspi_inst_t erase_ins } // Send erase command to driver - status = _qspi.command_transfer(erase_inst, (int)(addr >> 12), NULL, 0, NULL, 0); + status = _qspi.command_transfer(erase_inst, (int)(addr >> _page_shift), NULL, 0, NULL, 0); if (QSPI_STATUS_OK != status) { tr_error("QSPI Erase failed"); @@ -812,4 +1248,76 @@ qspi_status_t SPINANDBlockDevice::_qspi_send_general_command(qspi_inst_t instruc return QSPI_STATUS_OK; } +void SPINANDBlockDevice::_bch_init(uint8_t ecc_bits) +{ + unsigned int m, t, i; + unsigned char *erased_page; + unsigned int eccsize = 410; + unsigned int eccbytes = 0; + + m = fls(1 + 8 * eccsize); + t = ecc_bits; + + _ecc_bytes = eccbytes = ((m * t + 31) / 32) * 4; + _ecc_size = eccsize; + _ecc_steps = _page_size / eccsize; + _ecc_layout_pos = 2; // skip the bad block mark for Macronix spi nand + + _nbc.bch = bch_init(m, t); + if (!_nbc.bch) { + return; + } + + /* verify that eccbytes has the expected value */ + if (_nbc.bch->ecc_words * 4 != eccbytes) { + tr_error("invalid eccbytes %u, should be %u\n", + eccbytes, _nbc.bch->ecc_words); + return; + } + + _page_buf = (uint8_t *)malloc(_page_size + _oob_size); + _ecc_calc = (uint8_t *)malloc(_ecc_steps * _ecc_bytes); + _ecc_code = (uint8_t *)malloc(_ecc_steps * _ecc_bytes); + _nbc.eccmask = (unsigned char *)malloc(eccbytes); + _nbc.errloc = (unsigned int *)malloc(t * sizeof(*_nbc.errloc)); + if (!_nbc.eccmask || !_nbc.errloc) { + return; + } + /* + * compute and store the inverted ecc of an erased ecc block + */ + erased_page = (unsigned char *)malloc(eccsize); + if (!erased_page) { + return; + } + memset(_page_buf, 0xff, _page_size + _oob_size); + memset(erased_page, 0xff, eccsize); + memset(_nbc.eccmask, 0, eccbytes); + bch_encode(_nbc.bch, erased_page, (unsigned int *)_nbc.eccmask); + free(erased_page); + + for (i = 0; i < eccbytes; i++) { + _nbc.eccmask[i] ^= 0xff; + } +} + +void SPINANDBlockDevice::_bch_free() +{ + bch_free(_nbc.bch); + free(_nbc.errloc); + free(_nbc.eccmask); + free(_page_buf); + free(_ecc_calc); + free(_ecc_code); +} + +int SPINANDBlockDevice::_bch_calculate_ecc(unsigned char *buf, unsigned char *code) +{ + + memset(code, 0, _ecc_bytes); + + bch_encode(_nbc.bch, buf, (unsigned int *)code); + + return 0; +} diff --git a/storage/blockdevice/COMPONENT_SPINAND/source/bch.c b/storage/blockdevice/COMPONENT_SPINAND/source/bch.c new file mode 100644 index 00000000000..850020d9c22 --- /dev/null +++ b/storage/blockdevice/COMPONENT_SPINAND/source/bch.c @@ -0,0 +1,452 @@ +/* + * Copyright (c) 2022 Macronix International Co., Ltd. + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include "bch.h" + +int fls(int x) +{ + int r = 32; + + if (!x) { + return 0; + } + if (!(x & 0xffff0000u)) { + x <<= 16; + r -= 16; + } + if (!(x & 0xff000000u)) { + x <<= 8; + r -= 8; + } + if (!(x & 0xf0000000u)) { + x <<= 4; + r -= 4; + } + if (!(x & 0xc0000000u)) { + x <<= 2; + r -= 2; + } + if (!(x & 0x80000000u)) { + x <<= 1; + r -= 1; + } + return r; +} + +void bch_encode(struct bch_code *bch, unsigned char *data, unsigned int *ecc) +{ + unsigned int i, j, k, mlen; + unsigned int w; + unsigned int *p; + unsigned int *c[16]; + unsigned int *t[16]; + + t[0] = bch->mod_tab; + for (i = 1; i < 16; i++) { + t[i] = t[i - 1] + 4 * (bch->ecc_words); + } + + memset(bch->ecc, 0, bch->ecc_words * sizeof(*bch->ecc)); + + p = (unsigned int *)data; + mlen = bch->len / 4; + + while (mlen--) { + if (!bch->endian) { + w = ((unsigned int)(*p) & 0xff000000) >> 24 | + ((unsigned int)(*p) & 0x00ff0000) >> 8 | + ((unsigned int)(*p) & 0x0000ff00) << 8 | + ((unsigned int)(*p) & 0x000000ff) << 24; + } else { + w = *p; + } + p++; + w ^= bch->ecc[0]; + k = 0; + for (i = 0; i < 16; i++) { + c[i] = t[i] + (bch->ecc_words) * ((w >> k) & 0x03); + k = k + 2; + } + + for (i = 0; i < bch->ecc_words - 1; i++) { + bch->ecc[i] = bch->ecc[i + 1]; + for (j = 0; j < 16; j++) { + bch->ecc[i] ^= c[j][i]; + } + } + bch->ecc[i] = c[0][i]; + for (j = 1; j < 16; j++) { + bch->ecc[i] ^= c[j][i]; + } + } + + if (ecc != NULL) { + for (i = 0; i < bch->ecc_words; i++) { + ecc[i] = bch->ecc[i]; + } + } +} + +static inline int mod(struct bch_code *bch, unsigned int v) +{ + while (v >= bch->n) { + v -= bch->n; + v = (v & bch->n) + (v >> bch->m); + } + return v; +} + +static void build_syndrome(struct bch_code *bch) +{ + unsigned int i, j; + unsigned int ecc_bits; + unsigned int *ecc; + + memset(bch->syn, 0, 2 * bch->t * sizeof(*bch->syn)); + + ecc_bits = bch->ecc_bits; + ecc = bch->ecc; + while (ecc_bits > 0) { + i = ecc_bits - 32; + ecc_bits = i; + while (*ecc) { + if (*ecc & (unsigned int)1) { + for (j = 0; j < 2 * bch->t; j++) { + bch->syn[j] ^= bch->a_pow[mod(bch, (j + 1) * i)]; + } + } + *ecc >>= 1; + i++; + } + ecc++; + } +} + +static int build_error_location_poly(struct bch_code *bch) +{ + unsigned int i, j, k; + unsigned int tmp, dp = 1, d = bch->syn[0]; + unsigned int deg, buf_deg, tmp_deg = 0; + int pp = -1; + + memset(bch->elp, 0, (bch->t + 1) * sizeof(*bch->elp)); + + buf_deg = 0; + bch->buf[0] = 1; + deg = 0; + bch->elp[0] = 1; + + for (i = 0; (i < bch->t) && (deg <= bch->t); i++) { + if (d) { + k = 2 * i - pp; + if (buf_deg + k > deg) { + tmp_deg = deg; + for (j = 0; j <= deg; j++) { + bch->buf2[j] = bch->elp[j]; + } + } + tmp = bch->n + bch->a_log[d] - bch->a_log[dp]; + + for (j = 0; j <= buf_deg; j++) { + if (bch->buf[j]) { + bch->elp[j + k] ^= bch->a_pow[mod(bch, tmp + bch->a_log[bch->buf[j]])]; + } + } + if (buf_deg + k > deg) { + deg = buf_deg + k; + buf_deg = tmp_deg; + for (j = 0; j <= tmp_deg; j++) { + bch->buf[j] = bch->buf2[j]; + } + dp = d; + pp = 2 * i; + } + } + if (i < bch->t - 1) { + k = 2 * i + 1; + d = bch->syn[k + 1]; + for (j = 1; j <= deg; j++) { + if (bch->elp[j] && bch->syn[k]) { + d ^= bch->a_pow[mod(bch, bch->a_log[bch->elp[j]] + bch->a_log[bch->syn[k]])]; + } + k--; + } + } + } + return (deg > bch->t) ? -1 : (int)deg; +} + +static int chien_search(struct bch_code *bch, unsigned int deg) +{ + unsigned int i, j, k, nroot = 0; + unsigned int syn, syn0; + int *rep = (int *)bch->buf; + int *root = (int *)bch->buf2; + + k = bch->n - bch->a_log[bch->elp[deg]]; + for (i = 0; i < deg; i++) { + rep[i] = bch->elp[i] ? mod(bch, bch->a_log[ bch->elp[i] ] + k) : -1; + } + rep[i] = 0; + + syn0 = bch->elp[0] ? bch->a_pow[rep[0]] : 0; + for (i = 0; i <= bch->n; i++) { + for (j = 1, syn = syn0; j <= deg; j++) { + if (rep[j] >= 0) { + syn ^= bch->a_pow[mod(bch, rep[j] + j * i)]; + } + } + if (syn == 0) { + root[nroot++] = bch->n - i; + if (nroot == deg) { + return nroot; + } + } + } + return 0; +} + +int bch_decode(struct bch_code *bch, unsigned char *data, unsigned int *ecc) +{ + unsigned int nbits; + unsigned int i, err, nroot; + int *root = (int *)bch->buf2; + + bch_encode(bch, data, NULL); + + for (i = 0, err = 0; i < bch->ecc_words; i++) { + bch->ecc[i] ^= ecc[i]; + err |= bch->ecc[i]; + } + if (!err) { + return 0; + } + + build_syndrome(bch); + err = build_error_location_poly(bch); + if (err <= 0) { + return -1; + } + + nroot = chien_search(bch, err); + if (err != nroot) { + return -1; + } + nbits = (bch->len * 8) + bch->ecc_bits; + for (i = 0; i < err; i++) { + root[i] = nbits - 1 - root[i]; + root[i] = (root[i] & ~7) | (7 - (root[i] & 7)); + data[root[i] / 8] ^= 1 << root[i] % 8; + } + + return err; +} + +static void build_gf_table(struct bch_code *bch) +{ + unsigned int i, x; + unsigned int msb, poly; + unsigned int prim_poly[5] = {0x11d, 0x211, 0x409, 0x805, 0x1053}; + + poly = prim_poly[bch->m - 8]; + msb = 1 << bch->m; + bch->a_pow[0] = 1; + bch->a_log[1] = 0; + x = 2; + for (i = 1; i < bch->n; i++) { + bch->a_pow[i] = x; + bch->a_log[x] = i; + x <<= 1; + if (x & msb) { + x ^= poly; + } + } + bch->a_pow[bch->n] = 1; + bch->a_log[0] = 0; +} + +static void build_mod_tables(struct bch_code *bch, const unsigned int *g) +{ + unsigned int i, j, b, d; + unsigned int data, hi, lo, *tab, poly; + unsigned int plen = (bch->ecc_bits + 32) / 32; + unsigned int ecclen = (bch->ecc_bits + 31) / 32; + + memset(bch->mod_tab, 0, 16 * 4 * bch->ecc_words * sizeof(*bch->mod_tab)); + + for (i = 0; i < 4; i++) { + for (b = 0; b < 16; b++) { + tab = bch->mod_tab + (b * 4 + i) * bch->ecc_words; + data = i << (2 * b); + while (data) { + d = 0; + poly = (data >> 1); + while (poly) { + poly >>= 1; + d++; + } + data ^= g[0] >> (31 - d); + for (j = 0; j < ecclen; j++) { + hi = (d < 31) ? g[j] << (d + 1) : 0; + lo = (j + 1 < plen) ? g[j + 1] >> (31 - d) : 0; + tab[j] ^= hi | lo; + } + } + } + } +} + +static void *bch_alloc(size_t size, int *err) +{ + void *ptr = NULL; + if (*err == 0) { + ptr = malloc(size); + } + if (ptr == NULL) { + *err = 1; + } + return ptr; +} + +static unsigned int *build_generator_poly(struct bch_code *bch) +{ + unsigned int i, j, k; + unsigned int m, t; + int err = 0; + unsigned int n; + unsigned int *x; + unsigned int *g; + + x = bch_alloc((bch->m * bch->t + 1) * sizeof(*x), &err); + g = bch_alloc((bch->ecc_words + 1) * sizeof(*g), &err); + + if (err) { + free(g); + free(x); + bch_free(bch); + return NULL; + } + + bch->ecc_bits = 0; + x[0] = 1; + for (t = 0; t < bch->t; t++) { + for (m = 0, i = 2 * t + 1; m < bch->m; m++) { + x[bch->ecc_bits + 1] = 1; + for (j = bch->ecc_bits; j > 0; j--) { + if (x[j]) { + x[j] = bch->a_pow[mod(bch, bch->a_log[x[j]] + i)] ^ x[j - 1]; + } else { + x[j] = x[j - 1]; + } + } + if (x[j]) { + x[j] = bch->a_pow[mod(bch, bch->a_log[x[j]] + i)]; + } + bch->ecc_bits++; + i = mod(bch, 2 * i); + } + } + + i = 0; + memset(g, 0, (bch->ecc_words + 1) * sizeof(*g)); + + for (k = bch->ecc_bits + 1; k > 0; k = k - n) { + n = (k > 32) ? 32 : k; + for (j = 0; j < n; j++) { + if (x[k - 1 - j]) { + g[i] |= (unsigned int)1 << (31 - j); + } + } + i++; + } + + free(x); + return g; +} + +struct bch_code *bch_init(unsigned int m, unsigned int t) +{ + int err = 0; + unsigned int *genpoly; + struct bch_code *bch = NULL; + + short int a = 0x1234; + char *p = (char *)&a; + + if ((m < 8) || (m > 12)) { + return NULL; + } + if ((t < 1) || (t > 12)) { + return NULL; + } + + bch = (struct bch_code *)malloc(sizeof(struct bch_code)); + + if (bch == NULL) { + return NULL; + } + + bch->m = m; + bch->t = t; + bch->n = (1 << m) - 1; + bch->ecc_words = (m * t + 31) / 32; + bch->len = (bch->n + 1) / 8; + bch->a_pow = bch_alloc((1 + bch->n) * sizeof(*bch->a_pow), &err); + bch->a_log = bch_alloc((1 + bch->n) * sizeof(*bch->a_log), &err); + bch->mod_tab = bch_alloc(bch->ecc_words * 16 * 4 * sizeof(*bch->mod_tab), &err); + bch->ecc = bch_alloc(bch->ecc_words * sizeof(*bch->ecc), &err); + bch->syn = bch_alloc(2 * t * sizeof(*bch->syn), &err); + bch->elp = bch_alloc((t + 1) * sizeof(*bch->elp), &err); + bch->buf = bch_alloc((t + 1) * sizeof(*bch->buf), &err); + bch->buf2 = bch_alloc((t + 1) * sizeof(*bch->buf2), &err); + bch->input_data = bch_alloc((1 << m) / 8, &err); + + if (*p == 0x34) { + bch->endian = 0; + } else if (*p == 0x12) { + bch->endian = 1; + } else { + err = 1; + } + + if (err) { + bch_free(bch); + return NULL; + } + + build_gf_table(bch); + genpoly = build_generator_poly(bch); + if (genpoly == NULL) { + return NULL; + } + + build_mod_tables(bch, genpoly); + free(genpoly); + + if (err) { + bch_free(bch); + return NULL; + } + + return bch; +} + +void bch_free(struct bch_code *bch) +{ + if (bch) { + free(bch->a_pow); + free(bch->a_log); + free(bch->mod_tab); + free(bch->ecc); + free(bch->syn); + free(bch->elp); + free(bch->buf); + free(bch->buf2); + free(bch); + } +}