1 // SPDX-License-Identifier: (GPL-2.0+ OR MIT)
2
3 /* Copyright (c) 2018 Fuzhou Rockchip Electronics Co., Ltd */
4
5 #include <linux/fs.h>
6 #include <linux/kthread.h>
7 #include <linux/miscdevice.h>
8 #include <linux/module.h>
9 #include <linux/platform_device.h>
10 #include <linux/slab.h>
11 #include <linux/soc/rockchip/rk_vendor_storage.h>
12 #include <linux/uaccess.h>
13 #include <misc/rkflash_vendor_storage.h>
14
15 #include "flash_vendor_storage.h"
16
17 #define FLASH_VENDOR_TEST 0
18 #define DRM_DEBUG 1
19
20 #if DRM_DEBUG
21 #define DLOG(fmt, args...) pr_info(fmt, ##args)
22 #else
23 #define DLOG(x...)
24 #endif
25
26 #define FLASH_VENDOR_PART_START 8
27 #define FLASH_VENDOR_PART_NUM 4
28 #define FLASH_VENDOR_TAG VENDOR_HEAD_TAG
29
30 static int (*_flash_read)(u32 sec, u32 n_sec, void *p_data);
31 static int (*_flash_write)(u32 sec, u32 n_sec, void *p_data);
32 static struct flash_vendor_info *g_vendor;
33
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))34 int flash_vendor_dev_ops_register(int (*read)(u32 sec,
35 u32 n_sec,
36 void *p_data),
37 int (*write)(u32 sec,
38 u32 n_sec,
39 void *p_data))
40 {
41 if (!_flash_read) {
42 _flash_read = read;
43 _flash_write = write;
44 return 0;
45 }
46 return -1;
47 }
48
flash_vendor_init(void)49 static u32 flash_vendor_init(void)
50 {
51 u32 i, max_ver, max_index;
52
53 if (!_flash_read)
54 return -EPERM;
55
56 g_vendor = kmalloc(sizeof(*g_vendor), GFP_KERNEL | GFP_DMA);
57 if (!g_vendor)
58 return 0;
59
60 max_ver = 0;
61 max_index = 0;
62 for (i = 0; i < FLASH_VENDOR_PART_NUM; i++) {
63 _flash_read(FLASH_VENDOR_PART_START +
64 FLASH_VENDOR_PART_SIZE * i,
65 FLASH_VENDOR_PART_SIZE,
66 g_vendor);
67 if (g_vendor->tag == FLASH_VENDOR_TAG &&
68 g_vendor->version == g_vendor->version2) {
69 if (max_ver < g_vendor->version) {
70 max_index = i;
71 max_ver = g_vendor->version;
72 }
73 }
74 }
75 /* DLOG("max_ver = %d\n",max_ver); */
76 if (max_ver) {
77 _flash_read(FLASH_VENDOR_PART_START +
78 FLASH_VENDOR_PART_SIZE * max_index,
79 FLASH_VENDOR_PART_SIZE,
80 g_vendor);
81 } else {
82 memset(g_vendor, 0, sizeof(*g_vendor));
83 g_vendor->version = 1;
84 g_vendor->tag = FLASH_VENDOR_TAG;
85 g_vendor->version2 = g_vendor->version;
86 g_vendor->free_offset = 0;
87 g_vendor->free_size = sizeof(g_vendor->data);
88 }
89 /* rknand_print_hex("vendor:", g_vendor, 4, 1024); */
90
91 return 0;
92 }
93
flash_vendor_read(u32 id,void * pbuf,u32 size)94 static int flash_vendor_read(u32 id, void *pbuf, u32 size)
95 {
96 u32 i;
97
98 if (!g_vendor)
99 return -1;
100
101 for (i = 0; i < g_vendor->item_num; i++) {
102 if (g_vendor->item[i].id == id) {
103 if (size > g_vendor->item[i].size)
104 size = g_vendor->item[i].size;
105 memcpy(pbuf,
106 &g_vendor->data[g_vendor->item[i].offset],
107 size);
108 return size;
109 }
110 }
111 return (-1);
112 }
113
flash_vendor_write(u32 id,void * pbuf,u32 size)114 static int flash_vendor_write(u32 id, void *pbuf, u32 size)
115 {
116 u32 i, j, next_index, align_size, alloc_size, item_num;
117 u32 offset, next_size;
118 u8 *p_data;
119 struct vendor_item *item;
120 struct vendor_item *next_item;
121
122 if (!g_vendor)
123 return -1;
124
125 p_data = g_vendor->data;
126 item_num = g_vendor->item_num;
127 align_size = ALIGN(size, 0x40); /* align to 64 bytes*/
128 next_index = g_vendor->next_index;
129 for (i = 0; i < item_num; i++) {
130 item = &g_vendor->item[i];
131 if (item->id == id) {
132 alloc_size = ALIGN(item->size, 0x40);
133 if (size > alloc_size) {
134 if (g_vendor->free_size < align_size)
135 return -1;
136 offset = item->offset;
137 for (j = i; j < item_num - 1; j++) {
138 item = &g_vendor->item[j];
139 next_item = &g_vendor->item[j + 1];
140 item->id = next_item->id;
141 item->size = next_item->size;
142 item->offset = offset;
143 next_size = ALIGN(next_item->size,
144 0x40);
145 memcpy(&p_data[offset],
146 &p_data[next_item->offset],
147 next_size);
148 offset += next_size;
149 }
150 item = &g_vendor->item[j];
151 item->id = id;
152 item->offset = offset;
153 item->size = size;
154 memcpy(&p_data[item->offset], pbuf, size);
155 g_vendor->free_offset = offset + align_size;
156 g_vendor->free_size -= (align_size -
157 alloc_size);
158 } else {
159 memcpy(&p_data[item->offset],
160 pbuf,
161 size);
162 g_vendor->item[i].size = size;
163 }
164 g_vendor->version++;
165 g_vendor->version2 = g_vendor->version;
166 g_vendor->next_index++;
167 if (g_vendor->next_index >= FLASH_VENDOR_PART_NUM)
168 g_vendor->next_index = 0;
169 _flash_write(FLASH_VENDOR_PART_START +
170 FLASH_VENDOR_PART_SIZE * next_index,
171 FLASH_VENDOR_PART_SIZE,
172 g_vendor);
173 return 0;
174 }
175 }
176
177 if (g_vendor->free_size >= align_size) {
178 item = &g_vendor->item[g_vendor->item_num];
179 item->id = id;
180 item->offset = g_vendor->free_offset;
181 item->size = align_size;
182 item->size = size;
183 g_vendor->free_offset += align_size;
184 g_vendor->free_size -= align_size;
185 memcpy(&g_vendor->data[item->offset], pbuf, size);
186 g_vendor->item_num++;
187 g_vendor->version++;
188 g_vendor->next_index++;
189 g_vendor->version2 = g_vendor->version;
190 if (g_vendor->next_index >= FLASH_VENDOR_PART_NUM)
191 g_vendor->next_index = 0;
192 _flash_write(FLASH_VENDOR_PART_START +
193 FLASH_VENDOR_PART_SIZE * next_index,
194 FLASH_VENDOR_PART_SIZE,
195 g_vendor);
196 return 0;
197 }
198
199 return(-1);
200 }
201
202 #if (FLASH_VENDOR_TEST)
print_hex(char * s,void * buf,int width,int len)203 static void print_hex(char *s, void *buf, int width, int len)
204 {
205 print_hex_dump(KERN_WARNING, s, DUMP_PREFIX_OFFSET,
206 16, width, buf, len * width, 0);
207 }
208
flash_vendor_test(void)209 static void flash_vendor_test(void)
210 {
211 u32 i;
212 u8 test_buf[512];
213
214 memset(test_buf, 0, 512);
215 for (i = 0; i < 62; i++) {
216 memset(test_buf, i, i + 1);
217 flash_vendor_write(i, test_buf, i + 1);
218 }
219 memset(test_buf, 0, 512);
220 for (i = 0; i < 62; i++) {
221 flash_vendor_read(i, test_buf, i + 1);
222 DLOG("id = %d ,size = %d\n", i, i + 1);
223 print_hex("data:", test_buf, 1, i + 1);
224 }
225 flash_vendor_init();
226 memset(test_buf, 0, 512);
227 for (i = 0; i < 62; i++) {
228 flash_vendor_read(i, test_buf, i + 1);
229 DLOG("id = %d ,size = %d\n", i, i + 1);
230 print_hex("data:", test_buf, 1, i + 1);
231 }
232 while (1)
233 ;
234 }
235 #endif
236
vendor_storage_ioctl(struct file * file,unsigned int cmd,unsigned long arg)237 static long vendor_storage_ioctl(struct file *file,
238 unsigned int cmd,
239 unsigned long arg)
240 {
241 long ret = -EINVAL;
242 int size;
243 u32 *temp_buf;
244 struct RK_VENDOR_REQ *req;
245
246 req = kmalloc(sizeof(*req), GFP_KERNEL);
247 if (!req)
248 return ret;
249
250 temp_buf = (u32 *)req;
251
252 switch (cmd) {
253 case VENDOR_READ_IO:
254 {
255 if (copy_from_user(temp_buf,
256 (void __user *)arg,
257 sizeof(*req))) {
258 DLOG("copy_from_user error\n");
259 ret = -EFAULT;
260 break;
261 }
262 if (req->tag == VENDOR_REQ_TAG) {
263 size = flash_vendor_read(req->id,
264 req->data,
265 req->len);
266 if (size > 0) {
267 req->len = size;
268 ret = 0;
269 if (copy_to_user((void __user *)arg,
270 temp_buf,
271 sizeof(*req)))
272 ret = -EFAULT;
273 }
274 }
275 } break;
276 case VENDOR_WRITE_IO:
277 {
278 if (copy_from_user(temp_buf,
279 (void __user *)arg,
280 sizeof(struct RK_VENDOR_REQ))) {
281 DLOG("copy_from_user error\n");
282 ret = -EFAULT;
283 break;
284 }
285 if (req->tag == VENDOR_REQ_TAG)
286 ret = flash_vendor_write(req->id,
287 req->data,
288 req->len);
289 } break;
290 default:
291 return -EINVAL;
292 }
293 kfree(temp_buf);
294 DLOG("flash_vendor_ioctl cmd=%x ret = %lx\n", cmd, ret);
295 return ret;
296 }
297
298 static const struct file_operations vendor_storage_fops = {
299 .compat_ioctl = vendor_storage_ioctl,
300 .unlocked_ioctl = vendor_storage_ioctl,
301 };
302
303 static struct miscdevice vender_storage_dev = {
304 .minor = MISC_DYNAMIC_MINOR,
305 .name = "vendor_storage",
306 .fops = &vendor_storage_fops,
307 };
308
vendor_init_thread(void * arg)309 static int vendor_init_thread(void *arg)
310 {
311 int ret;
312
313 pr_info("flash %s!\n", __func__);
314 ret = flash_vendor_init();
315 if (!ret) {
316 ret = misc_register(&vender_storage_dev);
317 #ifdef CONFIG_ROCKCHIP_VENDOR_STORAGE
318 rk_vendor_register(flash_vendor_read, flash_vendor_write);
319 #endif
320 }
321 pr_info("flash vendor storage:20170308 ret = %d\n", ret);
322 return ret;
323 }
324
vendor_storage_init(void)325 static int __init vendor_storage_init(void)
326 {
327 kthread_run(vendor_init_thread, (void *)NULL, "vendor_storage_init");
328 return 0;
329 }
330
vendor_storage_deinit(void)331 static __exit void vendor_storage_deinit(void)
332 {
333 if (g_vendor) {
334 misc_deregister(&vender_storage_dev);
335 kfree(g_vendor);
336 g_vendor = NULL;
337 }
338 }
339
340 device_initcall_sync(vendor_storage_init);
341 module_exit(vendor_storage_deinit);
342 MODULE_LICENSE("GPL");
343