diff --git a/drivers/rkflash/rkflash_api.c b/drivers/rkflash/rkflash_api.c index 797b676fd4..88e1f1e8ce 100644 --- a/drivers/rkflash/rkflash_api.c +++ b/drivers/rkflash/rkflash_api.c @@ -9,14 +9,79 @@ #include "rkflash_api.h" #include "rkflash_blk.h" +#include "rkflash_debug.h" #ifdef CONFIG_RKSFC_NOR + +#define IDB_ALIGN_64 128 /* 64 KB */ +#define IDB_ALIGN_32 64 /* 32 KB */ + +static void P_RC4(u8 *buf, u16 len) +{ + u8 S[256], K[256], temp; + u16 i, j, t, x; + u8 key[16] = {124, 78, 3, 4, 85, 5, 9, 7, + 45, 44, 123, 56, 23, 13, 23, 17}; + + j = 0; + for (i = 0; i < 256; i++) { + S[i] = (u8)i; + j &= 0x0f; + K[i] = key[j]; + j++; + } + + j = 0; + for (i = 0; i < 256; i++) { + j = (j + S[i] + K[i]) % 256; + temp = S[i]; + S[i] = S[j]; + S[j] = temp; + } + + i = 0; + j = 0; + for (x = 0; x < len; x++) { + i = (i + 1) % 256; + j = (j + S[i]) % 256; + temp = S[i]; + S[i] = S[j]; + S[j] = temp; + t = (S[i] + (S[j] % 256)) % 256; + buf[x] = buf[x] ^ S[t]; + } +} + int rksfc_nor_init(struct udevice *udev) { struct rkflash_info *priv = dev_get_priv(udev); struct SFNOR_DEV *p_dev = (struct SFNOR_DEV *)&priv->flash_dev_info; + struct snor_info_packet *packet; + struct id_block_tag *idb_tag; + int ret; - return snor_init(p_dev); + ret = snor_init(p_dev); + if (ret == SFC_OK && p_dev->read_lines == DATA_LINES_X1) { + idb_tag = kzalloc(NOR_SECS_PAGE * 512, GFP_KERNEL); + if (!idb_tag) + return SFC_OK; + + if (sfc_get_version() >= SFC_VER_4) + snor_read(p_dev, IDB_ALIGN_32, NOR_SECS_PAGE, + idb_tag); + else + snor_read(p_dev, IDB_ALIGN_64, NOR_SECS_PAGE, + idb_tag); + packet = (struct snor_info_packet *)&idb_tag->dev_param[0]; + if (idb_tag->id == IDB_BLOCK_TAG_ID) { + P_RC4((u8 *)idb_tag, sizeof(struct id_block_tag)); + snor_reinit_from_table_packet(p_dev, packet); + rkflash_print_error("snor reinit, ret= %d\n", ret); + } + kfree(idb_tag); + } + + return ret; } u32 rksfc_nor_get_capacity(struct udevice *udev)