// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
|
|
/* Copyright (c) 2018 Fuzhou Rockchip Electronics Co., Ltd */
|
|
#include <linux/fs.h>
|
#include <linux/kthread.h>
|
#include <linux/miscdevice.h>
|
#include <linux/module.h>
|
#include <linux/platform_device.h>
|
#include <linux/slab.h>
|
#include <linux/soc/rockchip/rk_vendor_storage.h>
|
#include <linux/uaccess.h>
|
#include <misc/rkflash_vendor_storage.h>
|
|
#include "flash_vendor_storage.h"
|
|
#define FLASH_VENDOR_TEST 0
|
#define DRM_DEBUG 1
|
|
#if DRM_DEBUG
|
#define DLOG(fmt, args...) pr_info(fmt, ##args)
|
#else
|
#define DLOG(x...)
|
#endif
|
|
#define FLASH_VENDOR_PART_START 8
|
#define FLASH_VENDOR_PART_NUM 4
|
#define FLASH_VENDOR_TAG VENDOR_HEAD_TAG
|
|
static int (*_flash_read)(u32 sec, u32 n_sec, void *p_data);
|
static int (*_flash_write)(u32 sec, u32 n_sec, void *p_data);
|
static struct flash_vendor_info *g_vendor;
|
|
int flash_vendor_dev_ops_register(int (*read)(u32 sec,
|
u32 n_sec,
|
void *p_data),
|
int (*write)(u32 sec,
|
u32 n_sec,
|
void *p_data))
|
{
|
if (!_flash_read) {
|
_flash_read = read;
|
_flash_write = write;
|
return 0;
|
}
|
return -1;
|
}
|
|
static u32 flash_vendor_init(void)
|
{
|
u32 i, max_ver, max_index;
|
|
if (!_flash_read)
|
return -EPERM;
|
|
g_vendor = kmalloc(sizeof(*g_vendor), GFP_KERNEL | GFP_DMA);
|
if (!g_vendor)
|
return 0;
|
|
max_ver = 0;
|
max_index = 0;
|
for (i = 0; i < FLASH_VENDOR_PART_NUM; i++) {
|
_flash_read(FLASH_VENDOR_PART_START +
|
FLASH_VENDOR_PART_SIZE * i,
|
FLASH_VENDOR_PART_SIZE,
|
g_vendor);
|
if (g_vendor->tag == FLASH_VENDOR_TAG &&
|
g_vendor->version == g_vendor->version2) {
|
if (max_ver < g_vendor->version) {
|
max_index = i;
|
max_ver = g_vendor->version;
|
}
|
}
|
}
|
/* DLOG("max_ver = %d\n",max_ver); */
|
if (max_ver) {
|
_flash_read(FLASH_VENDOR_PART_START +
|
FLASH_VENDOR_PART_SIZE * max_index,
|
FLASH_VENDOR_PART_SIZE,
|
g_vendor);
|
} else {
|
memset(g_vendor, 0, sizeof(*g_vendor));
|
g_vendor->version = 1;
|
g_vendor->tag = FLASH_VENDOR_TAG;
|
g_vendor->version2 = g_vendor->version;
|
g_vendor->free_offset = 0;
|
g_vendor->free_size = sizeof(g_vendor->data);
|
}
|
/* rknand_print_hex("vendor:", g_vendor, 4, 1024); */
|
|
return 0;
|
}
|
|
static int flash_vendor_read(u32 id, void *pbuf, u32 size)
|
{
|
u32 i;
|
|
if (!g_vendor)
|
return -1;
|
|
for (i = 0; i < g_vendor->item_num; i++) {
|
if (g_vendor->item[i].id == id) {
|
if (size > g_vendor->item[i].size)
|
size = g_vendor->item[i].size;
|
memcpy(pbuf,
|
&g_vendor->data[g_vendor->item[i].offset],
|
size);
|
return size;
|
}
|
}
|
return (-1);
|
}
|
|
static int flash_vendor_write(u32 id, void *pbuf, u32 size)
|
{
|
u32 i, j, next_index, align_size, alloc_size, item_num;
|
u32 offset, next_size;
|
u8 *p_data;
|
struct vendor_item *item;
|
struct vendor_item *next_item;
|
|
if (!g_vendor)
|
return -1;
|
|
p_data = g_vendor->data;
|
item_num = g_vendor->item_num;
|
align_size = ALIGN(size, 0x40); /* align to 64 bytes*/
|
next_index = g_vendor->next_index;
|
for (i = 0; i < item_num; i++) {
|
item = &g_vendor->item[i];
|
if (item->id == id) {
|
alloc_size = ALIGN(item->size, 0x40);
|
if (size > alloc_size) {
|
if (g_vendor->free_size < align_size)
|
return -1;
|
offset = item->offset;
|
for (j = i; j < item_num - 1; j++) {
|
item = &g_vendor->item[j];
|
next_item = &g_vendor->item[j + 1];
|
item->id = next_item->id;
|
item->size = next_item->size;
|
item->offset = offset;
|
next_size = ALIGN(next_item->size,
|
0x40);
|
memcpy(&p_data[offset],
|
&p_data[next_item->offset],
|
next_size);
|
offset += next_size;
|
}
|
item = &g_vendor->item[j];
|
item->id = id;
|
item->offset = offset;
|
item->size = size;
|
memcpy(&p_data[item->offset], pbuf, size);
|
g_vendor->free_offset = offset + align_size;
|
g_vendor->free_size -= (align_size -
|
alloc_size);
|
} else {
|
memcpy(&p_data[item->offset],
|
pbuf,
|
size);
|
g_vendor->item[i].size = size;
|
}
|
g_vendor->version++;
|
g_vendor->version2 = g_vendor->version;
|
g_vendor->next_index++;
|
if (g_vendor->next_index >= FLASH_VENDOR_PART_NUM)
|
g_vendor->next_index = 0;
|
_flash_write(FLASH_VENDOR_PART_START +
|
FLASH_VENDOR_PART_SIZE * next_index,
|
FLASH_VENDOR_PART_SIZE,
|
g_vendor);
|
return 0;
|
}
|
}
|
|
if (g_vendor->free_size >= align_size) {
|
item = &g_vendor->item[g_vendor->item_num];
|
item->id = id;
|
item->offset = g_vendor->free_offset;
|
item->size = align_size;
|
item->size = size;
|
g_vendor->free_offset += align_size;
|
g_vendor->free_size -= align_size;
|
memcpy(&g_vendor->data[item->offset], pbuf, size);
|
g_vendor->item_num++;
|
g_vendor->version++;
|
g_vendor->next_index++;
|
g_vendor->version2 = g_vendor->version;
|
if (g_vendor->next_index >= FLASH_VENDOR_PART_NUM)
|
g_vendor->next_index = 0;
|
_flash_write(FLASH_VENDOR_PART_START +
|
FLASH_VENDOR_PART_SIZE * next_index,
|
FLASH_VENDOR_PART_SIZE,
|
g_vendor);
|
return 0;
|
}
|
|
return(-1);
|
}
|
|
#if (FLASH_VENDOR_TEST)
|
static void print_hex(char *s, void *buf, int width, int len)
|
{
|
print_hex_dump(KERN_WARNING, s, DUMP_PREFIX_OFFSET,
|
16, width, buf, len * width, 0);
|
}
|
|
static void flash_vendor_test(void)
|
{
|
u32 i;
|
u8 test_buf[512];
|
|
memset(test_buf, 0, 512);
|
for (i = 0; i < 62; i++) {
|
memset(test_buf, i, i + 1);
|
flash_vendor_write(i, test_buf, i + 1);
|
}
|
memset(test_buf, 0, 512);
|
for (i = 0; i < 62; i++) {
|
flash_vendor_read(i, test_buf, i + 1);
|
DLOG("id = %d ,size = %d\n", i, i + 1);
|
print_hex("data:", test_buf, 1, i + 1);
|
}
|
flash_vendor_init();
|
memset(test_buf, 0, 512);
|
for (i = 0; i < 62; i++) {
|
flash_vendor_read(i, test_buf, i + 1);
|
DLOG("id = %d ,size = %d\n", i, i + 1);
|
print_hex("data:", test_buf, 1, i + 1);
|
}
|
while (1)
|
;
|
}
|
#endif
|
|
static long vendor_storage_ioctl(struct file *file,
|
unsigned int cmd,
|
unsigned long arg)
|
{
|
long ret = -EINVAL;
|
int size;
|
u32 *temp_buf;
|
struct RK_VENDOR_REQ *req;
|
|
req = kmalloc(sizeof(*req), GFP_KERNEL);
|
if (!req)
|
return ret;
|
|
temp_buf = (u32 *)req;
|
|
switch (cmd) {
|
case VENDOR_READ_IO:
|
{
|
if (copy_from_user(temp_buf,
|
(void __user *)arg,
|
sizeof(*req))) {
|
DLOG("copy_from_user error\n");
|
ret = -EFAULT;
|
break;
|
}
|
if (req->tag == VENDOR_REQ_TAG) {
|
size = flash_vendor_read(req->id,
|
req->data,
|
req->len);
|
if (size > 0) {
|
req->len = size;
|
ret = 0;
|
if (copy_to_user((void __user *)arg,
|
temp_buf,
|
sizeof(*req)))
|
ret = -EFAULT;
|
}
|
}
|
} break;
|
case VENDOR_WRITE_IO:
|
{
|
if (copy_from_user(temp_buf,
|
(void __user *)arg,
|
sizeof(struct RK_VENDOR_REQ))) {
|
DLOG("copy_from_user error\n");
|
ret = -EFAULT;
|
break;
|
}
|
if (req->tag == VENDOR_REQ_TAG)
|
ret = flash_vendor_write(req->id,
|
req->data,
|
req->len);
|
} break;
|
default:
|
return -EINVAL;
|
}
|
kfree(temp_buf);
|
DLOG("flash_vendor_ioctl cmd=%x ret = %lx\n", cmd, ret);
|
return ret;
|
}
|
|
static const struct file_operations vendor_storage_fops = {
|
.compat_ioctl = vendor_storage_ioctl,
|
.unlocked_ioctl = vendor_storage_ioctl,
|
};
|
|
static struct miscdevice vender_storage_dev = {
|
.minor = MISC_DYNAMIC_MINOR,
|
.name = "vendor_storage",
|
.fops = &vendor_storage_fops,
|
};
|
|
static int vendor_init_thread(void *arg)
|
{
|
int ret;
|
|
pr_info("flash %s!\n", __func__);
|
ret = flash_vendor_init();
|
if (!ret) {
|
ret = misc_register(&vender_storage_dev);
|
#ifdef CONFIG_ROCKCHIP_VENDOR_STORAGE
|
rk_vendor_register(flash_vendor_read, flash_vendor_write);
|
#endif
|
}
|
pr_info("flash vendor storage:20170308 ret = %d\n", ret);
|
return ret;
|
}
|
|
static int __init vendor_storage_init(void)
|
{
|
kthread_run(vendor_init_thread, (void *)NULL, "vendor_storage_init");
|
return 0;
|
}
|
|
static __exit void vendor_storage_deinit(void)
|
{
|
if (g_vendor) {
|
misc_deregister(&vender_storage_dev);
|
kfree(g_vendor);
|
g_vendor = NULL;
|
}
|
}
|
|
device_initcall_sync(vendor_storage_init);
|
module_exit(vendor_storage_deinit);
|
MODULE_LICENSE("GPL");
|
