uboot/drivers/usb/gadget/f_sdp.c
<<
>>
Prefs
   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
  52struct 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
  78struct sdp_command {
  79        u16 cmd;
  80        u32 addr;
  81        u8 format;
  82        u32 cnt;
  83        u32 data;
  84        u8 rsvd;
  85} __packed;
  86
  87enum 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
 102struct 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
 131static struct f_sdp *sdp_func;
 132
 133static inline struct f_sdp *func_to_sdp(struct usb_function *f)
 134{
 135        return container_of(f, struct f_sdp, usb_function);
 136}
 137
 138static 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 */
 150static 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
 162static 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
 172static 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
 182static 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
 192static 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
 202static 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
 210static 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 */
 219static 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
 264static const char sdp_name[] = "Serial Downloader Protocol";
 265
 266/*
 267 * static strings, in UTF-8
 268 */
 269static struct usb_string strings_sdp_generic[] = {
 270        [0].s = sdp_name,
 271        {  }                    /* end of list */
 272};
 273
 274static struct usb_gadget_strings stringtab_sdp_generic = {
 275        .language       = 0x0409,       /* en-us */
 276        .strings        = strings_sdp_generic,
 277};
 278
 279static struct usb_gadget_strings *sdp_generic_strings[] = {
 280        &stringtab_sdp_generic,
 281        NULL,
 282};
 283
 284static inline void *sdp_ptr(u32 val)
 285{
 286        return (void *)(uintptr_t)val;
 287}
 288
 289static 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
 374static 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
 430static 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
 467static 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
 524static 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
 562error:
 563        return rv;
 564}
 565
 566static void sdp_unbind(struct usb_configuration *c, struct usb_function *f)
 567{
 568        free(sdp_func);
 569        sdp_func = NULL;
 570}
 571
 572static 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
 591static 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}
 617static 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
 651static 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
 658static 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
 677static 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
 705int 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                WATCHDOG_RESET();
 715                usb_gadget_handle_interrupts(controller_index);
 716        }
 717
 718        return 0;
 719}
 720
 721static 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
 740static 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
 749static 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
 761static 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
 776static 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                        image_header_t *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                        jump_to_image_no_args(&spl_image);
 869#else
 870                        /* In U-Boot, allow jumps to scripts */
 871                        image_source_script(sdp_func->jmp_address, "script@1");
 872#endif
 873                }
 874
 875                sdp_func->next_state = SDP_STATE_IDLE;
 876                sdp_func->error_status = status;
 877
 878                /* Only send Report 4 if there was an error */
 879                if (status)
 880                        sdp_func->state = SDP_STATE_TX_STATUS;
 881                else
 882                        sdp_func->state = SDP_STATE_IDLE;
 883                break;
 884        default:
 885                break;
 886        };
 887
 888        return 0;
 889}
 890
 891static void sdp_handle_out_ep(void)
 892{
 893        int rc;
 894
 895        if (sdp_func->state == SDP_STATE_IDLE) {
 896                sdp_func->out_req->complete = sdp_rx_command_complete;
 897                rc = usb_ep_queue(sdp_func->out_ep, sdp_func->out_req, 0);
 898                if (rc)
 899                        printf("error in submission: %s\n",
 900                               sdp_func->out_ep->name);
 901                sdp_func->state = SDP_STATE_RX_CMD;
 902        } else if (sdp_func->state == SDP_STATE_RX_FILE_DATA) {
 903                sdp_func->out_req->complete = sdp_rx_data_complete;
 904                rc = usb_ep_queue(sdp_func->out_ep, sdp_func->out_req, 0);
 905                if (rc)
 906                        printf("error in submission: %s\n",
 907                               sdp_func->out_ep->name);
 908                sdp_func->state = SDP_STATE_RX_FILE_DATA_BUSY;
 909        }
 910}
 911
 912#ifndef CONFIG_SPL_BUILD
 913int sdp_handle(int controller_index)
 914#else
 915int spl_sdp_handle(int controller_index, struct spl_image_info *spl_image,
 916                   struct spl_boot_device *bootdev)
 917#endif
 918{
 919        int flag = 0;
 920        printf("SDP: handle requests...\n");
 921        while (1) {
 922                if (ctrlc()) {
 923                        puts("\rCTRL+C - Operation aborted.\n");
 924                        return -EINVAL;
 925                }
 926
 927                if (flag == SDP_EXIT)
 928                        return 0;
 929
 930                WATCHDOG_RESET();
 931                usb_gadget_handle_interrupts(controller_index);
 932
 933#ifdef CONFIG_SPL_BUILD
 934                flag = sdp_handle_in_ep(spl_image, bootdev);
 935#else
 936                flag = sdp_handle_in_ep(NULL, NULL);
 937#endif
 938                if (sdp_func->ep_int_enable)
 939                        sdp_handle_out_ep();
 940        }
 941}
 942
 943int sdp_add(struct usb_configuration *c)
 944{
 945        int id;
 946
 947        id = usb_string_id(c->cdev);
 948        if (id < 0)
 949                return id;
 950        strings_sdp_generic[0].id = id;
 951        sdp_intf_runtime.iInterface = id;
 952
 953        debug("%s: cdev: %p gadget: %p gadget->ep0: %p\n", __func__,
 954              c->cdev, c->cdev->gadget, c->cdev->gadget->ep0);
 955
 956        return sdp_bind_config(c);
 957}
 958
 959DECLARE_GADGET_BIND_CALLBACK(usb_dnl_sdp, sdp_add);
 960