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 }