drivers/usb/c67x00/c67x00-sched.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * c67x00-sched.c: Cypress C67X00 USB Host Controller Driver - TD scheduling
   4  *
   5  * Copyright (C) 2006-2008 Barco N.V.
   6  *    Derived from the Cypress cy7c67200/300 ezusb linux driver and
   7  *    based on multiple host controller drivers inside the linux kernel.
   8  */
   9
  10 #include <linux/kthread.h>
  11 #include <linux/slab.h>
  12
  13 #include "c67x00.h"
  14 #include "c67x00-hcd.h"
  15
  16 /*
  17  * These are the stages for a control urb, they are kept
  18  * in both urb->interval and td->privdata.
  19  */
  20 #define SETUP_STAGE             0
  21 #define DATA_STAGE              1
  22 #define STATUS_STAGE            2
  23
  24 /* -------------------------------------------------------------------------- */
  25
  26 /*
  27  * struct c67x00_ep_data: Host endpoint data structure
  28  */
  29 struct c67x00_ep_data {
  30         struct list_head queue;
  31         struct list_head node;
  32         struct usb_host_endpoint *hep;
  33         struct usb_device *dev;
  34         u16 next_frame;         /* For int/isoc transactions */
  35 };
  36
  37 /*
  38  * struct c67x00_td
  39  *
  40  * Hardware parts are little endiannes, SW in CPU endianess.
  41  */
  42 struct c67x00_td {
  43         /* HW specific part */
  44         __le16 ly_base_addr;    /* Bytes 0-1 */
  45         __le16 port_length;     /* Bytes 2-3 */
  46         u8 pid_ep;              /* Byte 4 */
  47         u8 dev_addr;            /* Byte 5 */
  48         u8 ctrl_reg;            /* Byte 6 */
  49         u8 status;              /* Byte 7 */
  50         u8 retry_cnt;           /* Byte 8 */
  51 #define TT_OFFSET               2
  52 #define TT_CONTROL              0
  53 #define TT_ISOCHRONOUS          1
  54 #define TT_BULK                 2
  55 #define TT_INTERRUPT            3
  56         u8 residue;             /* Byte 9 */
  57         __le16 next_td_addr;    /* Bytes 10-11 */
  58         /* SW part */
  59         struct list_head td_list;
  60         u16 td_addr;
  61         void *data;
  62         struct urb *urb;
  63         unsigned long privdata;
  64
  65         /* These are needed for handling the toggle bits:
  66          * an urb can be dequeued while a td is in progress
  67          * after checking the td, the toggle bit might need to
  68          * be fixed */
  69         struct c67x00_ep_data *ep_data;
  70         unsigned int pipe;
  71 };
  72
  73 struct c67x00_urb_priv {
  74         struct list_head hep_node;
  75         struct urb *urb;
  76         int port;
  77         int cnt;                /* packet number for isoc */
  78         int status;
  79         struct c67x00_ep_data *ep_data;
  80 };
  81
  82 #define td_udev(td)     ((td)->ep_data->dev)
  83
  84 #define CY_TD_SIZE              12
  85
  86 #define TD_PIDEP_OFFSET         0x04
  87 #define TD_PIDEPMASK_PID        0xF0
  88 #define TD_PIDEPMASK_EP         0x0F
  89 #define TD_PORTLENMASK_DL       0x03FF
  90 #define TD_PORTLENMASK_PN       0xC000
  91
  92 #define TD_STATUS_OFFSET        0x07
  93 #define TD_STATUSMASK_ACK       0x01
  94 #define TD_STATUSMASK_ERR       0x02
  95 #define TD_STATUSMASK_TMOUT     0x04
  96 #define TD_STATUSMASK_SEQ       0x08
  97 #define TD_STATUSMASK_SETUP     0x10
  98 #define TD_STATUSMASK_OVF       0x20
  99 #define TD_STATUSMASK_NAK       0x40
 100 #define TD_STATUSMASK_STALL     0x80
 101
 102 #define TD_ERROR_MASK           (TD_STATUSMASK_ERR | TD_STATUSMASK_TMOUT | \
 103                                  TD_STATUSMASK_STALL)
 104
 105 #define TD_RETRYCNT_OFFSET      0x08
 106 #define TD_RETRYCNTMASK_ACT_FLG 0x10
 107 #define TD_RETRYCNTMASK_TX_TYPE 0x0C
 108 #define TD_RETRYCNTMASK_RTY_CNT 0x03
 109
 110 #define TD_RESIDUE_OVERFLOW     0x80
 111
 112 #define TD_PID_IN               0x90
 113
 114 /* Residue: signed 8bits, neg -> OVERFLOW, pos -> UNDERFLOW */
 115 #define td_residue(td)          ((__s8)(td->residue))
 116 #define td_ly_base_addr(td)     (__le16_to_cpu((td)->ly_base_addr))
 117 #define td_port_length(td)      (__le16_to_cpu((td)->port_length))
 118 #define td_next_td_addr(td)     (__le16_to_cpu((td)->next_td_addr))
 119
 120 #define td_active(td)           ((td)->retry_cnt & TD_RETRYCNTMASK_ACT_FLG)
 121 #define td_length(td)           (td_port_length(td) & TD_PORTLENMASK_DL)
 122
 123 #define td_sequence_ok(td)      (!td->status || \
 124                                  (!(td->status & TD_STATUSMASK_SEQ) ==  \
 125                                   !(td->ctrl_reg & SEQ_SEL)))
 126
 127 #define td_acked(td)            (!td->status || \
 128                                  (td->status & TD_STATUSMASK_ACK))
 129 #define td_actual_bytes(td)     (td_length(td) - td_residue(td))
 130
 131 /* -------------------------------------------------------------------------- */
 132
 133 /*
 134  * dbg_td - Dump the contents of the TD
 135  */
 136 static void dbg_td(struct c67x00_hcd *c67x00, struct c67x00_td *td, char *msg)
 137 {
 138         struct device *dev = c67x00_hcd_dev(c67x00);
 139
 140         dev_dbg(dev, "### %s at 0x%04x\n", msg, td->td_addr);
 141         dev_dbg(dev, "urb:      0x%p\n", td->urb);
 142         dev_dbg(dev, "endpoint:   %4d\n", usb_pipeendpoint(td->pipe));
 143         dev_dbg(dev, "pipeout:    %4d\n", usb_pipeout(td->pipe));
 144         dev_dbg(dev, "ly_base_addr: 0x%04x\n", td_ly_base_addr(td));
 145         dev_dbg(dev, "port_length:  0x%04x\n", td_port_length(td));
 146         dev_dbg(dev, "pid_ep:         0x%02x\n", td->pid_ep);
 147         dev_dbg(dev, "dev_addr:       0x%02x\n", td->dev_addr);
 148         dev_dbg(dev, "ctrl_reg:       0x%02x\n", td->ctrl_reg);
 149         dev_dbg(dev, "status:         0x%02x\n", td->status);
 150         dev_dbg(dev, "retry_cnt:      0x%02x\n", td->retry_cnt);
 151         dev_dbg(dev, "residue:        0x%02x\n", td->residue);
 152         dev_dbg(dev, "next_td_addr: 0x%04x\n", td_next_td_addr(td));
 153         dev_dbg(dev, "data: %*ph\n", td_length(td), td->data);
 154 }
 155
 156 /* -------------------------------------------------------------------------- */
 157 /* Helper functions */
 158
 159 static inline u16 c67x00_get_current_frame_number(struct c67x00_hcd *c67x00)
 160 {
 161         return c67x00_ll_husb_get_frame(c67x00->sie) & HOST_FRAME_MASK;
 162 }
 163
 164 /*
 165  * frame_add
 166  * Software wraparound for framenumbers.
 167  */
 168 static inline u16 frame_add(u16 a, u16 b)
 169 {
 170         return (a + b) & HOST_FRAME_MASK;
 171 }
 172
 173 /*
 174  * frame_after - is frame a after frame b
 175  */
 176 static inline int frame_after(u16 a, u16 b)
 177 {
 178         return ((HOST_FRAME_MASK + a - b) & HOST_FRAME_MASK) <
 179             (HOST_FRAME_MASK / 2);
 180 }
 181
 182 /*
 183  * frame_after_eq - is frame a after or equal to frame b
 184  */
 185 static inline int frame_after_eq(u16 a, u16 b)
 186 {
 187         return ((HOST_FRAME_MASK + 1 + a - b) & HOST_FRAME_MASK) <
 188             (HOST_FRAME_MASK / 2);
 189 }
 190
 191 /* -------------------------------------------------------------------------- */
 192
 193 /*
 194  * c67x00_release_urb - remove link from all tds to this urb
 195  * Disconnects the urb from it's tds, so that it can be given back.
 196  * pre: urb->hcpriv != NULL
 197  */
 198 static void c67x00_release_urb(struct c67x00_hcd *c67x00, struct urb *urb)
 199 {
 200         struct c67x00_td *td;
 201         struct c67x00_urb_priv *urbp;
 202
 203         BUG_ON(!urb);
 204
 205         c67x00->urb_count--;
 206
 207         if (usb_pipetype(urb->pipe) == PIPE_ISOCHRONOUS) {
 208                 c67x00->urb_iso_count--;
 209                 if (c67x00->urb_iso_count == 0)
 210                         c67x00->max_frame_bw = MAX_FRAME_BW_STD;
 211         }
 212
 213         /* TODO this might be not so efficient when we've got many urbs!
 214          * Alternatives:
 215          *   * only clear when needed
 216          *   * keep a list of tds with each urbp
 217          */
 218         list_for_each_entry(td, &c67x00->td_list, td_list)
 219                 if (urb == td->urb)
 220                         td->urb = NULL;
 221
 222         urbp = urb->hcpriv;
 223         urb->hcpriv = NULL;
 224         list_del(&urbp->hep_node);
 225         kfree(urbp);
 226 }
 227
 228 /* -------------------------------------------------------------------------- */
 229
 230 static struct c67x00_ep_data *
 231 c67x00_ep_data_alloc(struct c67x00_hcd *c67x00, struct urb *urb)
 232 {
 233         struct usb_host_endpoint *hep = urb->ep;
 234         struct c67x00_ep_data *ep_data;
 235         int type;
 236
 237         c67x00->current_frame = c67x00_get_current_frame_number(c67x00);
 238
 239         /* Check if endpoint already has a c67x00_ep_data struct allocated */
 240         if (hep->hcpriv) {
 241                 ep_data = hep->hcpriv;
 242                 if (frame_after(c67x00->current_frame, ep_data->next_frame))
 243                         ep_data->next_frame =
 244                             frame_add(c67x00->current_frame, 1);
 245                 return hep->hcpriv;
 246         }
 247
 248         /* Allocate and initialize a new c67x00 endpoint data structure */
 249         ep_data = kzalloc(sizeof(*ep_data), GFP_ATOMIC);
 250         if (!ep_data)
 251                 return NULL;
 252
 253         INIT_LIST_HEAD(&ep_data->queue);
 254         INIT_LIST_HEAD(&ep_data->node);
 255         ep_data->hep = hep;
 256
 257         /* hold a reference to udev as long as this endpoint lives,
 258          * this is needed to possibly fix the data toggle */
 259         ep_data->dev = usb_get_dev(urb->dev);
 260         hep->hcpriv = ep_data;
 261
 262         /* For ISOC and INT endpoints, start ASAP: */
 263         ep_data->next_frame = frame_add(c67x00->current_frame, 1);
 264
 265         /* Add the endpoint data to one of the pipe lists; must be added
 266            in order of endpoint address */
 267         type = usb_pipetype(urb->pipe);
 268         if (list_empty(&ep_data->node)) {
 269                 list_add(&ep_data->node, &c67x00->list[type]);
 270         } else {
 271                 struct c67x00_ep_data *prev;
 272
 273                 list_for_each_entry(prev, &c67x00->list[type], node) {
 274                         if (prev->hep->desc.bEndpointAddress >
 275                             hep->desc.bEndpointAddress) {
 276                                 list_add(&ep_data->node, prev->node.prev);
 277                                 break;
 278                         }
 279                 }
 280         }
 281
 282         return ep_data;
 283 }
 284
 285 static int c67x00_ep_data_free(struct usb_host_endpoint *hep)
 286 {
 287         struct c67x00_ep_data *ep_data = hep->hcpriv;
 288
 289         if (!ep_data)
 290                 return 0;
 291
 292         if (!list_empty(&ep_data->queue))
 293                 return -EBUSY;
 294
 295         usb_put_dev(ep_data->dev);
 296         list_del(&ep_data->queue);
 297         list_del(&ep_data->node);
 298
 299         kfree(ep_data);
 300         hep->hcpriv = NULL;
 301
 302         return 0;
 303 }
 304
 305 void c67x00_endpoint_disable(struct usb_hcd *hcd, struct usb_host_endpoint *ep)
 306 {
 307         struct c67x00_hcd *c67x00 = hcd_to_c67x00_hcd(hcd);
 308         unsigned long flags;
 309
 310         if (!list_empty(&ep->urb_list))
 311                 dev_warn(c67x00_hcd_dev(c67x00), "error: urb list not empty\n");
 312
 313         spin_lock_irqsave(&c67x00->lock, flags);
 314
 315         /* loop waiting for all transfers in the endpoint queue to complete */
 316         while (c67x00_ep_data_free(ep)) {
 317                 /* Drop the lock so we can sleep waiting for the hardware */
 318                 spin_unlock_irqrestore(&c67x00->lock, flags);
 319
 320                 /* it could happen that we reinitialize this completion, while
 321                  * somebody was waiting for that completion.  The timeout and
 322                  * while loop handle such cases, but this might be improved */
 323                 reinit_completion(&c67x00->endpoint_disable);
 324                 c67x00_sched_kick(c67x00);
 325                 wait_for_completion_timeout(&c67x00->endpoint_disable, 1 * HZ);
 326
 327                 spin_lock_irqsave(&c67x00->lock, flags);
 328         }
 329
 330         spin_unlock_irqrestore(&c67x00->lock, flags);
 331 }
 332
 333 /* -------------------------------------------------------------------------- */
 334
 335 static inline int get_root_port(struct usb_device *dev)
 336 {
 337         while (dev->parent->parent)
 338                 dev = dev->parent;
 339         return dev->portnum;
 340 }
 341
 342 int c67x00_urb_enqueue(struct usb_hcd *hcd,
 343                        struct urb *urb, gfp_t mem_flags)
 344 {
 345         int ret;
 346         unsigned long flags;
 347         struct c67x00_urb_priv *urbp;
 348         struct c67x00_hcd *c67x00 = hcd_to_c67x00_hcd(hcd);
 349         int port = get_root_port(urb->dev)-1;
 350
 351         /* Allocate and initialize urb private data */
 352         urbp = kzalloc(sizeof(*urbp), mem_flags);
 353         if (!urbp) {
 354                 ret = -ENOMEM;
 355                 goto err_urbp;
 356         }
 357
 358         spin_lock_irqsave(&c67x00->lock, flags);
 359
 360         /* Make sure host controller is running */
 361         if (!HC_IS_RUNNING(hcd->state)) {
 362                 ret = -ENODEV;
 363                 goto err_not_linked;
 364         }
 365
 366         ret = usb_hcd_link_urb_to_ep(hcd, urb);
 367         if (ret)
 368                 goto err_not_linked;
 369
 370         INIT_LIST_HEAD(&urbp->hep_node);
 371         urbp->urb = urb;
 372         urbp->port = port;
 373
 374         urbp->ep_data = c67x00_ep_data_alloc(c67x00, urb);
 375
 376         if (!urbp->ep_data) {
 377                 ret = -ENOMEM;
 378                 goto err_epdata;
 379         }
 380
 381         /* TODO claim bandwidth with usb_claim_bandwidth?
 382          * also release it somewhere! */
 383
 384         urb->hcpriv = urbp;
 385
 386         urb->actual_length = 0; /* Nothing received/transmitted yet */
 387
 388         switch (usb_pipetype(urb->pipe)) {
 389         case PIPE_CONTROL:
 390                 urb->interval = SETUP_STAGE;
 391                 break;
 392         case PIPE_INTERRUPT:
 393                 break;
 394         case PIPE_BULK:
 395                 break;
 396         case PIPE_ISOCHRONOUS:
 397                 if (c67x00->urb_iso_count == 0)
 398                         c67x00->max_frame_bw = MAX_FRAME_BW_ISO;
 399                 c67x00->urb_iso_count++;
 400                 /* Assume always URB_ISO_ASAP, FIXME */
 401                 if (list_empty(&urbp->ep_data->queue))
 402                         urb->start_frame = urbp->ep_data->next_frame;
 403                 else {
 404                         /* Go right after the last one */
 405                         struct urb *last_urb;
 406
 407                         last_urb = list_entry(urbp->ep_data->queue.prev,
 408                                               struct c67x00_urb_priv,
 409                                               hep_node)->urb;
 410                         urb->start_frame =
 411                             frame_add(last_urb->start_frame,
 412                                       last_urb->number_of_packets *
 413                                       last_urb->interval);
 414                 }
 415                 urbp->cnt = 0;
 416                 break;
 417         }
 418
 419         /* Add the URB to the endpoint queue */
 420         list_add_tail(&urbp->hep_node, &urbp->ep_data->queue);
 421
 422         /* If this is the only URB, kick start the controller */
 423         if (!c67x00->urb_count++)
 424                 c67x00_ll_hpi_enable_sofeop(c67x00->sie);
 425
 426         c67x00_sched_kick(c67x00);
 427         spin_unlock_irqrestore(&c67x00->lock, flags);
 428
 429         return 0;
 430
 431 err_epdata:
 432         usb_hcd_unlink_urb_from_ep(hcd, urb);
 433 err_not_linked:
 434         spin_unlock_irqrestore(&c67x00->lock, flags);
 435         kfree(urbp);
 436 err_urbp:
 437
 438         return ret;
 439 }
 440
 441 int c67x00_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 442 {
 443         struct c67x00_hcd *c67x00 = hcd_to_c67x00_hcd(hcd);
 444         unsigned long flags;
 445         int rc;
 446
 447         spin_lock_irqsave(&c67x00->lock, flags);
 448         rc = usb_hcd_check_unlink_urb(hcd, urb, status);
 449         if (rc)
 450                 goto done;
 451
 452         c67x00_release_urb(c67x00, urb);
 453         usb_hcd_unlink_urb_from_ep(hcd, urb);
 454
 455         spin_unlock(&c67x00->lock);
 456         usb_hcd_giveback_urb(hcd, urb, status);
 457         spin_lock(&c67x00->lock);
 458
 459         spin_unlock_irqrestore(&c67x00->lock, flags);
 460
 461         return 0;
 462
 463  done:
 464         spin_unlock_irqrestore(&c67x00->lock, flags);
 465         return rc;
 466 }
 467
 468 /* -------------------------------------------------------------------------- */
 469
 470 /*
 471  * pre: c67x00 locked, urb unlocked
 472  */
 473 static void
 474 c67x00_giveback_urb(struct c67x00_hcd *c67x00, struct urb *urb, int status)
 475 {
 476         struct c67x00_urb_priv *urbp;
 477
 478         if (!urb)
 479                 return;
 480
 481         urbp = urb->hcpriv;
 482         urbp->status = status;
 483
 484         list_del_init(&urbp->hep_node);
 485
 486         c67x00_release_urb(c67x00, urb);
 487         usb_hcd_unlink_urb_from_ep(c67x00_hcd_to_hcd(c67x00), urb);
 488         spin_unlock(&c67x00->lock);
 489         usb_hcd_giveback_urb(c67x00_hcd_to_hcd(c67x00), urb, status);
 490         spin_lock(&c67x00->lock);
 491 }
 492
 493 /* -------------------------------------------------------------------------- */
 494
 495 static int c67x00_claim_frame_bw(struct c67x00_hcd *c67x00, struct urb *urb,
 496                                  int len, int periodic)
 497 {
 498         struct c67x00_urb_priv *urbp = urb->hcpriv;
 499         int bit_time;
 500
 501         /* According to the C67x00 BIOS user manual, page 3-18,19, the
 502          * following calculations provide the full speed bit times for
 503          * a transaction.
 504          *
 505          * FS(in)       = 112.5 +  9.36*BC + HOST_DELAY
 506          * FS(in,iso)   =  90.5 +  9.36*BC + HOST_DELAY
 507          * FS(out)      = 112.5 +  9.36*BC + HOST_DELAY
 508          * FS(out,iso)  =  78.4 +  9.36*BC + HOST_DELAY
 509          * LS(in)       = 802.4 + 75.78*BC + HOST_DELAY
 510          * LS(out)      = 802.6 + 74.67*BC + HOST_DELAY
 511          *
 512          * HOST_DELAY == 106 for the c67200 and c67300.
 513          */
 514
 515         /* make calculations in 1/100 bit times to maintain resolution */
 516         if (urbp->ep_data->dev->speed == USB_SPEED_LOW) {
 517                 /* Low speed pipe */
 518                 if (usb_pipein(urb->pipe))
 519                         bit_time = 80240 + 7578*len;
 520                 else
 521                         bit_time = 80260 + 7467*len;
 522         } else {
 523                 /* FS pipes */
 524                 if (usb_pipeisoc(urb->pipe))
 525                         bit_time = usb_pipein(urb->pipe) ? 9050 : 7840;
 526                 else
 527                         bit_time = 11250;
 528                 bit_time += 936*len;
 529         }
 530
 531         /* Scale back down to integer bit times.  Use a host delay of 106.
 532          * (this is the only place it is used) */
 533         bit_time = ((bit_time+50) / 100) + 106;
 534
 535         if (unlikely(bit_time + c67x00->bandwidth_allocated >=
 536                      c67x00->max_frame_bw))
 537                 return -EMSGSIZE;
 538
 539         if (unlikely(c67x00->next_td_addr + CY_TD_SIZE >=
 540                      c67x00->td_base_addr + SIE_TD_SIZE))
 541                 return -EMSGSIZE;
 542
 543         if (unlikely(c67x00->next_buf_addr + len >=
 544                      c67x00->buf_base_addr + SIE_TD_BUF_SIZE))
 545                 return -EMSGSIZE;
 546
 547         if (periodic) {
 548                 if (unlikely(bit_time + c67x00->periodic_bw_allocated >=
 549                              MAX_PERIODIC_BW(c67x00->max_frame_bw)))
 550                         return -EMSGSIZE;
 551                 c67x00->periodic_bw_allocated += bit_time;
 552         }
 553
 554         c67x00->bandwidth_allocated += bit_time;
 555         return 0;
 556 }
 557
 558 /* -------------------------------------------------------------------------- */
 559
 560 /*
 561  * td_addr and buf_addr must be word aligned
 562  */
 563 static int c67x00_create_td(struct c67x00_hcd *c67x00, struct urb *urb,
 564                             void *data, int len, int pid, int toggle,
 565                             unsigned long privdata)
 566 {
 567         struct c67x00_td *td;
 568         struct c67x00_urb_priv *urbp = urb->hcpriv;
 569         const __u8 active_flag = 1, retry_cnt = 3;
 570         __u8 cmd = 0;
 571         int tt = 0;
 572
 573         if (c67x00_claim_frame_bw(c67x00, urb, len, usb_pipeisoc(urb->pipe)
 574                                   || usb_pipeint(urb->pipe)))
 575                 return -EMSGSIZE;       /* Not really an error, but expected */
 576
 577         td = kzalloc(sizeof(*td), GFP_ATOMIC);
 578         if (!td)
 579                 return -ENOMEM;
 580
 581         td->pipe = urb->pipe;
 582         td->ep_data = urbp->ep_data;
 583
 584         if ((td_udev(td)->speed == USB_SPEED_LOW) &&
 585             !(c67x00->low_speed_ports & (1 << urbp->port)))
 586                 cmd |= PREAMBLE_EN;
 587
 588         switch (usb_pipetype(td->pipe)) {
 589         case PIPE_ISOCHRONOUS:
 590                 tt = TT_ISOCHRONOUS;
 591                 cmd |= ISO_EN;
 592                 break;
 593         case PIPE_CONTROL:
 594                 tt = TT_CONTROL;
 595                 break;
 596         case PIPE_BULK:
 597                 tt = TT_BULK;
 598                 break;
 599         case PIPE_INTERRUPT:
 600                 tt = TT_INTERRUPT;
 601                 break;
 602         }
 603
 604         if (toggle)
 605                 cmd |= SEQ_SEL;
 606
 607         cmd |= ARM_EN;
 608
 609         /* SW part */
 610         td->td_addr = c67x00->next_td_addr;
 611         c67x00->next_td_addr = c67x00->next_td_addr + CY_TD_SIZE;
 612
 613         /* HW part */
 614         td->ly_base_addr = __cpu_to_le16(c67x00->next_buf_addr);
 615         td->port_length = __cpu_to_le16((c67x00->sie->sie_num << 15) |
 616                                         (urbp->port << 14) | (len & 0x3FF));
 617         td->pid_ep = ((pid & 0xF) << TD_PIDEP_OFFSET) |
 618             (usb_pipeendpoint(td->pipe) & 0xF);
 619         td->dev_addr = usb_pipedevice(td->pipe) & 0x7F;
 620         td->ctrl_reg = cmd;
 621         td->status = 0;
 622         td->retry_cnt = (tt << TT_OFFSET) | (active_flag << 4) | retry_cnt;
 623         td->residue = 0;
 624         td->next_td_addr = __cpu_to_le16(c67x00->next_td_addr);
 625
 626         /* SW part */
 627         td->data = data;
 628         td->urb = urb;
 629         td->privdata = privdata;
 630
 631         c67x00->next_buf_addr += (len + 1) & ~0x01;     /* properly align */
 632
 633         list_add_tail(&td->td_list, &c67x00->td_list);
 634         return 0;
 635 }
 636
 637 static inline void c67x00_release_td(struct c67x00_td *td)
 638 {
 639         list_del_init(&td->td_list);
 640         kfree(td);
 641 }
 642
 643 /* -------------------------------------------------------------------------- */
 644
 645 static int c67x00_add_data_urb(struct c67x00_hcd *c67x00, struct urb *urb)
 646 {
 647         int remaining;
 648         int toggle;
 649         int pid;
 650         int ret = 0;
 651         int maxps;
 652         int need_empty;
 653
 654         toggle = usb_gettoggle(urb->dev, usb_pipeendpoint(urb->pipe),
 655                                usb_pipeout(urb->pipe));
 656         remaining = urb->transfer_buffer_length - urb->actual_length;
 657
 658         maxps = usb_maxpacket(urb->dev, urb->pipe);
 659
 660         need_empty = (urb->transfer_flags & URB_ZERO_PACKET) &&
 661             usb_pipeout(urb->pipe) && !(remaining % maxps);
 662
 663         while (remaining || need_empty) {
 664                 int len;
 665                 char *td_buf;
 666
 667                 len = (remaining > maxps) ? maxps : remaining;
 668                 if (!len)
 669                         need_empty = 0;
 670
 671                 pid = usb_pipeout(urb->pipe) ? USB_PID_OUT : USB_PID_IN;
 672                 td_buf = urb->transfer_buffer + urb->transfer_buffer_length -
 673                     remaining;
 674                 ret = c67x00_create_td(c67x00, urb, td_buf, len, pid, toggle,
 675                                        DATA_STAGE);
 676                 if (ret)
 677                         return ret;     /* td wasn't created */
 678
 679                 toggle ^= 1;
 680                 remaining -= len;
 681                 if (usb_pipecontrol(urb->pipe))
 682                         break;
 683         }
 684
 685         return 0;
 686 }
 687
 688 /*
 689  * return 0 in case more bandwidth is available, else errorcode
 690  */
 691 static int c67x00_add_ctrl_urb(struct c67x00_hcd *c67x00, struct urb *urb)
 692 {
 693         int ret;
 694         int pid;
 695
 696         switch (urb->interval) {
 697         default:
 698         case SETUP_STAGE:
 699                 ret = c67x00_create_td(c67x00, urb, urb->setup_packet,
 700                                        8, USB_PID_SETUP, 0, SETUP_STAGE);
 701                 if (ret)
 702                         return ret;
 703                 urb->interval = SETUP_STAGE;
 704                 usb_settoggle(urb->dev, usb_pipeendpoint(urb->pipe),
 705                               usb_pipeout(urb->pipe), 1);
 706                 break;
 707         case DATA_STAGE:
 708                 if (urb->transfer_buffer_length) {
 709                         ret = c67x00_add_data_urb(c67x00, urb);
 710                         if (ret)
 711                                 return ret;
 712                         break;
 713                 }
 714                 fallthrough;
 715         case STATUS_STAGE:
 716                 pid = !usb_pipeout(urb->pipe) ? USB_PID_OUT : USB_PID_IN;
 717                 ret = c67x00_create_td(c67x00, urb, NULL, 0, pid, 1,
 718                                        STATUS_STAGE);
 719                 if (ret)
 720                         return ret;
 721                 break;
 722         }
 723
 724         return 0;
 725 }
 726
 727 /*
 728  * return 0 in case more bandwidth is available, else errorcode
 729  */
 730 static int c67x00_add_int_urb(struct c67x00_hcd *c67x00, struct urb *urb)
 731 {
 732         struct c67x00_urb_priv *urbp = urb->hcpriv;
 733
 734         if (frame_after_eq(c67x00->current_frame, urbp->ep_data->next_frame)) {
 735                 urbp->ep_data->next_frame =
 736                     frame_add(urbp->ep_data->next_frame, urb->interval);
 737                 return c67x00_add_data_urb(c67x00, urb);
 738         }
 739         return 0;
 740 }
 741
 742 static int c67x00_add_iso_urb(struct c67x00_hcd *c67x00, struct urb *urb)
 743 {
 744         struct c67x00_urb_priv *urbp = urb->hcpriv;
 745
 746         if (frame_after_eq(c67x00->current_frame, urbp->ep_data->next_frame)) {
 747                 char *td_buf;
 748                 int len, pid, ret;
 749
 750                 BUG_ON(urbp->cnt >= urb->number_of_packets);
 751
 752                 td_buf = urb->transfer_buffer +
 753                     urb->iso_frame_desc[urbp->cnt].offset;
 754                 len = urb->iso_frame_desc[urbp->cnt].length;
 755                 pid = usb_pipeout(urb->pipe) ? USB_PID_OUT : USB_PID_IN;
 756
 757                 ret = c67x00_create_td(c67x00, urb, td_buf, len, pid, 0,
 758                                        urbp->cnt);
 759                 if (ret) {
 760                         dev_dbg(c67x00_hcd_dev(c67x00), "create failed: %d\n",
 761                                 ret);
 762                         urb->iso_frame_desc[urbp->cnt].actual_length = 0;
 763                         urb->iso_frame_desc[urbp->cnt].status = ret;
 764                         if (urbp->cnt + 1 == urb->number_of_packets)
 765                                 c67x00_giveback_urb(c67x00, urb, 0);
 766                 }
 767
 768                 urbp->ep_data->next_frame =
 769                     frame_add(urbp->ep_data->next_frame, urb->interval);
 770                 urbp->cnt++;
 771         }
 772         return 0;
 773 }
 774
 775 /* -------------------------------------------------------------------------- */
 776
 777 static void c67x00_fill_from_list(struct c67x00_hcd *c67x00, int type,
 778                                   int (*add)(struct c67x00_hcd *, struct urb *))
 779 {
 780         struct c67x00_ep_data *ep_data;
 781         struct urb *urb;
 782
 783         /* traverse every endpoint on the list */
 784         list_for_each_entry(ep_data, &c67x00->list[type], node) {
 785                 if (!list_empty(&ep_data->queue)) {
 786                         /* and add the first urb */
 787                         /* isochronous transfer rely on this */
 788                         urb = list_entry(ep_data->queue.next,
 789                                          struct c67x00_urb_priv,
 790                                          hep_node)->urb;
 791                         add(c67x00, urb);
 792                 }
 793         }
 794 }
 795
 796 static void c67x00_fill_frame(struct c67x00_hcd *c67x00)
 797 {
 798         struct c67x00_td *td, *ttd;
 799
 800         /* Check if we can proceed */
 801         if (!list_empty(&c67x00->td_list)) {
 802                 dev_warn(c67x00_hcd_dev(c67x00),
 803                          "TD list not empty! This should not happen!\n");
 804                 list_for_each_entry_safe(td, ttd, &c67x00->td_list, td_list) {
 805                         dbg_td(c67x00, td, "Unprocessed td");
 806                         c67x00_release_td(td);
 807                 }
 808         }
 809
 810         /* Reinitialize variables */
 811         c67x00->bandwidth_allocated = 0;
 812         c67x00->periodic_bw_allocated = 0;
 813
 814         c67x00->next_td_addr = c67x00->td_base_addr;
 815         c67x00->next_buf_addr = c67x00->buf_base_addr;
 816
 817         /* Fill the list */
 818         c67x00_fill_from_list(c67x00, PIPE_ISOCHRONOUS, c67x00_add_iso_urb);
 819         c67x00_fill_from_list(c67x00, PIPE_INTERRUPT, c67x00_add_int_urb);
 820         c67x00_fill_from_list(c67x00, PIPE_CONTROL, c67x00_add_ctrl_urb);
 821         c67x00_fill_from_list(c67x00, PIPE_BULK, c67x00_add_data_urb);
 822 }
 823
 824 /* -------------------------------------------------------------------------- */
 825
 826 /*
 827  * Get TD from C67X00
 828  */
 829 static inline void
 830 c67x00_parse_td(struct c67x00_hcd *c67x00, struct c67x00_td *td)
 831 {
 832         c67x00_ll_read_mem_le16(c67x00->sie->dev,
 833                                 td->td_addr, td, CY_TD_SIZE);
 834
 835         if (usb_pipein(td->pipe) && td_actual_bytes(td))
 836                 c67x00_ll_read_mem_le16(c67x00->sie->dev, td_ly_base_addr(td),
 837                                         td->data, td_actual_bytes(td));
 838 }
 839
 840 static int c67x00_td_to_error(struct c67x00_hcd *c67x00, struct c67x00_td *td)
 841 {
 842         if (td->status & TD_STATUSMASK_ERR) {
 843                 dbg_td(c67x00, td, "ERROR_FLAG");
 844                 return -EILSEQ;
 845         }
 846         if (td->status & TD_STATUSMASK_STALL) {
 847                 /* dbg_td(c67x00, td, "STALL"); */
 848                 return -EPIPE;
 849         }
 850         if (td->status & TD_STATUSMASK_TMOUT) {
 851                 dbg_td(c67x00, td, "TIMEOUT");
 852                 return -ETIMEDOUT;
 853         }
 854
 855         return 0;
 856 }
 857
 858 static inline int c67x00_end_of_data(struct c67x00_td *td)
 859 {
 860         int maxps, need_empty, remaining;
 861         struct urb *urb = td->urb;
 862         int act_bytes;
 863
 864         act_bytes = td_actual_bytes(td);
 865
 866         if (unlikely(!act_bytes))
 867                 return 1;       /* This was an empty packet */
 868
 869         maxps = usb_maxpacket(td_udev(td), td->pipe);
 870
 871         if (unlikely(act_bytes < maxps))
 872                 return 1;       /* Smaller then full packet */
 873
 874         remaining = urb->transfer_buffer_length - urb->actual_length;
 875         need_empty = (urb->transfer_flags & URB_ZERO_PACKET) &&
 876             usb_pipeout(urb->pipe) && !(remaining % maxps);
 877
 878         if (unlikely(!remaining && !need_empty))
 879                 return 1;
 880
 881         return 0;
 882 }
 883
 884 /* -------------------------------------------------------------------------- */
 885
 886 /* Remove all td's from the list which come
 887  * after last_td and are meant for the same pipe.
 888  * This is used when a short packet has occurred */
 889 static inline void c67x00_clear_pipe(struct c67x00_hcd *c67x00,
 890                                      struct c67x00_td *last_td)
 891 {
 892         struct c67x00_td *td, *tmp;
 893         td = last_td;
 894         tmp = last_td;
 895         while (td->td_list.next != &c67x00->td_list) {
 896                 td = list_entry(td->td_list.next, struct c67x00_td, td_list);
 897                 if (td->pipe == last_td->pipe) {
 898                         c67x00_release_td(td);
 899                         td = tmp;
 900                 }
 901                 tmp = td;
 902         }
 903 }
 904
 905 /* -------------------------------------------------------------------------- */
 906
 907 static void c67x00_handle_successful_td(struct c67x00_hcd *c67x00,
 908                                         struct c67x00_td *td)
 909 {
 910         struct urb *urb = td->urb;
 911
 912         if (!urb)
 913                 return;
 914
 915         urb->actual_length += td_actual_bytes(td);
 916
 917         switch (usb_pipetype(td->pipe)) {
 918                 /* isochronous tds are handled separately */
 919         case PIPE_CONTROL:
 920                 switch (td->privdata) {
 921                 case SETUP_STAGE:
 922                         urb->interval =
 923                             urb->transfer_buffer_length ?
 924                             DATA_STAGE : STATUS_STAGE;
 925                         /* Don't count setup_packet with normal data: */
 926                         urb->actual_length = 0;
 927                         break;
 928
 929                 case DATA_STAGE:
 930                         if (c67x00_end_of_data(td)) {
 931                                 urb->interval = STATUS_STAGE;
 932                                 c67x00_clear_pipe(c67x00, td);
 933                         }
 934                         break;
 935
 936                 case STATUS_STAGE:
 937                         urb->interval = 0;
 938                         c67x00_giveback_urb(c67x00, urb, 0);
 939                         break;
 940                 }
 941                 break;
 942
 943         case PIPE_INTERRUPT:
 944         case PIPE_BULK:
 945                 if (unlikely(c67x00_end_of_data(td))) {
 946                         c67x00_clear_pipe(c67x00, td);
 947                         c67x00_giveback_urb(c67x00, urb, 0);
 948                 }
 949                 break;
 950         }
 951 }
 952
 953 static void c67x00_handle_isoc(struct c67x00_hcd *c67x00, struct c67x00_td *td)
 954 {
 955         struct urb *urb = td->urb;
 956         int cnt;
 957
 958         if (!urb)
 959                 return;
 960
 961         cnt = td->privdata;
 962
 963         if (td->status & TD_ERROR_MASK)
 964                 urb->error_count++;
 965
 966         urb->iso_frame_desc[cnt].actual_length = td_actual_bytes(td);
 967         urb->iso_frame_desc[cnt].status = c67x00_td_to_error(c67x00, td);
 968         if (cnt + 1 == urb->number_of_packets)  /* Last packet */
 969                 c67x00_giveback_urb(c67x00, urb, 0);
 970 }
 971
 972 /* -------------------------------------------------------------------------- */
 973
 974 /*
 975  * c67x00_check_td_list - handle tds which have been processed by the c67x00
 976  * pre: current_td == 0
 977  */
 978 static inline void c67x00_check_td_list(struct c67x00_hcd *c67x00)
 979 {
 980         struct c67x00_td *td, *tmp;
 981         struct urb *urb;
 982         int ack_ok;
 983         int clear_endpoint;
 984
 985         list_for_each_entry_safe(td, tmp, &c67x00->td_list, td_list) {
 986                 /* get the TD */
 987                 c67x00_parse_td(c67x00, td);
 988                 urb = td->urb;  /* urb can be NULL! */
 989                 ack_ok = 0;
 990                 clear_endpoint = 1;
 991
 992                 /* Handle isochronous transfers separately */
 993                 if (usb_pipeisoc(td->pipe)) {
 994                         clear_endpoint = 0;
 995                         c67x00_handle_isoc(c67x00, td);
 996                         goto cont;
 997                 }
 998
 999                 /* When an error occurs, all td's for that pipe go into an
1000                  * inactive state. This state matches successful transfers so
1001                  * we must make sure not to service them. */
1002                 if (td->status & TD_ERROR_MASK) {
1003                         c67x00_giveback_urb(c67x00, urb,
1004                                             c67x00_td_to_error(c67x00, td));
1005                         goto cont;
1006                 }
1007
1008                 if ((td->status & TD_STATUSMASK_NAK) || !td_sequence_ok(td) ||
1009                     !td_acked(td))
1010                         goto cont;
1011
1012                 /* Sequence ok and acked, don't need to fix toggle */
1013                 ack_ok = 1;
1014
1015                 if (unlikely(td->status & TD_STATUSMASK_OVF)) {
1016                         if (td_residue(td) & TD_RESIDUE_OVERFLOW) {
1017                                 /* Overflow */
1018                                 c67x00_giveback_urb(c67x00, urb, -EOVERFLOW);
1019                                 goto cont;
1020                         }
1021                 }
1022
1023                 clear_endpoint = 0;
1024                 c67x00_handle_successful_td(c67x00, td);
1025
1026 cont:
1027                 if (clear_endpoint)
1028                         c67x00_clear_pipe(c67x00, td);
1029                 if (ack_ok)
1030                         usb_settoggle(td_udev(td), usb_pipeendpoint(td->pipe),
1031                                       usb_pipeout(td->pipe),
1032                                       !(td->ctrl_reg & SEQ_SEL));
1033                 /* next in list could have been removed, due to clear_pipe! */
1034                 tmp = list_entry(td->td_list.next, typeof(*td), td_list);
1035                 c67x00_release_td(td);
1036         }
1037 }
1038
1039 /* -------------------------------------------------------------------------- */
1040
1041 static inline int c67x00_all_tds_processed(struct c67x00_hcd *c67x00)
1042 {
1043         /* If all tds are processed, we can check the previous frame (if
1044          * there was any) and start our next frame.
1045          */
1046         return !c67x00_ll_husb_get_current_td(c67x00->sie);
1047 }
1048
1049 /*
1050  * Send td to C67X00
1051  */
1052 static void c67x00_send_td(struct c67x00_hcd *c67x00, struct c67x00_td *td)
1053 {
1054         int len = td_length(td);
1055
1056         if (len && ((td->pid_ep & TD_PIDEPMASK_PID) != TD_PID_IN))
1057                 c67x00_ll_write_mem_le16(c67x00->sie->dev, td_ly_base_addr(td),
1058                                          td->data, len);
1059
1060         c67x00_ll_write_mem_le16(c67x00->sie->dev,
1061                                  td->td_addr, td, CY_TD_SIZE);
1062 }
1063
1064 static void c67x00_send_frame(struct c67x00_hcd *c67x00)
1065 {
1066         struct c67x00_td *td;
1067
1068         if (list_empty(&c67x00->td_list))
1069                 dev_warn(c67x00_hcd_dev(c67x00),
1070                          "%s: td list should not be empty here!\n",
1071                          __func__);
1072
1073         list_for_each_entry(td, &c67x00->td_list, td_list) {
1074                 if (td->td_list.next == &c67x00->td_list)
1075                         td->next_td_addr = 0;   /* Last td in list */
1076
1077                 c67x00_send_td(c67x00, td);
1078         }
1079
1080         c67x00_ll_husb_set_current_td(c67x00->sie, c67x00->td_base_addr);
1081 }
1082
1083 /* -------------------------------------------------------------------------- */
1084
1085 /*
1086  * c67x00_do_work - Schedulers state machine
1087  */
1088 static void c67x00_do_work(struct c67x00_hcd *c67x00)
1089 {
1090         spin_lock(&c67x00->lock);
1091         /* Make sure all tds are processed */
1092         if (!c67x00_all_tds_processed(c67x00))
1093                 goto out;
1094
1095         c67x00_check_td_list(c67x00);
1096
1097         /* no td's are being processed (current == 0)
1098          * and all have been "checked" */
1099         complete(&c67x00->endpoint_disable);
1100
1101         if (!list_empty(&c67x00->td_list))
1102                 goto out;
1103
1104         c67x00->current_frame = c67x00_get_current_frame_number(c67x00);
1105         if (c67x00->current_frame == c67x00->last_frame)
1106                 goto out;       /* Don't send tds in same frame */
1107         c67x00->last_frame = c67x00->current_frame;
1108
1109         /* If no urbs are scheduled, our work is done */
1110         if (!c67x00->urb_count) {
1111                 c67x00_ll_hpi_disable_sofeop(c67x00->sie);
1112                 goto out;
1113         }
1114
1115         c67x00_fill_frame(c67x00);
1116         if (!list_empty(&c67x00->td_list))
1117                 /* TD's have been added to the frame */
1118                 c67x00_send_frame(c67x00);
1119
1120  out:
1121         spin_unlock(&c67x00->lock);
1122 }
1123
1124 /* -------------------------------------------------------------------------- */
1125
1126 static void c67x00_sched_work(struct work_struct *work)
1127 {
1128         struct c67x00_hcd *c67x00;
1129
1130         c67x00 = container_of(work, struct c67x00_hcd, work);
1131         c67x00_do_work(c67x00);
1132 }
1133
1134 void c67x00_sched_kick(struct c67x00_hcd *c67x00)
1135 {
1136         queue_work(system_highpri_wq, &c67x00->work);
1137 }
1138
1139 int c67x00_sched_start_scheduler(struct c67x00_hcd *c67x00)
1140 {
1141         INIT_WORK(&c67x00->work, c67x00_sched_work);
1142         return 0;
1143 }
1144
1145 void c67x00_sched_stop_scheduler(struct c67x00_hcd *c67x00)
1146 {
1147         cancel_work_sync(&c67x00->work);
1148 }