1 /* 2 * Copyright 2017 Rockchip Electronics Co., Ltd 3 * Frank Wang <frank.wang@rock-chips.com> 4 * 5 * SPDX-License-Identifier: GPL-2.0+ 6 */ 7 8 #include <asm/io.h> 9 #include <android_avb/avb_ops_user.h> 10 #include <android_avb/rk_avb_ops_user.h> 11 #include <asm/arch/boot_mode.h> 12 #include <asm/arch/chip_info.h> 13 #include <asm/arch/rk_atags.h> 14 #include <write_keybox.h> 15 #include <linux/mtd/mtd.h> 16 #include <optee_include/OpteeClientInterface.h> 17 #include <mmc.h> 18 #include <stdlib.h> 19 20 #ifdef CONFIG_ROCKCHIP_VENDOR_PARTITION 21 #include <asm/arch/vendor.h> 22 #endif 23 #include <rockusb.h> 24 25 #define ROCKUSB_INTERFACE_CLASS 0xff 26 #define ROCKUSB_INTERFACE_SUB_CLASS 0x06 27 #define ROCKUSB_INTERFACE_PROTOCOL 0x05 28 29 #define ROCKCHIP_FLASH_BLOCK_SIZE 1024 30 #define ROCKCHIP_FLASH_PAGE_SIZE 4 31 32 static struct usb_interface_descriptor rkusb_intf_desc = { 33 .bLength = USB_DT_INTERFACE_SIZE, 34 .bDescriptorType = USB_DT_INTERFACE, 35 .bInterfaceNumber = 0x00, 36 .bAlternateSetting = 0x00, 37 .bNumEndpoints = 0x02, 38 .bInterfaceClass = ROCKUSB_INTERFACE_CLASS, 39 .bInterfaceSubClass = ROCKUSB_INTERFACE_SUB_CLASS, 40 .bInterfaceProtocol = ROCKUSB_INTERFACE_PROTOCOL, 41 }; 42 43 static struct usb_descriptor_header *rkusb_fs_function[] = { 44 (struct usb_descriptor_header *)&rkusb_intf_desc, 45 (struct usb_descriptor_header *)&fsg_fs_bulk_in_desc, 46 (struct usb_descriptor_header *)&fsg_fs_bulk_out_desc, 47 NULL, 48 }; 49 50 static struct usb_descriptor_header *rkusb_hs_function[] = { 51 (struct usb_descriptor_header *)&rkusb_intf_desc, 52 (struct usb_descriptor_header *)&fsg_hs_bulk_in_desc, 53 (struct usb_descriptor_header *)&fsg_hs_bulk_out_desc, 54 NULL, 55 }; 56 57 static struct usb_descriptor_header *rkusb_ss_function[] = { 58 (struct usb_descriptor_header *)&rkusb_intf_desc, 59 (struct usb_descriptor_header *)&fsg_ss_bulk_in_desc, 60 (struct usb_descriptor_header *)&fsg_ss_bulk_in_comp_desc, 61 (struct usb_descriptor_header *)&fsg_ss_bulk_out_desc, 62 (struct usb_descriptor_header *)&fsg_ss_bulk_out_comp_desc, 63 NULL, 64 }; 65 66 struct rk_flash_info { 67 u32 flash_size; 68 u16 block_size; 69 u8 page_size; 70 u8 ecc_bits; 71 u8 access_time; 72 u8 manufacturer; 73 u8 flash_mask; 74 } __packed; 75 76 static int rkusb_rst_code; /* The subcode in reset command (0xFF) */ 77 78 int g_dnl_bind_fixup(struct usb_device_descriptor *dev, const char *name) 79 { 80 if (IS_RKUSB_UMS_DNL(name)) { 81 /* Fix to Rockchip's VID and PID */ 82 dev->idVendor = __constant_cpu_to_le16(0x2207); 83 dev->idProduct = __constant_cpu_to_le16(CONFIG_ROCKUSB_G_DNL_PID); 84 85 /* Enumerate as a loader device */ 86 #if defined(CONFIG_SUPPORT_USBPLUG) 87 dev->bcdUSB = cpu_to_le16(0x0200); 88 #else 89 dev->bcdUSB = cpu_to_le16(0x0201); 90 #endif 91 } else if (!strncmp(name, "usb_dnl_fastboot", 16)) { 92 /* Fix to Google's VID and PID */ 93 dev->idVendor = __constant_cpu_to_le16(0x18d1); 94 dev->idProduct = __constant_cpu_to_le16(0xd00d); 95 } else if (!strncmp(name, "usb_dnl_dfu", 11)) { 96 /* Fix to Rockchip's VID and PID for DFU */ 97 dev->idVendor = cpu_to_le16(0x2207); 98 dev->idProduct = cpu_to_le16(0x0107); 99 } else if (!strncmp(name, "usb_dnl_ums", 11)) { 100 dev->idVendor = cpu_to_le16(0x2207); 101 dev->idProduct = cpu_to_le16(0x0010); 102 } 103 104 return 0; 105 } 106 107 __maybe_unused 108 static inline void dump_cbw(struct fsg_bulk_cb_wrap *cbw) 109 { 110 assert(!cbw); 111 112 debug("%s:\n", __func__); 113 debug("Signature %x\n", cbw->Signature); 114 debug("Tag %x\n", cbw->Tag); 115 debug("DataTransferLength %x\n", cbw->DataTransferLength); 116 debug("Flags %x\n", cbw->Flags); 117 debug("LUN %x\n", cbw->Lun); 118 debug("Length %x\n", cbw->Length); 119 debug("OptionCode %x\n", cbw->CDB[0]); 120 debug("SubCode %x\n", cbw->CDB[1]); 121 debug("SectorAddr %x\n", get_unaligned_be32(&cbw->CDB[2])); 122 debug("BlkSectors %x\n\n", get_unaligned_be16(&cbw->CDB[7])); 123 } 124 125 static int rkusb_check_lun(struct fsg_common *common) 126 { 127 struct fsg_lun *curlun; 128 129 /* Check the LUN */ 130 if (common->lun >= 0 && common->lun < common->nluns) { 131 curlun = &common->luns[common->lun]; 132 if (common->cmnd[0] != SC_REQUEST_SENSE) { 133 curlun->sense_data = SS_NO_SENSE; 134 curlun->info_valid = 0; 135 } 136 } else { 137 curlun = NULL; 138 common->bad_lun_okay = 0; 139 140 /* 141 * INQUIRY and REQUEST SENSE commands are explicitly allowed 142 * to use unsupported LUNs; all others may not. 143 */ 144 if (common->cmnd[0] != SC_INQUIRY && 145 common->cmnd[0] != SC_REQUEST_SENSE) { 146 debug("unsupported LUN %d\n", common->lun); 147 return -EINVAL; 148 } 149 } 150 151 return 0; 152 } 153 154 static void __do_reset(struct usb_ep *ep, struct usb_request *req) 155 { 156 u32 boot_flag = BOOT_NORMAL; 157 158 if (rkusb_rst_code == 0x03) 159 boot_flag = BOOT_BROM_DOWNLOAD; 160 161 rkusb_rst_code = 0; /* restore to default */ 162 writel(boot_flag, (void *)CONFIG_ROCKCHIP_BOOT_MODE_REG); 163 164 do_reset(NULL, 0, 0, NULL); 165 } 166 167 static int rkusb_do_reset(struct fsg_common *common, 168 struct fsg_buffhd *bh) 169 { 170 common->data_size_from_cmnd = common->cmnd[4]; 171 common->residue = 0; 172 bh->inreq->complete = __do_reset; 173 bh->state = BUF_STATE_EMPTY; 174 175 rkusb_rst_code = !common->cmnd[1] ? 0xff : common->cmnd[1]; 176 return 0; 177 } 178 179 __weak bool rkusb_usb3_capable(void) 180 { 181 return false; 182 } 183 184 static int rkusb_do_switch_to_usb3(struct fsg_common *common, 185 struct fsg_buffhd *bh) 186 { 187 g_dnl_set_serialnumber((char *)&common->cmnd[1]); 188 rkusb_switch_to_usb3_enable(true); 189 bh->state = BUF_STATE_EMPTY; 190 191 return 0; 192 } 193 194 static int rkusb_do_test_unit_ready(struct fsg_common *common, 195 struct fsg_buffhd *bh) 196 { 197 struct blk_desc *desc = &ums[common->lun].block_dev; 198 199 if ((desc->if_type == IF_TYPE_MTD && desc->devnum == BLK_MTD_SPI_NOR) || 200 desc->if_type == IF_TYPE_SPINOR) 201 common->residue = 0x03 << 24; /* 128KB Max block xfer for SPI Nor */ 202 else 203 common->residue = 0x06 << 24; /* Max block xfer support from host */ 204 205 common->data_dir = DATA_DIR_NONE; 206 bh->state = BUF_STATE_EMPTY; 207 208 return 0; 209 } 210 211 static int rkusb_do_read_flash_id(struct fsg_common *common, 212 struct fsg_buffhd *bh) 213 { 214 u8 *buf = (u8 *)bh->buf; 215 u32 len = 5; 216 enum if_type type = ums[common->lun].block_dev.if_type; 217 u32 devnum = ums[common->lun].block_dev.devnum; 218 const char *str; 219 220 switch (type) { 221 case IF_TYPE_MMC: 222 str = "EMMC "; 223 break; 224 case IF_TYPE_RKNAND: 225 str = "NAND "; 226 break; 227 case IF_TYPE_MTD: 228 if (devnum == BLK_MTD_SPI_NAND) 229 str ="SNAND"; 230 else if (devnum == BLK_MTD_NAND) 231 str = "NAND "; 232 else 233 str = "NOR "; 234 break; 235 default: 236 str = "UNKN "; /* unknown */ 237 break; 238 } 239 240 memcpy((void *)&buf[0], str, len); 241 242 /* Set data xfer size */ 243 common->residue = common->data_size_from_cmnd = len; 244 common->data_size = len; 245 246 return len; 247 } 248 249 static int rkusb_do_test_bad_block(struct fsg_common *common, 250 struct fsg_buffhd *bh) 251 { 252 u8 *buf = (u8 *)bh->buf; 253 u32 len = 64; 254 255 memset((void *)&buf[0], 0, len); 256 257 /* Set data xfer size */ 258 common->residue = common->data_size_from_cmnd = len; 259 common->data_size = len; 260 261 return len; 262 } 263 264 static int rkusb_do_read_flash_info(struct fsg_common *common, 265 struct fsg_buffhd *bh) 266 { 267 struct blk_desc *desc = &ums[common->lun].block_dev; 268 u8 *buf = (u8 *)bh->buf; 269 u32 len = sizeof(struct rk_flash_info); 270 struct rk_flash_info finfo = { 271 .block_size = ROCKCHIP_FLASH_BLOCK_SIZE, 272 .ecc_bits = 0, 273 .page_size = ROCKCHIP_FLASH_PAGE_SIZE, 274 .access_time = 40, 275 .manufacturer = 0, 276 .flash_mask = 0 277 }; 278 279 finfo.flash_size = (u32)desc->lba; 280 281 if (desc->if_type == IF_TYPE_MTD && 282 (desc->devnum == BLK_MTD_NAND || 283 desc->devnum == BLK_MTD_SPI_NAND)) { 284 struct mtd_info *mtd = (struct mtd_info *)desc->bdev->priv; 285 286 if (mtd) { 287 finfo.block_size = mtd->erasesize >> 9; 288 finfo.page_size = mtd->writesize >> 9; 289 } 290 } 291 292 if (desc->if_type == IF_TYPE_MTD && desc->devnum == BLK_MTD_SPI_NOR) { 293 /* RV1126/RK3308 mtd spinor keep the former upgrade mode */ 294 #if !defined(CONFIG_ROCKCHIP_RV1126) && !defined(CONFIG_ROCKCHIP_RK3308) 295 finfo.block_size = 0x80; /* Aligned to 64KB */ 296 #else 297 finfo.block_size = ROCKCHIP_FLASH_BLOCK_SIZE; 298 #endif 299 } 300 301 debug("Flash info: block_size= %x page_size= %x\n", finfo.block_size, 302 finfo.page_size); 303 304 if (finfo.flash_size) 305 finfo.flash_mask = 1; 306 307 memset((void *)&buf[0], 0, len); 308 memcpy((void *)&buf[0], (void *)&finfo, len); 309 310 /* Set data xfer size */ 311 common->residue = common->data_size_from_cmnd = len; 312 /* legacy upgrade_tool does not set correct transfer size */ 313 common->data_size = len; 314 315 return len; 316 } 317 318 static int rkusb_do_get_chip_info(struct fsg_common *common, 319 struct fsg_buffhd *bh) 320 { 321 u8 *buf = (u8 *)bh->buf; 322 u32 len = common->data_size; 323 u32 chip_info[4]; 324 325 memset((void *)chip_info, 0, sizeof(chip_info)); 326 rockchip_rockusb_get_chip_info(chip_info); 327 328 memset((void *)&buf[0], 0, len); 329 memcpy((void *)&buf[0], (void *)chip_info, len); 330 331 /* Set data xfer size */ 332 common->residue = common->data_size_from_cmnd = len; 333 334 return len; 335 } 336 337 static int rkusb_do_lba_erase(struct fsg_common *common, 338 struct fsg_buffhd *bh) 339 { 340 struct fsg_lun *curlun = &common->luns[common->lun]; 341 u32 lba, amount; 342 loff_t file_offset; 343 int rc; 344 345 lba = get_unaligned_be32(&common->cmnd[2]); 346 if (lba >= curlun->num_sectors) { 347 curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE; 348 rc = -EINVAL; 349 goto out; 350 } 351 352 file_offset = ((loff_t) lba) << 9; 353 amount = get_unaligned_be16(&common->cmnd[7]) << 9; 354 if (unlikely(amount == 0)) { 355 curlun->sense_data = SS_INVALID_FIELD_IN_CDB; 356 rc = -EIO; 357 goto out; 358 } 359 360 /* Perform the erase */ 361 rc = ums[common->lun].erase_sector(&ums[common->lun], 362 file_offset / SECTOR_SIZE, 363 amount / SECTOR_SIZE); 364 if (!rc) { 365 curlun->sense_data = SS_MEDIUM_NOT_PRESENT; 366 rc = -EIO; 367 } 368 369 out: 370 common->data_dir = DATA_DIR_NONE; 371 bh->state = BUF_STATE_EMPTY; 372 373 return rc; 374 } 375 376 static int rkusb_do_erase_force(struct fsg_common *common, 377 struct fsg_buffhd *bh) 378 { 379 struct blk_desc *desc = &ums[common->lun].block_dev; 380 struct fsg_lun *curlun = &common->luns[common->lun]; 381 u16 block_size = ROCKCHIP_FLASH_BLOCK_SIZE; 382 u32 lba, amount; 383 loff_t file_offset; 384 int rc; 385 386 lba = get_unaligned_be32(&common->cmnd[2]); 387 if (lba >= curlun->num_sectors) { 388 curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE; 389 rc = -EINVAL; 390 goto out; 391 } 392 393 if (desc->if_type == IF_TYPE_MTD && 394 (desc->devnum == BLK_MTD_NAND || 395 desc->devnum == BLK_MTD_SPI_NAND)) { 396 struct mtd_info *mtd = (struct mtd_info *)desc->bdev->priv; 397 398 if (mtd) 399 block_size = mtd->erasesize >> 9; 400 } 401 402 file_offset = ((loff_t)lba) * block_size; 403 amount = get_unaligned_be16(&common->cmnd[7]) * block_size; 404 405 debug("%s lba= %x, nsec= %x\n", __func__, lba, 406 (u32)get_unaligned_be16(&common->cmnd[7])); 407 408 if (unlikely(amount == 0)) { 409 curlun->sense_data = SS_INVALID_FIELD_IN_CDB; 410 rc = -EIO; 411 goto out; 412 } 413 414 /* Perform the erase */ 415 rc = ums[common->lun].erase_sector(&ums[common->lun], 416 file_offset, 417 amount); 418 if (!rc) { 419 curlun->sense_data = SS_MEDIUM_NOT_PRESENT; 420 rc = -EIO; 421 } 422 423 out: 424 common->data_dir = DATA_DIR_NONE; 425 bh->state = BUF_STATE_EMPTY; 426 427 return rc; 428 } 429 430 #ifdef CONFIG_ROCKCHIP_VENDOR_PARTITION 431 static int rkusb_do_vs_write(struct fsg_common *common) 432 { 433 struct fsg_lun *curlun = &common->luns[common->lun]; 434 u16 type = get_unaligned_be16(&common->cmnd[4]); 435 struct vendor_item *vhead; 436 struct fsg_buffhd *bh; 437 void *data; 438 int rc; 439 440 if (common->data_size >= (u32)65536) { 441 /* _MUST_ small than 64K */ 442 curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE; 443 return -EINVAL; 444 } 445 446 common->residue = common->data_size; 447 common->usb_amount_left = common->data_size; 448 449 /* Carry out the file writes */ 450 if (unlikely(common->data_size == 0)) 451 return -EIO; /* No data to write */ 452 453 for (;;) { 454 if (common->usb_amount_left > 0) { 455 /* Wait for the next buffer to become available */ 456 bh = common->next_buffhd_to_fill; 457 if (bh->state != BUF_STATE_EMPTY) 458 goto wait; 459 460 /* Request the next buffer */ 461 common->usb_amount_left -= common->data_size; 462 bh->outreq->length = common->data_size; 463 bh->bulk_out_intended_length = common->data_size; 464 bh->outreq->short_not_ok = 1; 465 466 START_TRANSFER_OR(common, bulk_out, bh->outreq, 467 &bh->outreq_busy, &bh->state) 468 /* 469 * Don't know what to do if 470 * common->fsg is NULL 471 */ 472 return -EIO; 473 common->next_buffhd_to_fill = bh->next; 474 } else { 475 /* Then, wait for the data to become available */ 476 bh = common->next_buffhd_to_drain; 477 if (bh->state != BUF_STATE_FULL) 478 goto wait; 479 480 common->next_buffhd_to_drain = bh->next; 481 bh->state = BUF_STATE_EMPTY; 482 483 /* Did something go wrong with the transfer? */ 484 if (bh->outreq->status != 0) { 485 curlun->sense_data = SS_COMMUNICATION_FAILURE; 486 curlun->info_valid = 1; 487 break; 488 } 489 490 /* Perform the write */ 491 vhead = (struct vendor_item *)bh->buf; 492 data = bh->buf + sizeof(struct vendor_item); 493 494 if (!type) { 495 if (vhead->id == HDCP_14_HDMI_ID || 496 vhead->id == HDCP_14_HDMIRX_ID || 497 vhead->id == HDCP_14_DP_ID) { 498 rc = vendor_handle_hdcp(vhead); 499 if (rc < 0) { 500 curlun->sense_data = SS_WRITE_ERROR; 501 return -EIO; 502 } 503 } 504 505 /* Vendor storage */ 506 rc = vendor_storage_write(vhead->id, 507 (char __user *)data, 508 vhead->size); 509 if (rc < 0) { 510 curlun->sense_data = SS_WRITE_ERROR; 511 return -EIO; 512 } 513 } else if (type == 1) { 514 /* RPMB */ 515 rc = 516 write_keybox_to_secure_storage((u8 *)data, 517 vhead->size); 518 if (rc < 0) { 519 curlun->sense_data = SS_WRITE_ERROR; 520 return -EIO; 521 } 522 } else if (type == 2) { 523 /* security storage */ 524 #ifdef CONFIG_RK_AVB_LIBAVB_USER 525 debug("%s call rk_avb_write_perm_attr %d, %d\n", 526 __func__, vhead->id, vhead->size); 527 rc = rk_avb_write_perm_attr(vhead->id, 528 (char __user *)data, 529 vhead->size); 530 if (rc < 0) { 531 curlun->sense_data = SS_WRITE_ERROR; 532 return -EIO; 533 } 534 #else 535 printf("Please enable CONFIG_RK_AVB_LIBAVB_USER\n"); 536 #endif 537 } else if (type == 3) { 538 /* efuse or otp*/ 539 #ifdef CONFIG_OPTEE_CLIENT 540 if (memcmp(data, "TAEK", 4) == 0) { 541 if (vhead->size - 8 != 32) { 542 printf("check ta encryption key size fail!\n"); 543 curlun->sense_data = SS_WRITE_ERROR; 544 return -EIO; 545 } 546 if (trusty_write_ta_encryption_key((uint32_t *)(data + 8), 8) != 0) { 547 printf("trusty_write_ta_encryption_key error!"); 548 curlun->sense_data = SS_WRITE_ERROR; 549 return -EIO; 550 } 551 } else if (memcmp(data, "EHUK", 4) == 0) { 552 if (vhead->size - 8 != 32) { 553 printf("check oem huk size fail!\n"); 554 curlun->sense_data = SS_WRITE_ERROR; 555 return -EIO; 556 } 557 if (trusty_write_oem_huk((uint32_t *)(data + 8), 8) != 0) { 558 printf("trusty_write_oem_huk error!"); 559 curlun->sense_data = SS_WRITE_ERROR; 560 return -EIO; 561 } 562 } else { 563 printf("Unknown tag\n"); 564 curlun->sense_data = SS_WRITE_ERROR; 565 return -EIO; 566 } 567 #else 568 printf("Please enable CONFIG_OPTEE_CLIENT\n"); 569 #endif 570 } else { 571 return -EINVAL; 572 } 573 574 common->residue -= common->data_size; 575 576 /* Did the host decide to stop early? */ 577 if (bh->outreq->actual != bh->outreq->length) 578 common->short_packet_received = 1; 579 break; /* Command done */ 580 } 581 wait: 582 /* Wait for something to happen */ 583 rc = sleep_thread(common); 584 if (rc) 585 return rc; 586 } 587 588 return -EIO; /* No default reply */ 589 } 590 591 static int rkusb_do_vs_read(struct fsg_common *common) 592 { 593 struct fsg_lun *curlun = &common->luns[common->lun]; 594 u16 type = get_unaligned_be16(&common->cmnd[4]); 595 struct vendor_item *vhead; 596 struct fsg_buffhd *bh; 597 void *data; 598 int rc; 599 600 if (common->data_size >= (u32)65536) { 601 /* _MUST_ small than 64K */ 602 curlun->sense_data = SS_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE; 603 return -EINVAL; 604 } 605 606 common->residue = common->data_size; 607 common->usb_amount_left = common->data_size; 608 609 /* Carry out the file reads */ 610 if (unlikely(common->data_size == 0)) 611 return -EIO; /* No default reply */ 612 613 for (;;) { 614 /* Wait for the next buffer to become available */ 615 bh = common->next_buffhd_to_fill; 616 while (bh->state != BUF_STATE_EMPTY) { 617 rc = sleep_thread(common); 618 if (rc) 619 return rc; 620 } 621 622 memset(bh->buf, 0, FSG_BUFLEN); 623 vhead = (struct vendor_item *)bh->buf; 624 data = bh->buf + sizeof(struct vendor_item); 625 vhead->id = get_unaligned_be16(&common->cmnd[2]); 626 627 if (!type) { 628 /* Vendor storage */ 629 rc = vendor_storage_read(vhead->id, 630 (char __user *)data, 631 common->data_size); 632 if (!rc) { 633 curlun->sense_data = SS_UNRECOVERED_READ_ERROR; 634 return -EIO; 635 } 636 vhead->size = rc; 637 } else if (type == 1) { 638 /* RPMB */ 639 rc = 640 read_raw_data_from_secure_storage((u8 *)data, 641 common->data_size); 642 if (!rc) { 643 curlun->sense_data = SS_UNRECOVERED_READ_ERROR; 644 return -EIO; 645 } 646 vhead->size = rc; 647 } else if (type == 2) { 648 /* security storage */ 649 #ifdef CONFIG_RK_AVB_LIBAVB_USER 650 rc = rk_avb_read_perm_attr(vhead->id, 651 (char __user *)data, 652 vhead->size); 653 if (rc < 0) 654 return -EIO; 655 vhead->size = rc; 656 #else 657 printf("Please enable CONFIG_RK_AVB_LIBAVB_USER!\n"); 658 #endif 659 } else if (type == 3) { 660 /* efuse or otp*/ 661 #ifdef CONFIG_OPTEE_CLIENT 662 if (vhead->id == 120) { 663 u8 value; 664 char *written_str = "key is written!"; 665 char *not_written_str = "key is not written!"; 666 if (trusty_ta_encryption_key_is_written(&value) != 0) { 667 printf("trusty_ta_encryption_key_is_written error!"); 668 return -EIO; 669 } 670 if (value) { 671 memcpy(data, written_str, strlen(written_str)); 672 vhead->size = strlen(written_str); 673 } else { 674 memcpy(data, not_written_str, strlen(not_written_str)); 675 vhead->size = strlen(not_written_str); 676 } 677 } else { 678 printf("Unknown tag\n"); 679 return -EIO; 680 } 681 #else 682 printf("Please enable CONFIG_OPTEE_CLIENT\n"); 683 #endif 684 } else { 685 return -EINVAL; 686 } 687 688 common->residue -= common->data_size; 689 bh->inreq->length = common->data_size; 690 bh->state = BUF_STATE_FULL; 691 692 break; /* No more left to read */ 693 } 694 695 return -EIO; /* No default reply */ 696 } 697 #endif 698 699 static int rkusb_do_switch_storage(struct fsg_common *common) 700 { 701 enum if_type type, cur_type = ums[common->lun].block_dev.if_type; 702 int devnum, cur_devnum = ums[common->lun].block_dev.devnum; 703 struct blk_desc *block_dev; 704 u32 media = BOOT_TYPE_UNKNOWN; 705 706 media = 1 << common->cmnd[1]; 707 708 switch (media) { 709 #ifdef CONFIG_MMC 710 case BOOT_TYPE_EMMC: 711 type = IF_TYPE_MMC; 712 devnum = 0; 713 mmc_initialize(gd->bd); 714 break; 715 #endif 716 case BOOT_TYPE_MTD_BLK_NAND: 717 type = IF_TYPE_MTD; 718 devnum = 0; 719 break; 720 case BOOT_TYPE_MTD_BLK_SPI_NAND: 721 type = IF_TYPE_MTD; 722 devnum = 1; 723 break; 724 case BOOT_TYPE_MTD_BLK_SPI_NOR: 725 type = IF_TYPE_MTD; 726 devnum = 2; 727 break; 728 default: 729 printf("Bootdev 0x%x is not support\n", media); 730 return -ENODEV; 731 } 732 733 if (cur_type == type && cur_devnum == devnum) 734 return 0; 735 736 block_dev = blk_get_devnum_by_type(type, devnum); 737 if (!block_dev) { 738 printf("Bootdev if_type=%d num=%d toggle fail\n", type, devnum); 739 return -ENODEV; 740 } 741 742 ums[common->lun].num_sectors = block_dev->lba; 743 ums[common->lun].block_dev = *block_dev; 744 745 printf("RKUSB: LUN %d, dev %d, hwpart %d, sector %#x, count %#x\n", 746 0, 747 ums[common->lun].block_dev.devnum, 748 ums[common->lun].block_dev.hwpart, 749 ums[common->lun].start_sector, 750 ums[common->lun].num_sectors); 751 752 return 0; 753 } 754 755 static int rkusb_do_get_storage_info(struct fsg_common *common, 756 struct fsg_buffhd *bh) 757 { 758 enum if_type type = ums[common->lun].block_dev.if_type; 759 int devnum = ums[common->lun].block_dev.devnum; 760 u32 media = BOOT_TYPE_UNKNOWN; 761 u32 len = common->data_size; 762 u8 *buf = (u8 *)bh->buf; 763 764 if (len > 4) 765 len = 4; 766 767 switch (type) { 768 case IF_TYPE_MMC: 769 media = BOOT_TYPE_EMMC; 770 break; 771 772 case IF_TYPE_SD: 773 media = BOOT_TYPE_SD0; 774 break; 775 776 case IF_TYPE_MTD: 777 if (devnum == BLK_MTD_SPI_NAND) 778 media = BOOT_TYPE_MTD_BLK_SPI_NAND; 779 else if (devnum == BLK_MTD_NAND) 780 media = BOOT_TYPE_NAND; 781 else 782 media = BOOT_TYPE_MTD_BLK_SPI_NOR; 783 break; 784 785 case IF_TYPE_SCSI: 786 media = BOOT_TYPE_SATA; 787 break; 788 789 case IF_TYPE_RKNAND: 790 media = BOOT_TYPE_NAND; 791 break; 792 793 case IF_TYPE_NVME: 794 media = BOOT_TYPE_PCIE; 795 break; 796 797 default: 798 break; 799 } 800 801 memcpy((void *)&buf[0], (void *)&media, len); 802 common->residue = len; 803 common->data_size_from_cmnd = len; 804 805 return len; 806 } 807 808 static int rkusb_do_read_capacity(struct fsg_common *common, 809 struct fsg_buffhd *bh) 810 { 811 u8 *buf = (u8 *)bh->buf; 812 u32 len = common->data_size; 813 enum if_type type = ums[common->lun].block_dev.if_type; 814 int devnum = ums[common->lun].block_dev.devnum; 815 816 /* 817 * bit[0]: Direct LBA, 0: Disabled; 818 * bit[1]: Vendor Storage API, 0: Disabed (default); 819 * bit[2]: First 4M Access, 0: Disabled; 820 * bit[3]: Read LBA On, 0: Disabed (default); 821 * bit[4]: New Vendor Storage API, 0: Disabed; 822 * bit[5]: Read uart data from ram 823 * bit[6]: Read IDB config 824 * bit[7]: Read SecureMode 825 * bit[8]: New IDB feature 826 * bit[9]: Get storage media info 827 * bit[10:63}: Reserved. 828 */ 829 memset((void *)&buf[0], 0, len); 830 if (type == IF_TYPE_MMC || type == IF_TYPE_SD || type == IF_TYPE_NVME) 831 buf[0] = BIT(0) | BIT(2) | BIT(4); 832 else 833 buf[0] = BIT(0) | BIT(4); 834 835 if (type == IF_TYPE_MTD && 836 (devnum == BLK_MTD_NAND || 837 devnum == BLK_MTD_SPI_NAND)) 838 buf[0] |= (1 << 6); 839 840 #if !defined(CONFIG_ROCKCHIP_RV1126) && !defined(CONFIG_ROCKCHIP_RK3308) 841 if (type == IF_TYPE_MTD && devnum == BLK_MTD_SPI_NOR) 842 buf[0] |= (1 << 6); 843 #endif 844 845 #if defined(CONFIG_ROCKCHIP_NEW_IDB) 846 buf[1] = BIT(0); 847 #endif 848 buf[1] |= BIT(1); /* Switch Storage */ 849 buf[1] |= BIT(2); /* LBAwrite Parity */ 850 851 if (rkusb_usb3_capable() && !rkusb_force_usb2_enabled()) 852 buf[1] |= (1 << 4); 853 else 854 buf[1] &= (0 << 4); 855 856 /* Set data xfer size */ 857 common->residue = len; 858 common->data_size_from_cmnd = len; 859 860 return len; 861 } 862 863 static void rkusb_fixup_cbwcb(struct fsg_common *common, 864 struct fsg_buffhd *bh) 865 { 866 struct usb_request *req = bh->outreq; 867 struct fsg_bulk_cb_wrap *cbw = req->buf; 868 869 /* FIXME cbw.DataTransferLength was not set by Upgrade Tool */ 870 common->data_size = le32_to_cpu(cbw->DataTransferLength); 871 if (common->data_size == 0) { 872 common->data_size = 873 get_unaligned_be16(&common->cmnd[7]) << 9; 874 printf("Trasfer Length NOT set, please use new version tool\n"); 875 debug("%s %d, cmnd1 %x\n", __func__, 876 get_unaligned_be16(&common->cmnd[7]), 877 get_unaligned_be16(&common->cmnd[1])); 878 } 879 if (cbw->Flags & USB_BULK_IN_FLAG) 880 common->data_dir = DATA_DIR_TO_HOST; 881 else 882 common->data_dir = DATA_DIR_FROM_HOST; 883 884 /* Not support */ 885 common->cmnd[1] = 0; 886 } 887 888 static int rkusb_cmd_process(struct fsg_common *common, 889 struct fsg_buffhd *bh, int *reply) 890 { 891 struct usb_request *req = bh->outreq; 892 struct fsg_bulk_cb_wrap *cbw = req->buf; 893 int rc; 894 895 dump_cbw(cbw); 896 897 if (rkusb_check_lun(common)) { 898 *reply = -EINVAL; 899 return RKUSB_RC_ERROR; 900 } 901 902 switch (common->cmnd[0]) { 903 case RKUSB_TEST_UNIT_READY: 904 *reply = rkusb_do_test_unit_ready(common, bh); 905 rc = RKUSB_RC_FINISHED; 906 break; 907 908 case RKUSB_READ_FLASH_ID: 909 *reply = rkusb_do_read_flash_id(common, bh); 910 rc = RKUSB_RC_FINISHED; 911 break; 912 913 case RKUSB_TEST_BAD_BLOCK: 914 *reply = rkusb_do_test_bad_block(common, bh); 915 rc = RKUSB_RC_FINISHED; 916 break; 917 918 case RKUSB_ERASE_10_FORCE: 919 *reply = rkusb_do_erase_force(common, bh); 920 rc = RKUSB_RC_FINISHED; 921 break; 922 923 case RKUSB_LBA_READ_10: 924 rkusb_fixup_cbwcb(common, bh); 925 common->cmnd[0] = SC_READ_10; 926 common->cmnd[1] = 0; /* Not support */ 927 rc = RKUSB_RC_CONTINUE; 928 break; 929 930 case RKUSB_LBA_WRITE_10: 931 rkusb_fixup_cbwcb(common, bh); 932 common->cmnd[0] = SC_WRITE_10; 933 common->cmnd[1] = 0; /* Not support */ 934 rc = RKUSB_RC_CONTINUE; 935 break; 936 937 case RKUSB_READ_FLASH_INFO: 938 *reply = rkusb_do_read_flash_info(common, bh); 939 rc = RKUSB_RC_FINISHED; 940 break; 941 942 case RKUSB_GET_CHIP_VER: 943 *reply = rkusb_do_get_chip_info(common, bh); 944 rc = RKUSB_RC_FINISHED; 945 break; 946 947 case RKUSB_LBA_ERASE: 948 *reply = rkusb_do_lba_erase(common, bh); 949 rc = RKUSB_RC_FINISHED; 950 break; 951 952 #ifdef CONFIG_ROCKCHIP_VENDOR_PARTITION 953 case RKUSB_VS_WRITE: 954 *reply = rkusb_do_vs_write(common); 955 rc = RKUSB_RC_FINISHED; 956 break; 957 958 case RKUSB_VS_READ: 959 *reply = rkusb_do_vs_read(common); 960 rc = RKUSB_RC_FINISHED; 961 break; 962 #endif 963 case RKUSB_SWITCH_STORAGE: 964 *reply = rkusb_do_switch_storage(common); 965 rc = RKUSB_RC_FINISHED; 966 break; 967 case RKUSB_GET_STORAGE_MEDIA: 968 *reply = rkusb_do_get_storage_info(common, bh); 969 rc = RKUSB_RC_FINISHED; 970 break; 971 972 case RKUSB_READ_CAPACITY: 973 *reply = rkusb_do_read_capacity(common, bh); 974 rc = RKUSB_RC_FINISHED; 975 break; 976 977 case RKUSB_SWITCH_USB3: 978 *reply = rkusb_do_switch_to_usb3(common, bh); 979 rc = RKUSB_RC_FINISHED; 980 break; 981 982 case RKUSB_RESET: 983 *reply = rkusb_do_reset(common, bh); 984 rc = RKUSB_RC_FINISHED; 985 break; 986 987 case RKUSB_READ_10: 988 case RKUSB_WRITE_10: 989 printf("CMD Not support, pls use new version Tool\n"); 990 case RKUSB_SET_DEVICE_ID: 991 case RKUSB_ERASE_10: 992 case RKUSB_WRITE_SPARE: 993 case RKUSB_READ_SPARE: 994 case RKUSB_GET_VERSION: 995 case RKUSB_ERASE_SYS_DISK: 996 case RKUSB_SDRAM_READ_10: 997 case RKUSB_SDRAM_WRITE_10: 998 case RKUSB_SDRAM_EXECUTE: 999 case RKUSB_LOW_FORMAT: 1000 case RKUSB_SET_RESET_FLAG: 1001 case RKUSB_SPI_READ_10: 1002 case RKUSB_SPI_WRITE_10: 1003 case RKUSB_SESSION: 1004 /* Fall through */ 1005 default: 1006 rc = RKUSB_RC_UNKNOWN_CMND; 1007 break; 1008 } 1009 1010 return rc; 1011 } 1012 1013 int rkusb_do_check_parity(struct fsg_common *common) 1014 { 1015 int ret = 0, rc; 1016 u32 parity, i, usb_parity, lba, len; 1017 static u32 usb_check_buffer[1024 * 256]; 1018 1019 usb_parity = common->cmnd[9] | (common->cmnd[10] << 8) | 1020 (common->cmnd[11] << 16) | (common->cmnd[12] << 24); 1021 1022 if (common->cmnd[0] == SC_WRITE_10 && (usb_parity)) { 1023 lba = get_unaligned_be32(&common->cmnd[2]); 1024 len = common->data_size_from_cmnd >> 9; 1025 rc = blk_dread(&ums[common->lun].block_dev, lba, len, usb_check_buffer); 1026 parity = 0x000055aa; 1027 for (i = 0; i < len * 128; i++) 1028 parity += usb_check_buffer[i]; 1029 if (!rc || parity != usb_parity) 1030 common->phase_error = 1; 1031 } 1032 1033 return ret; 1034 } 1035 1036 DECLARE_GADGET_BIND_CALLBACK(rkusb_ums_dnl, fsg_add); 1037