Merge branch 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/dtor/input
[muen/linux.git] / drivers / input / rmi4 / rmi_f34v7.c
1 /*
2  * Copyright (c) 2016, Zodiac Inflight Innovations
3  * Copyright (c) 2007-2016, Synaptics Incorporated
4  * Copyright (C) 2012 Alexandra Chin <alexandra.chin@tw.synaptics.com>
5  * Copyright (C) 2012 Scott Lin <scott.lin@tw.synaptics.com>
6  *
7  * This program is free software; you can redistribute it and/or modify it
8  * under the terms of the GNU General Public License version 2 as published by
9  * the Free Software Foundation.
10  */
11
12 #include <linux/bitops.h>
13 #include <linux/kernel.h>
14 #include <linux/rmi.h>
15 #include <linux/firmware.h>
16 #include <linux/delay.h>
17 #include <linux/slab.h>
18 #include <linux/jiffies.h>
19 #include <asm/unaligned.h>
20
21 #include "rmi_driver.h"
22 #include "rmi_f34.h"
23
24 static int rmi_f34v7_read_flash_status(struct f34_data *f34)
25 {
26         u8 status;
27         u8 command;
28         int ret;
29
30         ret = rmi_read_block(f34->fn->rmi_dev,
31                         f34->fn->fd.data_base_addr + f34->v7.off.flash_status,
32                         &status,
33                         sizeof(status));
34         if (ret < 0) {
35                 rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
36                         "%s: Error %d reading flash status\n", __func__, ret);
37                 return ret;
38         }
39
40         f34->v7.in_bl_mode = status >> 7;
41         f34->v7.flash_status = status & 0x1f;
42
43         if (f34->v7.flash_status != 0x00) {
44                 dev_err(&f34->fn->dev, "%s: status=%d, command=0x%02x\n",
45                         __func__, f34->v7.flash_status, f34->v7.command);
46         }
47
48         ret = rmi_read_block(f34->fn->rmi_dev,
49                         f34->fn->fd.data_base_addr + f34->v7.off.flash_cmd,
50                         &command,
51                         sizeof(command));
52         if (ret < 0) {
53                 dev_err(&f34->fn->dev, "%s: Failed to read flash command\n",
54                         __func__);
55                 return ret;
56         }
57
58         f34->v7.command = command;
59
60         return 0;
61 }
62
63 static int rmi_f34v7_wait_for_idle(struct f34_data *f34, int timeout_ms)
64 {
65         unsigned long timeout;
66
67         timeout = msecs_to_jiffies(timeout_ms);
68
69         if (!wait_for_completion_timeout(&f34->v7.cmd_done, timeout)) {
70                 dev_warn(&f34->fn->dev, "%s: Timed out waiting for idle status\n",
71                          __func__);
72                 return -ETIMEDOUT;
73         }
74
75         return 0;
76 }
77
78 static int rmi_f34v7_write_command_single_transaction(struct f34_data *f34,
79                                                       u8 cmd)
80 {
81         int ret;
82         u8 base;
83         struct f34v7_data_1_5 data_1_5;
84
85         base = f34->fn->fd.data_base_addr;
86
87         memset(&data_1_5, 0, sizeof(data_1_5));
88
89         switch (cmd) {
90         case v7_CMD_ERASE_ALL:
91                 data_1_5.partition_id = CORE_CODE_PARTITION;
92                 data_1_5.command = CMD_V7_ERASE_AP;
93                 break;
94         case v7_CMD_ERASE_UI_FIRMWARE:
95                 data_1_5.partition_id = CORE_CODE_PARTITION;
96                 data_1_5.command = CMD_V7_ERASE;
97                 break;
98         case v7_CMD_ERASE_BL_CONFIG:
99                 data_1_5.partition_id = GLOBAL_PARAMETERS_PARTITION;
100                 data_1_5.command = CMD_V7_ERASE;
101                 break;
102         case v7_CMD_ERASE_UI_CONFIG:
103                 data_1_5.partition_id = CORE_CONFIG_PARTITION;
104                 data_1_5.command = CMD_V7_ERASE;
105                 break;
106         case v7_CMD_ERASE_DISP_CONFIG:
107                 data_1_5.partition_id = DISPLAY_CONFIG_PARTITION;
108                 data_1_5.command = CMD_V7_ERASE;
109                 break;
110         case v7_CMD_ERASE_FLASH_CONFIG:
111                 data_1_5.partition_id = FLASH_CONFIG_PARTITION;
112                 data_1_5.command = CMD_V7_ERASE;
113                 break;
114         case v7_CMD_ERASE_GUEST_CODE:
115                 data_1_5.partition_id = GUEST_CODE_PARTITION;
116                 data_1_5.command = CMD_V7_ERASE;
117                 break;
118         case v7_CMD_ENABLE_FLASH_PROG:
119                 data_1_5.partition_id = BOOTLOADER_PARTITION;
120                 data_1_5.command = CMD_V7_ENTER_BL;
121                 break;
122         }
123
124         data_1_5.payload[0] = f34->bootloader_id[0];
125         data_1_5.payload[1] = f34->bootloader_id[1];
126
127         ret = rmi_write_block(f34->fn->rmi_dev,
128                         base + f34->v7.off.partition_id,
129                         &data_1_5, sizeof(data_1_5));
130         if (ret < 0) {
131                 dev_err(&f34->fn->dev,
132                         "%s: Failed to write single transaction command\n",
133                         __func__);
134                 return ret;
135         }
136
137         return 0;
138 }
139
140 static int rmi_f34v7_write_command(struct f34_data *f34, u8 cmd)
141 {
142         int ret;
143         u8 base;
144         u8 command;
145
146         base = f34->fn->fd.data_base_addr;
147
148         switch (cmd) {
149         case v7_CMD_WRITE_FW:
150         case v7_CMD_WRITE_CONFIG:
151         case v7_CMD_WRITE_GUEST_CODE:
152                 command = CMD_V7_WRITE;
153                 break;
154         case v7_CMD_READ_CONFIG:
155                 command = CMD_V7_READ;
156                 break;
157         case v7_CMD_ERASE_ALL:
158                 command = CMD_V7_ERASE_AP;
159                 break;
160         case v7_CMD_ERASE_UI_FIRMWARE:
161         case v7_CMD_ERASE_BL_CONFIG:
162         case v7_CMD_ERASE_UI_CONFIG:
163         case v7_CMD_ERASE_DISP_CONFIG:
164         case v7_CMD_ERASE_FLASH_CONFIG:
165         case v7_CMD_ERASE_GUEST_CODE:
166                 command = CMD_V7_ERASE;
167                 break;
168         case v7_CMD_ENABLE_FLASH_PROG:
169                 command = CMD_V7_ENTER_BL;
170                 break;
171         default:
172                 dev_err(&f34->fn->dev, "%s: Invalid command 0x%02x\n",
173                         __func__, cmd);
174                 return -EINVAL;
175         }
176
177         f34->v7.command = command;
178
179         switch (cmd) {
180         case v7_CMD_ERASE_ALL:
181         case v7_CMD_ERASE_UI_FIRMWARE:
182         case v7_CMD_ERASE_BL_CONFIG:
183         case v7_CMD_ERASE_UI_CONFIG:
184         case v7_CMD_ERASE_DISP_CONFIG:
185         case v7_CMD_ERASE_FLASH_CONFIG:
186         case v7_CMD_ERASE_GUEST_CODE:
187         case v7_CMD_ENABLE_FLASH_PROG:
188                 ret = rmi_f34v7_write_command_single_transaction(f34, cmd);
189                 if (ret < 0)
190                         return ret;
191                 else
192                         return 0;
193         default:
194                 break;
195         }
196
197         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev, "%s: writing cmd %02X\n",
198                 __func__, command);
199
200         ret = rmi_write_block(f34->fn->rmi_dev,
201                         base + f34->v7.off.flash_cmd,
202                         &command, sizeof(command));
203         if (ret < 0) {
204                 dev_err(&f34->fn->dev, "%s: Failed to write flash command\n",
205                         __func__);
206                 return ret;
207         }
208
209         return 0;
210 }
211
212 static int rmi_f34v7_write_partition_id(struct f34_data *f34, u8 cmd)
213 {
214         int ret;
215         u8 base;
216         u8 partition;
217
218         base = f34->fn->fd.data_base_addr;
219
220         switch (cmd) {
221         case v7_CMD_WRITE_FW:
222                 partition = CORE_CODE_PARTITION;
223                 break;
224         case v7_CMD_WRITE_CONFIG:
225         case v7_CMD_READ_CONFIG:
226                 if (f34->v7.config_area == v7_UI_CONFIG_AREA)
227                         partition = CORE_CONFIG_PARTITION;
228                 else if (f34->v7.config_area == v7_DP_CONFIG_AREA)
229                         partition = DISPLAY_CONFIG_PARTITION;
230                 else if (f34->v7.config_area == v7_PM_CONFIG_AREA)
231                         partition = GUEST_SERIALIZATION_PARTITION;
232                 else if (f34->v7.config_area == v7_BL_CONFIG_AREA)
233                         partition = GLOBAL_PARAMETERS_PARTITION;
234                 else if (f34->v7.config_area == v7_FLASH_CONFIG_AREA)
235                         partition = FLASH_CONFIG_PARTITION;
236                 break;
237         case v7_CMD_WRITE_GUEST_CODE:
238                 partition = GUEST_CODE_PARTITION;
239                 break;
240         case v7_CMD_ERASE_ALL:
241                 partition = CORE_CODE_PARTITION;
242                 break;
243         case v7_CMD_ERASE_BL_CONFIG:
244                 partition = GLOBAL_PARAMETERS_PARTITION;
245                 break;
246         case v7_CMD_ERASE_UI_CONFIG:
247                 partition = CORE_CONFIG_PARTITION;
248                 break;
249         case v7_CMD_ERASE_DISP_CONFIG:
250                 partition = DISPLAY_CONFIG_PARTITION;
251                 break;
252         case v7_CMD_ERASE_FLASH_CONFIG:
253                 partition = FLASH_CONFIG_PARTITION;
254                 break;
255         case v7_CMD_ERASE_GUEST_CODE:
256                 partition = GUEST_CODE_PARTITION;
257                 break;
258         case v7_CMD_ENABLE_FLASH_PROG:
259                 partition = BOOTLOADER_PARTITION;
260                 break;
261         default:
262                 dev_err(&f34->fn->dev, "%s: Invalid command 0x%02x\n",
263                         __func__, cmd);
264                 return -EINVAL;
265         }
266
267         ret = rmi_write_block(f34->fn->rmi_dev,
268                         base + f34->v7.off.partition_id,
269                         &partition, sizeof(partition));
270         if (ret < 0) {
271                 dev_err(&f34->fn->dev, "%s: Failed to write partition ID\n",
272                         __func__);
273                 return ret;
274         }
275
276         return 0;
277 }
278
279 static int rmi_f34v7_read_partition_table(struct f34_data *f34)
280 {
281         int ret;
282         unsigned long timeout;
283         u8 base;
284         __le16 length;
285         u16 block_number = 0;
286
287         base = f34->fn->fd.data_base_addr;
288
289         f34->v7.config_area = v7_FLASH_CONFIG_AREA;
290
291         ret = rmi_f34v7_write_partition_id(f34, v7_CMD_READ_CONFIG);
292         if (ret < 0)
293                 return ret;
294
295         ret = rmi_write_block(f34->fn->rmi_dev,
296                         base + f34->v7.off.block_number,
297                         &block_number, sizeof(block_number));
298         if (ret < 0) {
299                 dev_err(&f34->fn->dev, "%s: Failed to write block number\n",
300                         __func__);
301                 return ret;
302         }
303
304         put_unaligned_le16(f34->v7.flash_config_length, &length);
305
306         ret = rmi_write_block(f34->fn->rmi_dev,
307                         base + f34->v7.off.transfer_length,
308                         &length, sizeof(length));
309         if (ret < 0) {
310                 dev_err(&f34->fn->dev, "%s: Failed to write transfer length\n",
311                         __func__);
312                 return ret;
313         }
314
315         init_completion(&f34->v7.cmd_done);
316
317         ret = rmi_f34v7_write_command(f34, v7_CMD_READ_CONFIG);
318         if (ret < 0) {
319                 dev_err(&f34->fn->dev, "%s: Failed to write command\n",
320                         __func__);
321                 return ret;
322         }
323
324         timeout = msecs_to_jiffies(F34_WRITE_WAIT_MS);
325         while (time_before(jiffies, timeout)) {
326                 usleep_range(5000, 6000);
327                 rmi_f34v7_read_flash_status(f34);
328
329                 if (f34->v7.command == v7_CMD_IDLE &&
330                     f34->v7.flash_status == 0x00) {
331                         break;
332                 }
333         }
334
335         ret = rmi_read_block(f34->fn->rmi_dev,
336                         base + f34->v7.off.payload,
337                         f34->v7.read_config_buf,
338                         f34->v7.partition_table_bytes);
339         if (ret < 0) {
340                 dev_err(&f34->fn->dev, "%s: Failed to read block data\n",
341                         __func__);
342                 return ret;
343         }
344
345         return 0;
346 }
347
348 static void rmi_f34v7_parse_partition_table(struct f34_data *f34,
349                                             const void *partition_table,
350                                             struct block_count *blkcount,
351                                             struct physical_address *phyaddr)
352 {
353         int i;
354         int index;
355         u16 partition_length;
356         u16 physical_address;
357         const struct partition_table *ptable;
358
359         for (i = 0; i < f34->v7.partitions; i++) {
360                 index = i * 8 + 2;
361                 ptable = partition_table + index;
362                 partition_length = le16_to_cpu(ptable->partition_length);
363                 physical_address = le16_to_cpu(ptable->start_physical_address);
364                 rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
365                         "%s: Partition entry %d: %*ph\n",
366                         __func__, i, sizeof(struct partition_table), ptable);
367                 switch (ptable->partition_id & 0x1f) {
368                 case CORE_CODE_PARTITION:
369                         blkcount->ui_firmware = partition_length;
370                         phyaddr->ui_firmware = physical_address;
371                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
372                                 "%s: Core code block count: %d\n",
373                                 __func__, blkcount->ui_firmware);
374                         break;
375                 case CORE_CONFIG_PARTITION:
376                         blkcount->ui_config = partition_length;
377                         phyaddr->ui_config = physical_address;
378                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
379                                 "%s: Core config block count: %d\n",
380                                 __func__, blkcount->ui_config);
381                         break;
382                 case DISPLAY_CONFIG_PARTITION:
383                         blkcount->dp_config = partition_length;
384                         phyaddr->dp_config = physical_address;
385                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
386                                 "%s: Display config block count: %d\n",
387                                 __func__, blkcount->dp_config);
388                         break;
389                 case FLASH_CONFIG_PARTITION:
390                         blkcount->fl_config = partition_length;
391                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
392                                 "%s: Flash config block count: %d\n",
393                                 __func__, blkcount->fl_config);
394                         break;
395                 case GUEST_CODE_PARTITION:
396                         blkcount->guest_code = partition_length;
397                         phyaddr->guest_code = physical_address;
398                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
399                                 "%s: Guest code block count: %d\n",
400                                 __func__, blkcount->guest_code);
401                         break;
402                 case GUEST_SERIALIZATION_PARTITION:
403                         blkcount->pm_config = partition_length;
404                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
405                                 "%s: Guest serialization block count: %d\n",
406                                 __func__, blkcount->pm_config);
407                         break;
408                 case GLOBAL_PARAMETERS_PARTITION:
409                         blkcount->bl_config = partition_length;
410                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
411                                 "%s: Global parameters block count: %d\n",
412                                 __func__, blkcount->bl_config);
413                         break;
414                 case DEVICE_CONFIG_PARTITION:
415                         blkcount->lockdown = partition_length;
416                         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
417                                 "%s: Device config block count: %d\n",
418                                 __func__, blkcount->lockdown);
419                         break;
420                 }
421         }
422 }
423
424 static int rmi_f34v7_read_queries_bl_version(struct f34_data *f34)
425 {
426         int ret;
427         u8 base;
428         int offset;
429         u8 query_0;
430         struct f34v7_query_1_7 query_1_7;
431
432         base = f34->fn->fd.query_base_addr;
433
434         ret = rmi_read_block(f34->fn->rmi_dev,
435                         base,
436                         &query_0,
437                         sizeof(query_0));
438         if (ret < 0) {
439                 dev_err(&f34->fn->dev,
440                         "%s: Failed to read query 0\n", __func__);
441                 return ret;
442         }
443
444         offset = (query_0 & 0x7) + 1;
445
446         ret = rmi_read_block(f34->fn->rmi_dev,
447                         base + offset,
448                         &query_1_7,
449                         sizeof(query_1_7));
450         if (ret < 0) {
451                 dev_err(&f34->fn->dev, "%s: Failed to read queries 1 to 7\n",
452                         __func__);
453                 return ret;
454         }
455
456         f34->bootloader_id[0] = query_1_7.bl_minor_revision;
457         f34->bootloader_id[1] = query_1_7.bl_major_revision;
458
459         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev, "Bootloader V%d.%d\n",
460                 f34->bootloader_id[1], f34->bootloader_id[0]);
461
462         return 0;
463 }
464
465 static int rmi_f34v7_read_queries(struct f34_data *f34)
466 {
467         int ret;
468         int i;
469         u8 base;
470         int offset;
471         u8 *ptable;
472         u8 query_0;
473         struct f34v7_query_1_7 query_1_7;
474
475         base = f34->fn->fd.query_base_addr;
476
477         ret = rmi_read_block(f34->fn->rmi_dev,
478                         base,
479                         &query_0,
480                         sizeof(query_0));
481         if (ret < 0) {
482                 dev_err(&f34->fn->dev,
483                         "%s: Failed to read query 0\n", __func__);
484                 return ret;
485         }
486
487         offset = (query_0 & 0x07) + 1;
488
489         ret = rmi_read_block(f34->fn->rmi_dev,
490                         base + offset,
491                         &query_1_7,
492                         sizeof(query_1_7));
493         if (ret < 0) {
494                 dev_err(&f34->fn->dev, "%s: Failed to read queries 1 to 7\n",
495                         __func__);
496                 return ret;
497         }
498
499         f34->bootloader_id[0] = query_1_7.bl_minor_revision;
500         f34->bootloader_id[1] = query_1_7.bl_major_revision;
501
502         f34->v7.block_size = le16_to_cpu(query_1_7.block_size);
503         f34->v7.flash_config_length =
504                         le16_to_cpu(query_1_7.flash_config_length);
505         f34->v7.payload_length = le16_to_cpu(query_1_7.payload_length);
506
507         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev, "%s: f34->v7.block_size = %d\n",
508                  __func__, f34->v7.block_size);
509
510         f34->v7.off.flash_status = V7_FLASH_STATUS_OFFSET;
511         f34->v7.off.partition_id = V7_PARTITION_ID_OFFSET;
512         f34->v7.off.block_number = V7_BLOCK_NUMBER_OFFSET;
513         f34->v7.off.transfer_length = V7_TRANSFER_LENGTH_OFFSET;
514         f34->v7.off.flash_cmd = V7_COMMAND_OFFSET;
515         f34->v7.off.payload = V7_PAYLOAD_OFFSET;
516
517         f34->v7.has_display_cfg = query_1_7.partition_support[1] & HAS_DISP_CFG;
518         f34->v7.has_guest_code =
519                         query_1_7.partition_support[1] & HAS_GUEST_CODE;
520
521         if (query_0 & HAS_CONFIG_ID) {
522                 u8 f34_ctrl[CONFIG_ID_SIZE];
523
524                 ret = rmi_read_block(f34->fn->rmi_dev,
525                                 f34->fn->fd.control_base_addr,
526                                 f34_ctrl,
527                                 sizeof(f34_ctrl));
528                 if (ret)
529                         return ret;
530
531                 /* Eat leading zeros */
532                 for (i = 0; i < sizeof(f34_ctrl) - 1 && !f34_ctrl[i]; i++)
533                         /* Empty */;
534
535                 snprintf(f34->configuration_id, sizeof(f34->configuration_id),
536                          "%*phN", (int)sizeof(f34_ctrl) - i, f34_ctrl + i);
537
538                 rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev, "Configuration ID: %s\n",
539                         f34->configuration_id);
540         }
541
542         f34->v7.partitions = 0;
543         for (i = 0; i < sizeof(query_1_7.partition_support); i++)
544                 f34->v7.partitions += hweight8(query_1_7.partition_support[i]);
545
546         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev, "%s: Supported partitions: %*ph\n",
547                 __func__, sizeof(query_1_7.partition_support),
548                 query_1_7.partition_support);
549
550
551         f34->v7.partition_table_bytes = f34->v7.partitions * 8 + 2;
552
553         f34->v7.read_config_buf = devm_kzalloc(&f34->fn->dev,
554                         f34->v7.partition_table_bytes,
555                         GFP_KERNEL);
556         if (!f34->v7.read_config_buf) {
557                 f34->v7.read_config_buf_size = 0;
558                 return -ENOMEM;
559         }
560
561         f34->v7.read_config_buf_size = f34->v7.partition_table_bytes;
562         ptable = f34->v7.read_config_buf;
563
564         ret = rmi_f34v7_read_partition_table(f34);
565         if (ret < 0) {
566                 dev_err(&f34->fn->dev, "%s: Failed to read partition table\n",
567                                 __func__);
568                 return ret;
569         }
570
571         rmi_f34v7_parse_partition_table(f34, ptable,
572                                         &f34->v7.blkcount, &f34->v7.phyaddr);
573
574         return 0;
575 }
576
577 static int rmi_f34v7_check_ui_firmware_size(struct f34_data *f34)
578 {
579         u16 block_count;
580
581         block_count = f34->v7.img.ui_firmware.size / f34->v7.block_size;
582         f34->update_size += block_count;
583
584         if (block_count != f34->v7.blkcount.ui_firmware) {
585                 dev_err(&f34->fn->dev,
586                         "UI firmware size mismatch: %d != %d\n",
587                         block_count, f34->v7.blkcount.ui_firmware);
588                 return -EINVAL;
589         }
590
591         return 0;
592 }
593
594 static int rmi_f34v7_check_ui_config_size(struct f34_data *f34)
595 {
596         u16 block_count;
597
598         block_count = f34->v7.img.ui_config.size / f34->v7.block_size;
599         f34->update_size += block_count;
600
601         if (block_count != f34->v7.blkcount.ui_config) {
602                 dev_err(&f34->fn->dev, "UI config size mismatch\n");
603                 return -EINVAL;
604         }
605
606         return 0;
607 }
608
609 static int rmi_f34v7_check_dp_config_size(struct f34_data *f34)
610 {
611         u16 block_count;
612
613         block_count = f34->v7.img.dp_config.size / f34->v7.block_size;
614         f34->update_size += block_count;
615
616         if (block_count != f34->v7.blkcount.dp_config) {
617                 dev_err(&f34->fn->dev, "Display config size mismatch\n");
618                 return -EINVAL;
619         }
620
621         return 0;
622 }
623
624 static int rmi_f34v7_check_guest_code_size(struct f34_data *f34)
625 {
626         u16 block_count;
627
628         block_count = f34->v7.img.guest_code.size / f34->v7.block_size;
629         f34->update_size += block_count;
630
631         if (block_count != f34->v7.blkcount.guest_code) {
632                 dev_err(&f34->fn->dev, "Guest code size mismatch\n");
633                 return -EINVAL;
634         }
635
636         return 0;
637 }
638
639 static int rmi_f34v7_check_bl_config_size(struct f34_data *f34)
640 {
641         u16 block_count;
642
643         block_count = f34->v7.img.bl_config.size / f34->v7.block_size;
644         f34->update_size += block_count;
645
646         if (block_count != f34->v7.blkcount.bl_config) {
647                 dev_err(&f34->fn->dev, "Bootloader config size mismatch\n");
648                 return -EINVAL;
649         }
650
651         return 0;
652 }
653
654 static int rmi_f34v7_erase_config(struct f34_data *f34)
655 {
656         int ret;
657
658         dev_info(&f34->fn->dev, "Erasing config...\n");
659
660         init_completion(&f34->v7.cmd_done);
661
662         switch (f34->v7.config_area) {
663         case v7_UI_CONFIG_AREA:
664                 ret = rmi_f34v7_write_command(f34, v7_CMD_ERASE_UI_CONFIG);
665                 if (ret < 0)
666                         return ret;
667                 break;
668         case v7_DP_CONFIG_AREA:
669                 ret = rmi_f34v7_write_command(f34, v7_CMD_ERASE_DISP_CONFIG);
670                 if (ret < 0)
671                         return ret;
672                 break;
673         case v7_BL_CONFIG_AREA:
674                 ret = rmi_f34v7_write_command(f34, v7_CMD_ERASE_BL_CONFIG);
675                 if (ret < 0)
676                         return ret;
677                 break;
678         }
679
680         ret = rmi_f34v7_wait_for_idle(f34, F34_ERASE_WAIT_MS);
681         if (ret < 0)
682                 return ret;
683
684         return 0;
685 }
686
687 static int rmi_f34v7_erase_guest_code(struct f34_data *f34)
688 {
689         int ret;
690
691         dev_info(&f34->fn->dev, "Erasing guest code...\n");
692
693         init_completion(&f34->v7.cmd_done);
694
695         ret = rmi_f34v7_write_command(f34, v7_CMD_ERASE_GUEST_CODE);
696         if (ret < 0)
697                 return ret;
698
699         ret = rmi_f34v7_wait_for_idle(f34, F34_ERASE_WAIT_MS);
700         if (ret < 0)
701                 return ret;
702
703         return 0;
704 }
705
706 static int rmi_f34v7_erase_all(struct f34_data *f34)
707 {
708         int ret;
709
710         dev_info(&f34->fn->dev, "Erasing firmware...\n");
711
712         init_completion(&f34->v7.cmd_done);
713
714         ret = rmi_f34v7_write_command(f34, v7_CMD_ERASE_UI_FIRMWARE);
715         if (ret < 0)
716                 return ret;
717
718         ret = rmi_f34v7_wait_for_idle(f34, F34_ERASE_WAIT_MS);
719         if (ret < 0)
720                 return ret;
721
722         f34->v7.config_area = v7_UI_CONFIG_AREA;
723         ret = rmi_f34v7_erase_config(f34);
724         if (ret < 0)
725                 return ret;
726
727         if (f34->v7.has_display_cfg) {
728                 f34->v7.config_area = v7_DP_CONFIG_AREA;
729                 ret = rmi_f34v7_erase_config(f34);
730                 if (ret < 0)
731                         return ret;
732         }
733
734         if (f34->v7.new_partition_table && f34->v7.has_guest_code) {
735                 ret = rmi_f34v7_erase_guest_code(f34);
736                 if (ret < 0)
737                         return ret;
738         }
739
740         return 0;
741 }
742
743 static int rmi_f34v7_read_blocks(struct f34_data *f34,
744                                  u16 block_cnt, u8 command)
745 {
746         int ret;
747         u8 base;
748         __le16 length;
749         u16 transfer;
750         u16 max_transfer;
751         u16 remaining = block_cnt;
752         u16 block_number = 0;
753         u16 index = 0;
754
755         base = f34->fn->fd.data_base_addr;
756
757         ret = rmi_f34v7_write_partition_id(f34, command);
758         if (ret < 0)
759                 return ret;
760
761         ret = rmi_write_block(f34->fn->rmi_dev,
762                         base + f34->v7.off.block_number,
763                         &block_number, sizeof(block_number));
764         if (ret < 0) {
765                 dev_err(&f34->fn->dev, "%s: Failed to write block number\n",
766                         __func__);
767                 return ret;
768         }
769
770         max_transfer = min(f34->v7.payload_length,
771                            (u16)(PAGE_SIZE / f34->v7.block_size));
772
773         do {
774                 transfer = min(remaining, max_transfer);
775                 put_unaligned_le16(transfer, &length);
776
777                 ret = rmi_write_block(f34->fn->rmi_dev,
778                                 base + f34->v7.off.transfer_length,
779                                 &length, sizeof(length));
780                 if (ret < 0) {
781                         dev_err(&f34->fn->dev,
782                                 "%s: Write transfer length fail (%d remaining)\n",
783                                 __func__, remaining);
784                         return ret;
785                 }
786
787                 init_completion(&f34->v7.cmd_done);
788
789                 ret = rmi_f34v7_write_command(f34, command);
790                 if (ret < 0)
791                         return ret;
792
793                 ret = rmi_f34v7_wait_for_idle(f34, F34_ENABLE_WAIT_MS);
794                 if (ret < 0)
795                         return ret;
796
797                 ret = rmi_read_block(f34->fn->rmi_dev,
798                                 base + f34->v7.off.payload,
799                                 &f34->v7.read_config_buf[index],
800                                 transfer * f34->v7.block_size);
801                 if (ret < 0) {
802                         dev_err(&f34->fn->dev,
803                                 "%s: Read block failed (%d blks remaining)\n",
804                                 __func__, remaining);
805                         return ret;
806                 }
807
808                 index += (transfer * f34->v7.block_size);
809                 remaining -= transfer;
810         } while (remaining);
811
812         return 0;
813 }
814
815 static int rmi_f34v7_write_f34v7_blocks(struct f34_data *f34,
816                                         const void *block_ptr, u16 block_cnt,
817                                         u8 command)
818 {
819         int ret;
820         u8 base;
821         __le16 length;
822         u16 transfer;
823         u16 max_transfer;
824         u16 remaining = block_cnt;
825         u16 block_number = 0;
826
827         base = f34->fn->fd.data_base_addr;
828
829         ret = rmi_f34v7_write_partition_id(f34, command);
830         if (ret < 0)
831                 return ret;
832
833         ret = rmi_write_block(f34->fn->rmi_dev,
834                         base + f34->v7.off.block_number,
835                         &block_number, sizeof(block_number));
836         if (ret < 0) {
837                 dev_err(&f34->fn->dev, "%s: Failed to write block number\n",
838                         __func__);
839                 return ret;
840         }
841
842         if (f34->v7.payload_length > (PAGE_SIZE / f34->v7.block_size))
843                 max_transfer = PAGE_SIZE / f34->v7.block_size;
844         else
845                 max_transfer = f34->v7.payload_length;
846
847         do {
848                 transfer = min(remaining, max_transfer);
849                 put_unaligned_le16(transfer, &length);
850
851                 init_completion(&f34->v7.cmd_done);
852
853                 ret = rmi_write_block(f34->fn->rmi_dev,
854                                 base + f34->v7.off.transfer_length,
855                                 &length, sizeof(length));
856                 if (ret < 0) {
857                         dev_err(&f34->fn->dev,
858                                 "%s: Write transfer length fail (%d remaining)\n",
859                                 __func__, remaining);
860                         return ret;
861                 }
862
863                 ret = rmi_f34v7_write_command(f34, command);
864                 if (ret < 0)
865                         return ret;
866
867                 ret = rmi_write_block(f34->fn->rmi_dev,
868                                 base + f34->v7.off.payload,
869                                 block_ptr, transfer * f34->v7.block_size);
870                 if (ret < 0) {
871                         dev_err(&f34->fn->dev,
872                                 "%s: Failed writing data (%d blks remaining)\n",
873                                 __func__, remaining);
874                         return ret;
875                 }
876
877                 ret = rmi_f34v7_wait_for_idle(f34, F34_ENABLE_WAIT_MS);
878                 if (ret < 0)
879                         return ret;
880
881                 block_ptr += (transfer * f34->v7.block_size);
882                 remaining -= transfer;
883                 f34->update_progress += transfer;
884                 f34->update_status = (f34->update_progress * 100) /
885                                      f34->update_size;
886         } while (remaining);
887
888         return 0;
889 }
890
891 static int rmi_f34v7_write_config(struct f34_data *f34)
892 {
893         return rmi_f34v7_write_f34v7_blocks(f34, f34->v7.config_data,
894                                             f34->v7.config_block_count,
895                                             v7_CMD_WRITE_CONFIG);
896 }
897
898 static int rmi_f34v7_write_ui_config(struct f34_data *f34)
899 {
900         f34->v7.config_area = v7_UI_CONFIG_AREA;
901         f34->v7.config_data = f34->v7.img.ui_config.data;
902         f34->v7.config_size = f34->v7.img.ui_config.size;
903         f34->v7.config_block_count = f34->v7.config_size / f34->v7.block_size;
904
905         return rmi_f34v7_write_config(f34);
906 }
907
908 static int rmi_f34v7_write_dp_config(struct f34_data *f34)
909 {
910         f34->v7.config_area = v7_DP_CONFIG_AREA;
911         f34->v7.config_data = f34->v7.img.dp_config.data;
912         f34->v7.config_size = f34->v7.img.dp_config.size;
913         f34->v7.config_block_count = f34->v7.config_size / f34->v7.block_size;
914
915         return rmi_f34v7_write_config(f34);
916 }
917
918 static int rmi_f34v7_write_guest_code(struct f34_data *f34)
919 {
920         return rmi_f34v7_write_f34v7_blocks(f34, f34->v7.img.guest_code.data,
921                                             f34->v7.img.guest_code.size /
922                                                         f34->v7.block_size,
923                                             v7_CMD_WRITE_GUEST_CODE);
924 }
925
926 static int rmi_f34v7_write_flash_config(struct f34_data *f34)
927 {
928         int ret;
929
930         f34->v7.config_area = v7_FLASH_CONFIG_AREA;
931         f34->v7.config_data = f34->v7.img.fl_config.data;
932         f34->v7.config_size = f34->v7.img.fl_config.size;
933         f34->v7.config_block_count = f34->v7.config_size / f34->v7.block_size;
934
935         if (f34->v7.config_block_count != f34->v7.blkcount.fl_config) {
936                 dev_err(&f34->fn->dev, "%s: Flash config size mismatch\n",
937                         __func__);
938                 return -EINVAL;
939         }
940
941         init_completion(&f34->v7.cmd_done);
942
943         ret = rmi_f34v7_write_command(f34, v7_CMD_ERASE_FLASH_CONFIG);
944         if (ret < 0)
945                 return ret;
946
947         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
948                 "%s: Erase flash config command written\n", __func__);
949
950         ret = rmi_f34v7_wait_for_idle(f34, F34_WRITE_WAIT_MS);
951         if (ret < 0)
952                 return ret;
953
954         ret = rmi_f34v7_write_config(f34);
955         if (ret < 0)
956                 return ret;
957
958         return 0;
959 }
960
961 static int rmi_f34v7_write_partition_table(struct f34_data *f34)
962 {
963         u16 block_count;
964         int ret;
965
966         block_count = f34->v7.blkcount.bl_config;
967         f34->v7.config_area = v7_BL_CONFIG_AREA;
968         f34->v7.config_size = f34->v7.block_size * block_count;
969         devm_kfree(&f34->fn->dev, f34->v7.read_config_buf);
970         f34->v7.read_config_buf = devm_kzalloc(&f34->fn->dev,
971                                                f34->v7.config_size, GFP_KERNEL);
972         if (!f34->v7.read_config_buf) {
973                 f34->v7.read_config_buf_size = 0;
974                 return -ENOMEM;
975         }
976
977         f34->v7.read_config_buf_size = f34->v7.config_size;
978
979         ret = rmi_f34v7_read_blocks(f34, block_count, v7_CMD_READ_CONFIG);
980         if (ret < 0)
981                 return ret;
982
983         ret = rmi_f34v7_erase_config(f34);
984         if (ret < 0)
985                 return ret;
986
987         ret = rmi_f34v7_write_flash_config(f34);
988         if (ret < 0)
989                 return ret;
990
991         f34->v7.config_area = v7_BL_CONFIG_AREA;
992         f34->v7.config_data = f34->v7.read_config_buf;
993         f34->v7.config_size = f34->v7.img.bl_config.size;
994         f34->v7.config_block_count = f34->v7.config_size / f34->v7.block_size;
995
996         ret = rmi_f34v7_write_config(f34);
997         if (ret < 0)
998                 return ret;
999
1000         return 0;
1001 }
1002
1003 static int rmi_f34v7_write_firmware(struct f34_data *f34)
1004 {
1005         u16 blk_count;
1006
1007         blk_count = f34->v7.img.ui_firmware.size / f34->v7.block_size;
1008
1009         return rmi_f34v7_write_f34v7_blocks(f34, f34->v7.img.ui_firmware.data,
1010                                             blk_count, v7_CMD_WRITE_FW);
1011 }
1012
1013 static void rmi_f34v7_compare_partition_tables(struct f34_data *f34)
1014 {
1015         if (f34->v7.phyaddr.ui_firmware != f34->v7.img.phyaddr.ui_firmware) {
1016                 f34->v7.new_partition_table = true;
1017                 return;
1018         }
1019
1020         if (f34->v7.phyaddr.ui_config != f34->v7.img.phyaddr.ui_config) {
1021                 f34->v7.new_partition_table = true;
1022                 return;
1023         }
1024
1025         if (f34->v7.has_display_cfg &&
1026             f34->v7.phyaddr.dp_config != f34->v7.img.phyaddr.dp_config) {
1027                 f34->v7.new_partition_table = true;
1028                 return;
1029         }
1030
1031         if (f34->v7.has_guest_code &&
1032             f34->v7.phyaddr.guest_code != f34->v7.img.phyaddr.guest_code) {
1033                 f34->v7.new_partition_table = true;
1034                 return;
1035         }
1036
1037         f34->v7.new_partition_table = false;
1038 }
1039
1040 static void rmi_f34v7_parse_img_header_10_bl_container(struct f34_data *f34,
1041                                                        const void *image)
1042 {
1043         int i;
1044         int num_of_containers;
1045         unsigned int addr;
1046         unsigned int container_id;
1047         unsigned int length;
1048         const void *content;
1049         const struct container_descriptor *descriptor;
1050
1051         num_of_containers = f34->v7.img.bootloader.size / 4 - 1;
1052
1053         for (i = 1; i <= num_of_containers; i++) {
1054                 addr = get_unaligned_le32(f34->v7.img.bootloader.data + i * 4);
1055                 descriptor = image + addr;
1056                 container_id = le16_to_cpu(descriptor->container_id);
1057                 content = image + le32_to_cpu(descriptor->content_address);
1058                 length = le32_to_cpu(descriptor->content_length);
1059                 switch (container_id) {
1060                 case BL_CONFIG_CONTAINER:
1061                 case GLOBAL_PARAMETERS_CONTAINER:
1062                         f34->v7.img.bl_config.data = content;
1063                         f34->v7.img.bl_config.size = length;
1064                         break;
1065                 case BL_LOCKDOWN_INFO_CONTAINER:
1066                 case DEVICE_CONFIG_CONTAINER:
1067                         f34->v7.img.lockdown.data = content;
1068                         f34->v7.img.lockdown.size = length;
1069                         break;
1070                 default:
1071                         break;
1072                 }
1073         }
1074 }
1075
1076 static void rmi_f34v7_parse_image_header_10(struct f34_data *f34)
1077 {
1078         unsigned int i;
1079         unsigned int num_of_containers;
1080         unsigned int addr;
1081         unsigned int offset;
1082         unsigned int container_id;
1083         unsigned int length;
1084         const void *image = f34->v7.image;
1085         const u8 *content;
1086         const struct container_descriptor *descriptor;
1087         const struct image_header_10 *header = image;
1088
1089         f34->v7.img.checksum = le32_to_cpu(header->checksum);
1090
1091         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev, "%s: f34->v7.img.checksum=%X\n",
1092                 __func__, f34->v7.img.checksum);
1093
1094         /* address of top level container */
1095         offset = le32_to_cpu(header->top_level_container_start_addr);
1096         descriptor = image + offset;
1097
1098         /* address of top level container content */
1099         offset = le32_to_cpu(descriptor->content_address);
1100         num_of_containers = le32_to_cpu(descriptor->content_length) / 4;
1101
1102         for (i = 0; i < num_of_containers; i++) {
1103                 addr = get_unaligned_le32(image + offset);
1104                 offset += 4;
1105                 descriptor = image + addr;
1106                 container_id = le16_to_cpu(descriptor->container_id);
1107                 content = image + le32_to_cpu(descriptor->content_address);
1108                 length = le32_to_cpu(descriptor->content_length);
1109
1110                 rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
1111                         "%s: container_id=%d, length=%d\n", __func__,
1112                         container_id, length);
1113
1114                 switch (container_id) {
1115                 case UI_CONTAINER:
1116                 case CORE_CODE_CONTAINER:
1117                         f34->v7.img.ui_firmware.data = content;
1118                         f34->v7.img.ui_firmware.size = length;
1119                         break;
1120                 case UI_CONFIG_CONTAINER:
1121                 case CORE_CONFIG_CONTAINER:
1122                         f34->v7.img.ui_config.data = content;
1123                         f34->v7.img.ui_config.size = length;
1124                         break;
1125                 case BL_CONTAINER:
1126                         f34->v7.img.bl_version = *content;
1127                         f34->v7.img.bootloader.data = content;
1128                         f34->v7.img.bootloader.size = length;
1129                         rmi_f34v7_parse_img_header_10_bl_container(f34, image);
1130                         break;
1131                 case GUEST_CODE_CONTAINER:
1132                         f34->v7.img.contains_guest_code = true;
1133                         f34->v7.img.guest_code.data = content;
1134                         f34->v7.img.guest_code.size = length;
1135                         break;
1136                 case DISPLAY_CONFIG_CONTAINER:
1137                         f34->v7.img.contains_display_cfg = true;
1138                         f34->v7.img.dp_config.data = content;
1139                         f34->v7.img.dp_config.size = length;
1140                         break;
1141                 case FLASH_CONFIG_CONTAINER:
1142                         f34->v7.img.contains_flash_config = true;
1143                         f34->v7.img.fl_config.data = content;
1144                         f34->v7.img.fl_config.size = length;
1145                         break;
1146                 case GENERAL_INFORMATION_CONTAINER:
1147                         f34->v7.img.contains_firmware_id = true;
1148                         f34->v7.img.firmware_id =
1149                                 get_unaligned_le32(content + 4);
1150                         break;
1151                 default:
1152                         break;
1153                 }
1154         }
1155 }
1156
1157 static int rmi_f34v7_parse_image_info(struct f34_data *f34)
1158 {
1159         const struct image_header_10 *header = f34->v7.image;
1160
1161         memset(&f34->v7.img, 0x00, sizeof(f34->v7.img));
1162
1163         rmi_dbg(RMI_DEBUG_FN, &f34->fn->dev,
1164                 "%s: header->major_header_version = %d\n",
1165                 __func__, header->major_header_version);
1166
1167         switch (header->major_header_version) {
1168         case IMAGE_HEADER_VERSION_10:
1169                 rmi_f34v7_parse_image_header_10(f34);
1170                 break;
1171         default:
1172                 dev_err(&f34->fn->dev, "Unsupported image file format %02X\n",
1173                         header->major_header_version);
1174                 return -EINVAL;
1175         }
1176
1177         if (!f34->v7.img.contains_flash_config) {
1178                 dev_err(&f34->fn->dev, "%s: No flash config in fw image\n",
1179                         __func__);
1180                 return -EINVAL;
1181         }
1182
1183         rmi_f34v7_parse_partition_table(f34, f34->v7.img.fl_config.data,
1184                         &f34->v7.img.blkcount, &f34->v7.img.phyaddr);
1185
1186         rmi_f34v7_compare_partition_tables(f34);
1187
1188         return 0;
1189 }
1190
1191 int rmi_f34v7_do_reflash(struct f34_data *f34, const struct firmware *fw)
1192 {
1193         int ret;
1194
1195         rmi_f34v7_read_queries_bl_version(f34);
1196
1197         f34->v7.image = fw->data;
1198         f34->update_progress = 0;
1199         f34->update_size = 0;
1200
1201         ret = rmi_f34v7_parse_image_info(f34);
1202         if (ret < 0)
1203                 goto fail;
1204
1205         if (!f34->v7.new_partition_table) {
1206                 ret = rmi_f34v7_check_ui_firmware_size(f34);
1207                 if (ret < 0)
1208                         goto fail;
1209
1210                 ret = rmi_f34v7_check_ui_config_size(f34);
1211                 if (ret < 0)
1212                         goto fail;
1213
1214                 if (f34->v7.has_display_cfg &&
1215                     f34->v7.img.contains_display_cfg) {
1216                         ret = rmi_f34v7_check_dp_config_size(f34);
1217                         if (ret < 0)
1218                                 goto fail;
1219                 }
1220
1221                 if (f34->v7.has_guest_code && f34->v7.img.contains_guest_code) {
1222                         ret = rmi_f34v7_check_guest_code_size(f34);
1223                         if (ret < 0)
1224                                 goto fail;
1225                 }
1226         } else {
1227                 ret = rmi_f34v7_check_bl_config_size(f34);
1228                 if (ret < 0)
1229                         goto fail;
1230         }
1231
1232         ret = rmi_f34v7_erase_all(f34);
1233         if (ret < 0)
1234                 goto fail;
1235
1236         if (f34->v7.new_partition_table) {
1237                 ret = rmi_f34v7_write_partition_table(f34);
1238                 if (ret < 0)
1239                         goto fail;
1240                 dev_info(&f34->fn->dev, "%s: Partition table programmed\n",
1241                          __func__);
1242         }
1243
1244         dev_info(&f34->fn->dev, "Writing firmware (%d bytes)...\n",
1245                  f34->v7.img.ui_firmware.size);
1246
1247         ret = rmi_f34v7_write_firmware(f34);
1248         if (ret < 0)
1249                 goto fail;
1250
1251         dev_info(&f34->fn->dev, "Writing config (%d bytes)...\n",
1252                  f34->v7.img.ui_config.size);
1253
1254         f34->v7.config_area = v7_UI_CONFIG_AREA;
1255         ret = rmi_f34v7_write_ui_config(f34);
1256         if (ret < 0)
1257                 goto fail;
1258
1259         if (f34->v7.has_display_cfg && f34->v7.img.contains_display_cfg) {
1260                 dev_info(&f34->fn->dev, "Writing display config...\n");
1261
1262                 ret = rmi_f34v7_write_dp_config(f34);
1263                 if (ret < 0)
1264                         goto fail;
1265         }
1266
1267         if (f34->v7.new_partition_table) {
1268                 if (f34->v7.has_guest_code && f34->v7.img.contains_guest_code) {
1269                         dev_info(&f34->fn->dev, "Writing guest code...\n");
1270
1271                         ret = rmi_f34v7_write_guest_code(f34);
1272                         if (ret < 0)
1273                                 goto fail;
1274                 }
1275         }
1276
1277 fail:
1278         return ret;
1279 }
1280
1281 static int rmi_f34v7_enter_flash_prog(struct f34_data *f34)
1282 {
1283         int ret;
1284
1285         f34->fn->rmi_dev->driver->set_irq_bits(f34->fn->rmi_dev, f34->fn->irq_mask);
1286
1287         ret = rmi_f34v7_read_flash_status(f34);
1288         if (ret < 0)
1289                 return ret;
1290
1291         if (f34->v7.in_bl_mode)
1292                 return 0;
1293
1294         init_completion(&f34->v7.cmd_done);
1295
1296         ret = rmi_f34v7_write_command(f34, v7_CMD_ENABLE_FLASH_PROG);
1297         if (ret < 0)
1298                 return ret;
1299
1300         ret = rmi_f34v7_wait_for_idle(f34, F34_ENABLE_WAIT_MS);
1301         if (ret < 0)
1302                 return ret;
1303
1304         return 0;
1305 }
1306
1307 int rmi_f34v7_start_reflash(struct f34_data *f34, const struct firmware *fw)
1308 {
1309         int ret = 0;
1310
1311         f34->fn->rmi_dev->driver->set_irq_bits(f34->fn->rmi_dev, f34->fn->irq_mask);
1312
1313         f34->v7.config_area = v7_UI_CONFIG_AREA;
1314         f34->v7.image = fw->data;
1315
1316         ret = rmi_f34v7_parse_image_info(f34);
1317         if (ret < 0)
1318                 goto exit;
1319
1320         if (!f34->v7.force_update && f34->v7.new_partition_table) {
1321                 dev_err(&f34->fn->dev, "%s: Partition table mismatch\n",
1322                                 __func__);
1323                 ret = -EINVAL;
1324                 goto exit;
1325         }
1326
1327         dev_info(&f34->fn->dev, "Firmware image OK\n");
1328
1329         ret = rmi_f34v7_read_flash_status(f34);
1330         if (ret < 0)
1331                 goto exit;
1332
1333         if (f34->v7.in_bl_mode) {
1334                 dev_info(&f34->fn->dev, "%s: Device in bootloader mode\n",
1335                                 __func__);
1336         }
1337
1338         rmi_f34v7_enter_flash_prog(f34);
1339
1340         return 0;
1341
1342 exit:
1343         return ret;
1344 }
1345
1346 int rmi_f34v7_probe(struct f34_data *f34)
1347 {
1348         int ret;
1349
1350         /* Read bootloader version */
1351         ret = rmi_read_block(f34->fn->rmi_dev,
1352                         f34->fn->fd.query_base_addr + V7_BOOTLOADER_ID_OFFSET,
1353                         f34->bootloader_id,
1354                         sizeof(f34->bootloader_id));
1355         if (ret < 0) {
1356                 dev_err(&f34->fn->dev, "%s: Failed to read bootloader ID\n",
1357                         __func__);
1358                 return ret;
1359         }
1360
1361         if (f34->bootloader_id[1] == '5') {
1362                 f34->bl_version = 5;
1363         } else if (f34->bootloader_id[1] == '6') {
1364                 f34->bl_version = 6;
1365         } else if (f34->bootloader_id[1] == 7) {
1366                 f34->bl_version = 7;
1367         } else {
1368                 dev_err(&f34->fn->dev, "%s: Unrecognized bootloader version\n",
1369                                 __func__);
1370                 return -EINVAL;
1371         }
1372
1373         memset(&f34->v7.blkcount, 0x00, sizeof(f34->v7.blkcount));
1374         memset(&f34->v7.phyaddr, 0x00, sizeof(f34->v7.phyaddr));
1375
1376         init_completion(&f34->v7.cmd_done);
1377
1378         ret = rmi_f34v7_read_queries(f34);
1379         if (ret < 0)
1380                 return ret;
1381
1382         f34->v7.force_update = true;
1383         return 0;
1384 }