drivers/usb/gadget/f_sdp.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * f_sdp.c -- USB HID Serial Download Protocol
   4  *
   5  * Copyright (C) 2017 Toradex
   6  * Author: Stefan Agner <stefan.agner@toradex.com>
   7  *
   8  * This file implements the Serial Download Protocol (SDP) as specified in
   9  * the i.MX 6 Reference Manual. The SDP is a USB HID based protocol and
  10  * allows to download images directly to memory. The implementation
  11  * works with the imx_loader (imx_usb) USB client software on host side.
  12  *
  13  * Not all commands are implemented, e.g. WRITE_REGISTER, DCD_WRITE and
  14  * SKIP_DCD_HEADER are only stubs.
  15  *
  16  * Parts of the implementation are based on f_dfu and f_thor.
  17  */
  18
  19 #include <errno.h>
  20 #include <common.h>
  21 #include <console.h>
  22 #include <env.h>
  23 #include <log.h>
  24 #include <malloc.h>
  25
  26 #include <linux/usb/ch9.h>
  27 #include <linux/usb/gadget.h>
  28 #include <linux/usb/composite.h>
  29
  30 #include <asm/io.h>
  31 #include <g_dnl.h>
  32 #include <sdp.h>
  33 #include <spl.h>
  34 #include <image.h>
  35 #include <imximage.h>
  36 #include <watchdog.h>
  37
  38 #define HID_REPORT_ID_MASK      0x000000ff
  39
  40 /*
  41  * HID class requests
  42  */
  43 #define HID_REQ_GET_REPORT              0x01
  44 #define HID_REQ_GET_IDLE                0x02
  45 #define HID_REQ_GET_PROTOCOL            0x03
  46 #define HID_REQ_SET_REPORT              0x09
  47 #define HID_REQ_SET_IDLE                0x0A
  48 #define HID_REQ_SET_PROTOCOL            0x0B
  49
  50 #define HID_USAGE_PAGE_LEN              76
  51
  52 struct hid_report {
  53         u8 usage_page[HID_USAGE_PAGE_LEN];
  54 } __packed;
  55
  56 #define SDP_READ_REGISTER       0x0101
  57 #define SDP_WRITE_REGISTER      0x0202
  58 #define SDP_WRITE_FILE          0x0404
  59 #define SDP_ERROR_STATUS        0x0505
  60 #define SDP_DCD_WRITE           0x0a0a
  61 #define SDP_JUMP_ADDRESS        0x0b0b
  62 #define SDP_SKIP_DCD_HEADER     0x0c0c
  63
  64 #define SDP_SECURITY_CLOSED             0x12343412
  65 #define SDP_SECURITY_OPEN               0x56787856
  66
  67 #define SDP_WRITE_FILE_COMPLETE         0x88888888
  68 #define SDP_WRITE_REGISTER_COMPLETE     0x128A8A12
  69 #define SDP_SKIP_DCD_HEADER_COMPLETE    0x900DD009
  70 #define SDP_ERROR_IMXHEADER             0x000a0533
  71
  72 #define SDP_COMMAND_LEN         16
  73
  74 #define SDP_HID_PACKET_SIZE_EP1 1024
  75
  76 #define SDP_EXIT 1
  77
  78 struct sdp_command {
  79         u16 cmd;
  80         u32 addr;
  81         u8 format;
  82         u32 cnt;
  83         u32 data;
  84         u8 rsvd;
  85 } __packed;
  86
  87 enum sdp_state {
  88         SDP_STATE_IDLE,
  89         SDP_STATE_RX_CMD,
  90         SDP_STATE_RX_DCD_DATA,
  91         SDP_STATE_RX_FILE_DATA,
  92         SDP_STATE_RX_FILE_DATA_BUSY,
  93         SDP_STATE_TX_SEC_CONF,
  94         SDP_STATE_TX_SEC_CONF_BUSY,
  95         SDP_STATE_TX_REGISTER,
  96         SDP_STATE_TX_REGISTER_BUSY,
  97         SDP_STATE_TX_STATUS,
  98         SDP_STATE_TX_STATUS_BUSY,
  99         SDP_STATE_JUMP,
 100 };
 101
 102 struct f_sdp {
 103         struct usb_function             usb_function;
 104
 105         struct usb_descriptor_header    **function;
 106
 107         u8                              altsetting;
 108         enum sdp_state                  state;
 109         enum sdp_state                  next_state;
 110         u32                             dnl_address;
 111         u32                             dnl_bytes;
 112         u32                             dnl_bytes_remaining;
 113         u32                             jmp_address;
 114         bool                            always_send_status;
 115         u32                             error_status;
 116
 117         /* EP0 request */
 118         struct usb_request              *req;
 119
 120         /* EP1 IN */
 121         struct usb_ep                   *in_ep;
 122         struct usb_request              *in_req;
 123         /* EP1 OUT */
 124         struct usb_ep                   *out_ep;
 125         struct usb_request              *out_req;
 126
 127         bool                            configuration_done;
 128         bool                            ep_int_enable;
 129 };
 130
 131 static struct f_sdp *sdp_func;
 132
 133 static inline struct f_sdp *func_to_sdp(struct usb_function *f)
 134 {
 135         return container_of(f, struct f_sdp, usb_function);
 136 }
 137
 138 static struct usb_interface_descriptor sdp_intf_runtime = {
 139         .bLength =              sizeof(sdp_intf_runtime),
 140         .bDescriptorType =      USB_DT_INTERFACE,
 141         .bAlternateSetting =    0,
 142         .bNumEndpoints =        2,
 143         .bInterfaceClass =      USB_CLASS_HID,
 144         .bInterfaceSubClass =   0,
 145         .bInterfaceProtocol =   0,
 146         /* .iInterface = DYNAMIC */
 147 };
 148
 149 /* HID configuration */
 150 static struct usb_class_hid_descriptor sdp_hid_desc = {
 151         .bLength =              sizeof(sdp_hid_desc),
 152         .bDescriptorType =      USB_DT_CS_DEVICE,
 153
 154         .bcdCDC =               __constant_cpu_to_le16(0x0110),
 155         .bCountryCode =         0,
 156         .bNumDescriptors =      1,
 157
 158         .bDescriptorType0       = USB_DT_HID_REPORT,
 159         .wDescriptorLength0     = HID_USAGE_PAGE_LEN,
 160 };
 161
 162 static struct usb_endpoint_descriptor in_desc = {
 163         .bLength =              USB_DT_ENDPOINT_SIZE,
 164         .bDescriptorType =      USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
 165
 166         .bEndpointAddress =     1 | USB_DIR_IN,
 167         .bmAttributes = USB_ENDPOINT_XFER_INT,
 168         .wMaxPacketSize =       64,
 169         .bInterval =            1,
 170 };
 171
 172 static struct usb_endpoint_descriptor out_desc = {
 173         .bLength =              USB_DT_ENDPOINT_SIZE,
 174         .bDescriptorType =      USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
 175
 176         .bEndpointAddress =     1 | USB_DIR_OUT,
 177         .bmAttributes =         USB_ENDPOINT_XFER_INT,
 178         .wMaxPacketSize =       64,
 179         .bInterval =            1,
 180 };
 181
 182 static struct usb_endpoint_descriptor in_hs_desc = {
 183         .bLength =              USB_DT_ENDPOINT_SIZE,
 184         .bDescriptorType =      USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
 185
 186         .bEndpointAddress =     1 | USB_DIR_IN,
 187         .bmAttributes = USB_ENDPOINT_XFER_INT,
 188         .wMaxPacketSize =       512,
 189         .bInterval =            3,
 190 };
 191
 192 static struct usb_endpoint_descriptor out_hs_desc = {
 193         .bLength =              USB_DT_ENDPOINT_SIZE,
 194         .bDescriptorType =      USB_DT_ENDPOINT, /*USB_DT_CS_ENDPOINT*/
 195
 196         .bEndpointAddress =     1 | USB_DIR_OUT,
 197         .bmAttributes =         USB_ENDPOINT_XFER_INT,
 198         .wMaxPacketSize =       SDP_HID_PACKET_SIZE_EP1,
 199         .bInterval =            3,
 200 };
 201
 202 static struct usb_descriptor_header *sdp_runtime_descs[] = {
 203         (struct usb_descriptor_header *)&sdp_intf_runtime,
 204         (struct usb_descriptor_header *)&sdp_hid_desc,
 205         (struct usb_descriptor_header *)&in_desc,
 206         (struct usb_descriptor_header *)&out_desc,
 207         NULL,
 208 };
 209
 210 static struct usb_descriptor_header *sdp_runtime_hs_descs[] = {
 211         (struct usb_descriptor_header *)&sdp_intf_runtime,
 212         (struct usb_descriptor_header *)&sdp_hid_desc,
 213         (struct usb_descriptor_header *)&in_hs_desc,
 214         (struct usb_descriptor_header *)&out_hs_desc,
 215         NULL,
 216 };
 217
 218 /* This is synchronized with what the SoC implementation reports */
 219 static struct hid_report sdp_hid_report = {
 220         .usage_page = {
 221                 0x06, 0x00, 0xff, /* Usage Page */
 222                 0x09, 0x01, /* Usage (Pointer?) */
 223                 0xa1, 0x01, /* Collection */
 224
 225                 0x85, 0x01, /* Report ID */
 226                 0x19, 0x01, /* Usage Minimum */
 227                 0x29, 0x01, /* Usage Maximum */
 228                 0x15, 0x00, /* Local Minimum */
 229                 0x26, 0xFF, 0x00, /* Local Maximum? */
 230                 0x75, 0x08, /* Report Size */
 231                 0x95, 0x10, /* Report Count */
 232                 0x91, 0x02, /* Output Data */
 233
 234                 0x85, 0x02, /* Report ID */
 235                 0x19, 0x01, /* Usage Minimum */
 236                 0x29, 0x01, /* Usage Maximum */
 237                 0x15, 0x00, /* Local Minimum */
 238                 0x26, 0xFF, 0x00, /* Local Maximum? */
 239                 0x75, 0x80, /* Report Size 128 */
 240                 0x95, 0x40, /* Report Count */
 241                 0x91, 0x02, /* Output Data */
 242
 243                 0x85, 0x03, /* Report ID */
 244                 0x19, 0x01, /* Usage Minimum */
 245                 0x29, 0x01, /* Usage Maximum */
 246                 0x15, 0x00, /* Local Minimum */
 247                 0x26, 0xFF, 0x00, /* Local Maximum? */
 248                 0x75, 0x08, /* Report Size 8 */
 249                 0x95, 0x04, /* Report Count */
 250                 0x81, 0x02, /* Input Data */
 251
 252                 0x85, 0x04, /* Report ID */
 253                 0x19, 0x01, /* Usage Minimum */
 254                 0x29, 0x01, /* Usage Maximum */
 255                 0x15, 0x00, /* Local Minimum */
 256                 0x26, 0xFF, 0x00, /* Local Maximum? */
 257                 0x75, 0x08, /* Report Size 8 */
 258                 0x95, 0x40, /* Report Count */
 259                 0x81, 0x02, /* Input Data */
 260                 0xc0
 261         },
 262 };
 263
 264 static const char sdp_name[] = "Serial Downloader Protocol";
 265
 266 /*
 267  * static strings, in UTF-8
 268  */
 269 static struct usb_string strings_sdp_generic[] = {
 270         [0].s = sdp_name,
 271         {  }                    /* end of list */
 272 };
 273
 274 static struct usb_gadget_strings stringtab_sdp_generic = {
 275         .language       = 0x0409,       /* en-us */
 276         .strings        = strings_sdp_generic,
 277 };
 278
 279 static struct usb_gadget_strings *sdp_generic_strings[] = {
 280         &stringtab_sdp_generic,
 281         NULL,
 282 };
 283
 284 static inline void *sdp_ptr(u32 val)
 285 {
 286         return (void *)(uintptr_t)val;
 287 }
 288
 289 static void sdp_rx_command_complete(struct usb_ep *ep, struct usb_request *req)
 290 {
 291         struct f_sdp *sdp = req->context;
 292         int status = req->status;
 293         u8 *data = req->buf;
 294         u8 report = data[0];
 295
 296         if (status != 0) {
 297                 pr_err("Status: %d\n", status);
 298                 return;
 299         }
 300
 301         if (report != 1) {
 302                 pr_err("Unexpected report %d\n", report);
 303                 return;
 304         }
 305
 306         struct sdp_command *cmd = req->buf + 1;
 307
 308         debug("%s: command: %04x, addr: %08x, cnt: %u\n",
 309               __func__, be16_to_cpu(cmd->cmd),
 310               be32_to_cpu(cmd->addr), be32_to_cpu(cmd->cnt));
 311
 312         switch (be16_to_cpu(cmd->cmd)) {
 313         case SDP_READ_REGISTER:
 314                 sdp->always_send_status = false;
 315                 sdp->error_status = 0x0;
 316
 317                 sdp->state = SDP_STATE_TX_SEC_CONF;
 318                 sdp->dnl_address = be32_to_cpu(cmd->addr);
 319                 sdp->dnl_bytes_remaining = be32_to_cpu(cmd->cnt);
 320                 sdp->next_state = SDP_STATE_TX_REGISTER;
 321                 printf("Reading %d registers at 0x%08x... ",
 322                        sdp->dnl_bytes_remaining, sdp->dnl_address);
 323                 break;
 324         case SDP_WRITE_FILE:
 325                 sdp->always_send_status = true;
 326                 sdp->error_status = SDP_WRITE_FILE_COMPLETE;
 327
 328                 sdp->state = SDP_STATE_RX_FILE_DATA;
 329                 sdp->dnl_address = cmd->addr ? be32_to_cpu(cmd->addr) : CONFIG_SDP_LOADADDR;
 330                 sdp->dnl_bytes_remaining = be32_to_cpu(cmd->cnt);
 331                 sdp->dnl_bytes = sdp->dnl_bytes_remaining;
 332                 sdp->next_state = SDP_STATE_IDLE;
 333
 334                 printf("Downloading file of size %d to 0x%08x... ",
 335                        sdp->dnl_bytes_remaining, sdp->dnl_address);
 336
 337                 break;
 338         case SDP_ERROR_STATUS:
 339                 sdp->always_send_status = true;
 340                 sdp->error_status = 0;
 341
 342                 sdp->state = SDP_STATE_TX_SEC_CONF;
 343                 sdp->next_state = SDP_STATE_IDLE;
 344                 break;
 345         case SDP_DCD_WRITE:
 346                 sdp->always_send_status = true;
 347                 sdp->error_status = SDP_WRITE_REGISTER_COMPLETE;
 348
 349                 sdp->state = SDP_STATE_RX_DCD_DATA;
 350                 sdp->dnl_bytes_remaining = be32_to_cpu(cmd->cnt);
 351                 sdp->next_state = SDP_STATE_IDLE;
 352                 break;
 353         case SDP_JUMP_ADDRESS:
 354                 sdp->always_send_status = false;
 355                 sdp->error_status = 0;
 356
 357                 sdp->jmp_address = cmd->addr ? be32_to_cpu(cmd->addr) : CONFIG_SDP_LOADADDR;
 358                 sdp->state = SDP_STATE_TX_SEC_CONF;
 359                 sdp->next_state = SDP_STATE_JUMP;
 360                 break;
 361         case SDP_SKIP_DCD_HEADER:
 362                 sdp->always_send_status = true;
 363                 sdp->error_status = SDP_SKIP_DCD_HEADER_COMPLETE;
 364
 365                 /* Ignore command, DCD not supported anyway */
 366                 sdp->state = SDP_STATE_TX_SEC_CONF;
 367                 sdp->next_state = SDP_STATE_IDLE;
 368                 break;
 369         default:
 370                 pr_err("Unknown command: %04x\n", be16_to_cpu(cmd->cmd));
 371         }
 372 }
 373
 374 static void sdp_rx_data_complete(struct usb_ep *ep, struct usb_request *req)
 375 {
 376         struct f_sdp *sdp = req->context;
 377         int status = req->status;
 378         u8 *data = req->buf;
 379         u8 report = data[0];
 380         int datalen = req->actual - 1;
 381
 382         if (status != 0) {
 383                 pr_err("Status: %d\n", status);
 384                 return;
 385         }
 386
 387         if (report != 2) {
 388                 pr_err("Unexpected report %d\n", report);
 389                 return;
 390         }
 391
 392         if (sdp->dnl_bytes_remaining < datalen) {
 393                 /*
 394                  * Some USB stacks require to send a complete buffer as
 395                  * specified in the HID descriptor. This leads to longer
 396                  * transfers than the file length, no problem for us.
 397                  */
 398                 sdp->dnl_bytes_remaining = 0;
 399         } else {
 400                 sdp->dnl_bytes_remaining -= datalen;
 401         }
 402
 403         if (sdp->state == SDP_STATE_RX_FILE_DATA_BUSY) {
 404                 memcpy(sdp_ptr(sdp->dnl_address), req->buf + 1, datalen);
 405                 sdp->dnl_address += datalen;
 406         }
 407
 408         if (sdp->dnl_bytes_remaining) {
 409                 sdp->state = SDP_STATE_RX_FILE_DATA;
 410                 return;
 411         }
 412
 413 #ifndef CONFIG_SPL_BUILD
 414         env_set_hex("filesize", sdp->dnl_bytes);
 415 #endif
 416         printf("done\n");
 417
 418         switch (sdp->state) {
 419         case SDP_STATE_RX_FILE_DATA_BUSY:
 420                 sdp->state = SDP_STATE_TX_SEC_CONF;
 421                 break;
 422         case SDP_STATE_RX_DCD_DATA:
 423                 sdp->state = SDP_STATE_TX_SEC_CONF;
 424                 break;
 425         default:
 426                 pr_err("Invalid state: %d\n", sdp->state);
 427         }
 428 }
 429
 430 static void sdp_tx_complete(struct usb_ep *ep, struct usb_request *req)
 431 {
 432         struct f_sdp *sdp = req->context;
 433         int status = req->status;
 434
 435         if (status != 0) {
 436                 pr_err("Status: %d\n", status);
 437                 return;
 438         }
 439
 440         switch (sdp->state) {
 441         case SDP_STATE_TX_SEC_CONF_BUSY:
 442                 /* Not all commands require status report */
 443                 if (sdp->always_send_status || sdp->error_status)
 444                         sdp->state = SDP_STATE_TX_STATUS;
 445                 else
 446                         sdp->state = sdp->next_state;
 447
 448                 break;
 449         case SDP_STATE_TX_STATUS_BUSY:
 450                 sdp->state = sdp->next_state;
 451                 break;
 452         case SDP_STATE_TX_REGISTER_BUSY:
 453                 if (sdp->dnl_bytes_remaining)
 454                         sdp->state = SDP_STATE_TX_REGISTER;
 455                 else
 456                         sdp->state = SDP_STATE_IDLE;
 457                 break;
 458         default:
 459                 pr_err("Wrong State: %d\n", sdp->state);
 460                 sdp->state = SDP_STATE_IDLE;
 461                 break;
 462         }
 463         debug("%s complete --> %d, %d/%d\n", ep->name,
 464               status, req->actual, req->length);
 465 }
 466
 467 static int sdp_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 468 {
 469         struct usb_gadget *gadget = f->config->cdev->gadget;
 470         struct usb_request *req = f->config->cdev->req;
 471         struct f_sdp *sdp = f->config->cdev->req->context;
 472         u16 len = le16_to_cpu(ctrl->wLength);
 473         u16 w_value = le16_to_cpu(ctrl->wValue);
 474         int value = 0;
 475         u8 req_type = ctrl->bRequestType & USB_TYPE_MASK;
 476
 477         debug("w_value: 0x%04x len: 0x%04x\n", w_value, len);
 478         debug("req_type: 0x%02x ctrl->bRequest: 0x%02x sdp->state: %d\n",
 479               req_type, ctrl->bRequest, sdp->state);
 480
 481         if (req_type == USB_TYPE_STANDARD) {
 482                 if (ctrl->bRequest == USB_REQ_GET_DESCRIPTOR) {
 483                         /* Send HID report descriptor */
 484                         value = min(len, (u16) sizeof(sdp_hid_report));
 485                         memcpy(req->buf, &sdp_hid_report, value);
 486                         sdp->configuration_done = true;
 487                 }
 488         }
 489
 490         if (req_type == USB_TYPE_CLASS) {
 491                 int report = w_value & HID_REPORT_ID_MASK;
 492
 493                 /* HID (SDP) request */
 494                 switch (ctrl->bRequest) {
 495                 case HID_REQ_SET_REPORT:
 496                         switch (report) {
 497                         case 1:
 498                                 value = SDP_COMMAND_LEN + 1;
 499                                 req->complete = sdp_rx_command_complete;
 500                                 sdp_func->ep_int_enable = false;
 501                                 break;
 502                         case 2:
 503                                 value = len;
 504                                 req->complete = sdp_rx_data_complete;
 505                                 sdp_func->state = SDP_STATE_RX_FILE_DATA_BUSY;
 506                                 break;
 507                         }
 508                 }
 509         }
 510
 511         if (value >= 0) {
 512                 req->length = value;
 513                 req->zero = value < len;
 514                 value = usb_ep_queue(gadget->ep0, req, 0);
 515                 if (value < 0) {
 516                         debug("ep_queue --> %d\n", value);
 517                         req->status = 0;
 518                 }
 519         }
 520
 521         return value;
 522 }
 523
 524 static int sdp_bind(struct usb_configuration *c, struct usb_function *f)
 525 {
 526         struct usb_gadget *gadget = c->cdev->gadget;
 527         struct usb_composite_dev *cdev = c->cdev;
 528         struct f_sdp *sdp = func_to_sdp(f);
 529         int rv = 0, id;
 530
 531         id = usb_interface_id(c, f);
 532         if (id < 0)
 533                 return id;
 534         sdp_intf_runtime.bInterfaceNumber = id;
 535
 536         struct usb_ep *ep_in, *ep_out;
 537
 538         /* allocate instance-specific endpoints */
 539         ep_in = usb_ep_autoconfig(gadget, &in_desc);
 540         if (!ep_in) {
 541                 rv = -ENODEV;
 542                 goto error;
 543         }
 544
 545         ep_out = usb_ep_autoconfig(gadget, &out_desc);
 546         if (!ep_out) {
 547                 rv = -ENODEV;
 548                 goto error;
 549         }
 550
 551         if (gadget_is_dualspeed(gadget)) {
 552                 /* Assume endpoint addresses are the same for both speeds */
 553                 in_hs_desc.bEndpointAddress = in_desc.bEndpointAddress;
 554                 out_hs_desc.bEndpointAddress = out_desc.bEndpointAddress;
 555         }
 556
 557         sdp->in_ep = ep_in; /* Store IN EP for enabling @ setup */
 558         sdp->out_ep = ep_out;
 559
 560         cdev->req->context = sdp;
 561
 562 error:
 563         return rv;
 564 }
 565
 566 static void sdp_unbind(struct usb_configuration *c, struct usb_function *f)
 567 {
 568         free(sdp_func);
 569         sdp_func = NULL;
 570 }
 571
 572 static struct usb_request *alloc_ep_req(struct usb_ep *ep, unsigned length)
 573 {
 574         struct usb_request *req;
 575
 576         req = usb_ep_alloc_request(ep, 0);
 577         if (!req)
 578                 return req;
 579
 580         req->length = length;
 581         req->buf = memalign(CONFIG_SYS_CACHELINE_SIZE, length);
 582         if (!req->buf) {
 583                 usb_ep_free_request(ep, req);
 584                 req = NULL;
 585         }
 586
 587         return req;
 588 }
 589
 590
 591 static struct usb_request *sdp_start_ep(struct usb_ep *ep, bool in)
 592 {
 593         struct usb_request *req;
 594
 595         if (in)
 596                 req = alloc_ep_req(ep, 65);
 597         else
 598                 req = alloc_ep_req(ep, 2048);
 599 /*
 600  * OUT endpoint request length should be an integral multiple of
 601  * maxpacket size 1024, else we break on certain controllers like
 602  * DWC3 that expect bulk OUT requests to be divisible by maxpacket size.
 603  */
 604         debug("%s: ep:%p req:%p\n", __func__, ep, req);
 605
 606         if (!req)
 607                 return NULL;
 608
 609         memset(req->buf, 0, req->length);
 610         if (in)
 611                 req->complete = sdp_tx_complete;
 612         else
 613                 req->complete = sdp_rx_command_complete;
 614
 615         return req;
 616 }
 617 static int sdp_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 618 {
 619         struct f_sdp *sdp = func_to_sdp(f);
 620         struct usb_composite_dev *cdev = f->config->cdev;
 621         struct usb_gadget *gadget = cdev->gadget;
 622         int result;
 623
 624         debug("%s: intf: %d alt: %d\n", __func__, intf, alt);
 625
 626         if (gadget_is_dualspeed(gadget) && gadget->speed == USB_SPEED_HIGH) {
 627                 result = usb_ep_enable(sdp->in_ep, &in_hs_desc);
 628                 result |= usb_ep_enable(sdp->out_ep, &out_hs_desc);
 629         } else {
 630                 result = usb_ep_enable(sdp->in_ep, &in_desc);
 631                 result |= usb_ep_enable(sdp->out_ep, &out_desc);
 632         }
 633         if (result)
 634                 return result;
 635
 636         sdp->in_req = sdp_start_ep(sdp->in_ep, true);
 637         sdp->in_req->context = sdp;
 638         sdp->out_req = sdp_start_ep(sdp->out_ep, false);
 639         sdp->out_req->context = sdp;
 640
 641         sdp->in_ep->driver_data = cdev; /* claim */
 642         sdp->out_ep->driver_data = cdev; /* claim */
 643
 644         sdp->altsetting = alt;
 645         sdp->state = SDP_STATE_IDLE;
 646         sdp->ep_int_enable = true;
 647
 648         return 0;
 649 }
 650
 651 static int sdp_get_alt(struct usb_function *f, unsigned intf)
 652 {
 653         struct f_sdp *sdp = func_to_sdp(f);
 654
 655         return sdp->altsetting;
 656 }
 657
 658 static void sdp_disable(struct usb_function *f)
 659 {
 660         struct f_sdp *sdp = func_to_sdp(f);
 661
 662         usb_ep_disable(sdp->in_ep);
 663         usb_ep_disable(sdp->out_ep);
 664
 665         if (sdp->in_req) {
 666                 free(sdp->in_req->buf);
 667                 usb_ep_free_request(sdp->in_ep, sdp->in_req);
 668                 sdp->in_req = NULL;
 669         }
 670         if (sdp->out_req) {
 671                 free(sdp->out_req->buf);
 672                 usb_ep_free_request(sdp->out_ep, sdp->out_req);
 673                 sdp->out_req = NULL;
 674         }
 675 }
 676
 677 static int sdp_bind_config(struct usb_configuration *c)
 678 {
 679         int status;
 680
 681         if (!sdp_func) {
 682                 sdp_func = memalign(CONFIG_SYS_CACHELINE_SIZE, sizeof(*sdp_func));
 683                 if (!sdp_func)
 684                         return -ENOMEM;
 685         }
 686
 687         memset(sdp_func, 0, sizeof(*sdp_func));
 688
 689         sdp_func->usb_function.name = "sdp";
 690         sdp_func->usb_function.hs_descriptors = sdp_runtime_hs_descs;
 691         sdp_func->usb_function.descriptors = sdp_runtime_descs;
 692         sdp_func->usb_function.bind = sdp_bind;
 693         sdp_func->usb_function.unbind = sdp_unbind;
 694         sdp_func->usb_function.set_alt = sdp_set_alt;
 695         sdp_func->usb_function.get_alt = sdp_get_alt;
 696         sdp_func->usb_function.disable = sdp_disable;
 697         sdp_func->usb_function.strings = sdp_generic_strings;
 698         sdp_func->usb_function.setup = sdp_setup;
 699
 700         status = usb_add_function(c, &sdp_func->usb_function);
 701
 702         return status;
 703 }
 704
 705 int sdp_init(int controller_index)
 706 {
 707         printf("SDP: initialize...\n");
 708         while (!sdp_func->configuration_done) {
 709                 if (ctrlc()) {
 710                         puts("\rCTRL+C - Operation aborted.\n");
 711                         return 1;
 712                 }
 713
 714                 schedule();
 715                 usb_gadget_handle_interrupts(controller_index);
 716         }
 717
 718         return 0;
 719 }
 720
 721 static u32 sdp_jump_imxheader(void *address)
 722 {
 723         flash_header_v2_t *headerv2 = address;
 724         ulong (*entry)(void);
 725
 726         if (headerv2->header.tag != IVT_HEADER_TAG) {
 727                 printf("Header Tag is not an IMX image\n");
 728                 return SDP_ERROR_IMXHEADER;
 729         }
 730
 731         printf("Jumping to 0x%08x\n", headerv2->entry);
 732         entry = sdp_ptr(headerv2->entry);
 733         entry();
 734
 735         /* The image probably never returns hence we won't reach that point */
 736         return 0;
 737 }
 738
 739 #ifdef CONFIG_SPL_BUILD
 740 static ulong sdp_load_read(struct spl_load_info *load, ulong sector,
 741                            ulong count, void *buf)
 742 {
 743         debug("%s: sector %lx, count %lx, buf %lx\n",
 744               __func__, sector, count, (ulong)buf);
 745         memcpy(buf, (void *)(load->dev + sector), count);
 746         return count;
 747 }
 748
 749 static ulong search_fit_header(ulong p, int size)
 750 {
 751         int i;
 752
 753         for (i = 0; i < size; i += 4) {
 754                 if (genimg_get_format((const void *)(p + i)) == IMAGE_FORMAT_FIT)
 755                         return p + i;
 756         }
 757
 758         return 0;
 759 }
 760
 761 static ulong search_container_header(ulong p, int size)
 762 {
 763         int i;
 764         u8 *hdr;
 765
 766         for (i = 0; i < size; i += 4) {
 767                 hdr = (u8 *)(p + i);
 768                 if (*(hdr + 3) == 0x87 && *hdr == 0)
 769                         if (*(hdr + 1) != 0 || *(hdr + 2) != 0)
 770                                 return p + i;
 771         }
 772         return 0;
 773 }
 774 #endif
 775
 776 static int sdp_handle_in_ep(struct spl_image_info *spl_image,
 777                             struct spl_boot_device *bootdev)
 778 {
 779         u8 *data = sdp_func->in_req->buf;
 780         u32 status;
 781         int datalen;
 782
 783         switch (sdp_func->state) {
 784         case SDP_STATE_TX_SEC_CONF:
 785                 debug("Report 3: HAB security\n");
 786                 data[0] = 3;
 787
 788                 status = SDP_SECURITY_OPEN;
 789                 memcpy(&data[1], &status, 4);
 790                 sdp_func->in_req->length = 5;
 791                 usb_ep_queue(sdp_func->in_ep, sdp_func->in_req, 0);
 792                 sdp_func->state = SDP_STATE_TX_SEC_CONF_BUSY;
 793                 break;
 794
 795         case SDP_STATE_TX_STATUS:
 796                 debug("Report 4: Status\n");
 797                 data[0] = 4;
 798
 799                 memcpy(&data[1], &sdp_func->error_status, 4);
 800                 sdp_func->in_req->length = 65;
 801                 usb_ep_queue(sdp_func->in_ep, sdp_func->in_req, 0);
 802                 sdp_func->state = SDP_STATE_TX_STATUS_BUSY;
 803                 break;
 804         case SDP_STATE_TX_REGISTER:
 805                 debug("Report 4: Register Values\n");
 806                 data[0] = 4;
 807
 808                 datalen = sdp_func->dnl_bytes_remaining;
 809
 810                 if (datalen > 64)
 811                         datalen = 64;
 812
 813                 memcpy(&data[1], sdp_ptr(sdp_func->dnl_address), datalen);
 814                 sdp_func->in_req->length = 65;
 815
 816                 sdp_func->dnl_bytes_remaining -= datalen;
 817                 sdp_func->dnl_address += datalen;
 818
 819                 usb_ep_queue(sdp_func->in_ep, sdp_func->in_req, 0);
 820                 sdp_func->state = SDP_STATE_TX_REGISTER_BUSY;
 821                 break;
 822         case SDP_STATE_JUMP:
 823                 printf("Jumping to header at 0x%08x\n", sdp_func->jmp_address);
 824                 status = sdp_jump_imxheader(sdp_ptr(sdp_func->jmp_address));
 825
 826                 /* If imx header fails, try some U-Boot specific headers */
 827                 if (status) {
 828 #ifdef CONFIG_SPL_BUILD
 829                         if (IS_ENABLED(CONFIG_SPL_LOAD_IMX_CONTAINER))
 830                                 sdp_func->jmp_address = (u32)search_container_header((ulong)sdp_func->jmp_address, sdp_func->dnl_bytes);
 831                         else if (IS_ENABLED(CONFIG_SPL_LOAD_FIT))
 832                                 sdp_func->jmp_address = (u32)search_fit_header((ulong)sdp_func->jmp_address, sdp_func->dnl_bytes);
 833                         if (sdp_func->jmp_address == 0)
 834                                 panic("Error in search header, failed to jump\n");
 835
 836                         printf("Found header at 0x%08x\n", sdp_func->jmp_address);
 837
 838                         struct legacy_img_hdr *header =
 839                                 sdp_ptr(sdp_func->jmp_address);
 840 #ifdef CONFIG_SPL_LOAD_FIT
 841                         if (image_get_magic(header) == FDT_MAGIC) {
 842                                 struct spl_load_info load;
 843
 844                                 debug("Found FIT\n");
 845                                 load.dev = header;
 846                                 load.bl_len = 1;
 847                                 load.read = sdp_load_read;
 848                                 spl_load_simple_fit(spl_image, &load, 0,
 849                                                     header);
 850
 851                                 return SDP_EXIT;
 852                         }
 853 #endif
 854                         if (IS_ENABLED(CONFIG_SPL_LOAD_IMX_CONTAINER)) {
 855                                 struct spl_load_info load;
 856
 857                                 load.dev = header;
 858                                 load.bl_len = 1;
 859                                 load.read = sdp_load_read;
 860                                 spl_load_imx_container(spl_image, &load, 0);
 861                                 return SDP_EXIT;
 862                         }
 863
 864                         /* In SPL, allow jumps to U-Boot images */
 865                         struct spl_image_info spl_image = {};
 866                         struct spl_boot_device bootdev = {};
 867                         spl_parse_image_header(&spl_image, &bootdev, header);
 868                         spl_board_prepare_for_boot();
 869                         jump_to_image_no_args(&spl_image);
 870 #else
 871                         /* In U-Boot, allow jumps to scripts */
 872                         cmd_source_script(sdp_func->jmp_address, NULL, NULL);
 873 #endif
 874                 }
 875
 876                 sdp_func->next_state = SDP_STATE_IDLE;
 877                 sdp_func->error_status = status;
 878
 879                 /* Only send Report 4 if there was an error */
 880                 if (status)
 881                         sdp_func->state = SDP_STATE_TX_STATUS;
 882                 else
 883                         sdp_func->state = SDP_STATE_IDLE;
 884                 break;
 885         default:
 886                 break;
 887         };
 888
 889         return 0;
 890 }
 891
 892 static void sdp_handle_out_ep(void)
 893 {
 894         int rc;
 895
 896         if (sdp_func->state == SDP_STATE_IDLE) {
 897                 sdp_func->out_req->complete = sdp_rx_command_complete;
 898                 rc = usb_ep_queue(sdp_func->out_ep, sdp_func->out_req, 0);
 899                 if (rc)
 900                         printf("error in submission: %s\n",
 901                                sdp_func->out_ep->name);
 902                 sdp_func->state = SDP_STATE_RX_CMD;
 903         } else if (sdp_func->state == SDP_STATE_RX_FILE_DATA) {
 904                 sdp_func->out_req->complete = sdp_rx_data_complete;
 905                 rc = usb_ep_queue(sdp_func->out_ep, sdp_func->out_req, 0);
 906                 if (rc)
 907                         printf("error in submission: %s\n",
 908                                sdp_func->out_ep->name);
 909                 sdp_func->state = SDP_STATE_RX_FILE_DATA_BUSY;
 910         }
 911 }
 912
 913 #ifndef CONFIG_SPL_BUILD
 914 int sdp_handle(int controller_index)
 915 #else
 916 int spl_sdp_handle(int controller_index, struct spl_image_info *spl_image,
 917                    struct spl_boot_device *bootdev)
 918 #endif
 919 {
 920         int flag = 0;
 921         printf("SDP: handle requests...\n");
 922         while (1) {
 923                 if (ctrlc()) {
 924                         puts("\rCTRL+C - Operation aborted.\n");
 925                         return -EINVAL;
 926                 }
 927
 928                 if (flag == SDP_EXIT)
 929                         return 0;
 930
 931                 schedule();
 932                 usb_gadget_handle_interrupts(controller_index);
 933
 934 #ifdef CONFIG_SPL_BUILD
 935                 flag = sdp_handle_in_ep(spl_image, bootdev);
 936 #else
 937                 flag = sdp_handle_in_ep(NULL, NULL);
 938 #endif
 939                 if (sdp_func->ep_int_enable)
 940                         sdp_handle_out_ep();
 941         }
 942 }
 943
 944 int sdp_add(struct usb_configuration *c)
 945 {
 946         int id;
 947
 948         id = usb_string_id(c->cdev);
 949         if (id < 0)
 950                 return id;
 951         strings_sdp_generic[0].id = id;
 952         sdp_intf_runtime.iInterface = id;
 953
 954         debug("%s: cdev: %p gadget: %p gadget->ep0: %p\n", __func__,
 955               c->cdev, c->cdev->gadget, c->cdev->gadget->ep0);
 956
 957         return sdp_bind_config(c);
 958 }
 959
 960 DECLARE_GADGET_BIND_CALLBACK(usb_dnl_sdp, sdp_add);