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, usb_pipeout(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                 }               /* else fallthrough */
 714         case STATUS_STAGE:
 715                 pid = !usb_pipeout(urb->pipe) ? USB_PID_OUT : USB_PID_IN;
 716                 ret = c67x00_create_td(c67x00, urb, NULL, 0, pid, 1,
 717                                        STATUS_STAGE);
 718                 if (ret)
 719                         return ret;
 720                 break;
 721         }
 722
 723         return 0;
 724 }
 725
 726 /*
 727  * return 0 in case more bandwidth is available, else errorcode
 728  */
 729 static int c67x00_add_int_urb(struct c67x00_hcd *c67x00, struct urb *urb)
 730 {
 731         struct c67x00_urb_priv *urbp = urb->hcpriv;
 732
 733         if (frame_after_eq(c67x00->current_frame, urbp->ep_data->next_frame)) {
 734                 urbp->ep_data->next_frame =
 735                     frame_add(urbp->ep_data->next_frame, urb->interval);
 736                 return c67x00_add_data_urb(c67x00, urb);
 737         }
 738         return 0;
 739 }
 740
 741 static int c67x00_add_iso_urb(struct c67x00_hcd *c67x00, struct urb *urb)
 742 {
 743         struct c67x00_urb_priv *urbp = urb->hcpriv;
 744
 745         if (frame_after_eq(c67x00->current_frame, urbp->ep_data->next_frame)) {
 746                 char *td_buf;
 747                 int len, pid, ret;
 748
 749                 BUG_ON(urbp->cnt >= urb->number_of_packets);
 750
 751                 td_buf = urb->transfer_buffer +
 752                     urb->iso_frame_desc[urbp->cnt].offset;
 753                 len = urb->iso_frame_desc[urbp->cnt].length;
 754                 pid = usb_pipeout(urb->pipe) ? USB_PID_OUT : USB_PID_IN;
 755
 756                 ret = c67x00_create_td(c67x00, urb, td_buf, len, pid, 0,
 757                                        urbp->cnt);
 758                 if (ret) {
 759                         dev_dbg(c67x00_hcd_dev(c67x00), "create failed: %d\n",
 760                                 ret);
 761                         urb->iso_frame_desc[urbp->cnt].actual_length = 0;
 762                         urb->iso_frame_desc[urbp->cnt].status = ret;
 763                         if (urbp->cnt + 1 == urb->number_of_packets)
 764                                 c67x00_giveback_urb(c67x00, urb, 0);
 765                 }
 766
 767                 urbp->ep_data->next_frame =
 768                     frame_add(urbp->ep_data->next_frame, urb->interval);
 769                 urbp->cnt++;
 770         }
 771         return 0;
 772 }
 773
 774 /* -------------------------------------------------------------------------- */
 775
 776 static void c67x00_fill_from_list(struct c67x00_hcd *c67x00, int type,
 777                                   int (*add)(struct c67x00_hcd *, struct urb *))
 778 {
 779         struct c67x00_ep_data *ep_data;
 780         struct urb *urb;
 781
 782         /* traverse every endpoint on the list */
 783         list_for_each_entry(ep_data, &c67x00->list[type], node) {
 784                 if (!list_empty(&ep_data->queue)) {
 785                         /* and add the first urb */
 786                         /* isochronous transfer rely on this */
 787                         urb = list_entry(ep_data->queue.next,
 788                                          struct c67x00_urb_priv,
 789                                          hep_node)->urb;
 790                         add(c67x00, urb);
 791                 }
 792         }
 793 }
 794
 795 static void c67x00_fill_frame(struct c67x00_hcd *c67x00)
 796 {
 797         struct c67x00_td *td, *ttd;
 798
 799         /* Check if we can proceed */
 800         if (!list_empty(&c67x00->td_list)) {
 801                 dev_warn(c67x00_hcd_dev(c67x00),
 802                          "TD list not empty! This should not happen!\n");
 803                 list_for_each_entry_safe(td, ttd, &c67x00->td_list, td_list) {
 804                         dbg_td(c67x00, td, "Unprocessed td");
 805                         c67x00_release_td(td);
 806                 }
 807         }
 808
 809         /* Reinitialize variables */
 810         c67x00->bandwidth_allocated = 0;
 811         c67x00->periodic_bw_allocated = 0;
 812
 813         c67x00->next_td_addr = c67x00->td_base_addr;
 814         c67x00->next_buf_addr = c67x00->buf_base_addr;
 815
 816         /* Fill the list */
 817         c67x00_fill_from_list(c67x00, PIPE_ISOCHRONOUS, c67x00_add_iso_urb);
 818         c67x00_fill_from_list(c67x00, PIPE_INTERRUPT, c67x00_add_int_urb);
 819         c67x00_fill_from_list(c67x00, PIPE_CONTROL, c67x00_add_ctrl_urb);
 820         c67x00_fill_from_list(c67x00, PIPE_BULK, c67x00_add_data_urb);
 821 }
 822
 823 /* -------------------------------------------------------------------------- */
 824
 825 /**
 826  * Get TD from C67X00
 827  */
 828 static inline void
 829 c67x00_parse_td(struct c67x00_hcd *c67x00, struct c67x00_td *td)
 830 {
 831         c67x00_ll_read_mem_le16(c67x00->sie->dev,
 832                                 td->td_addr, td, CY_TD_SIZE);
 833
 834         if (usb_pipein(td->pipe) && td_actual_bytes(td))
 835                 c67x00_ll_read_mem_le16(c67x00->sie->dev, td_ly_base_addr(td),
 836                                         td->data, td_actual_bytes(td));
 837 }
 838
 839 static int c67x00_td_to_error(struct c67x00_hcd *c67x00, struct c67x00_td *td)
 840 {
 841         if (td->status & TD_STATUSMASK_ERR) {
 842                 dbg_td(c67x00, td, "ERROR_FLAG");
 843                 return -EILSEQ;
 844         }
 845         if (td->status & TD_STATUSMASK_STALL) {
 846                 /* dbg_td(c67x00, td, "STALL"); */
 847                 return -EPIPE;
 848         }
 849         if (td->status & TD_STATUSMASK_TMOUT) {
 850                 dbg_td(c67x00, td, "TIMEOUT");
 851                 return -ETIMEDOUT;
 852         }
 853
 854         return 0;
 855 }
 856
 857 static inline int c67x00_end_of_data(struct c67x00_td *td)
 858 {
 859         int maxps, need_empty, remaining;
 860         struct urb *urb = td->urb;
 861         int act_bytes;
 862
 863         act_bytes = td_actual_bytes(td);
 864
 865         if (unlikely(!act_bytes))
 866                 return 1;       /* This was an empty packet */
 867
 868         maxps = usb_maxpacket(td_udev(td), td->pipe, usb_pipeout(td->pipe));
 869
 870         if (unlikely(act_bytes < maxps))
 871                 return 1;       /* Smaller then full packet */
 872
 873         remaining = urb->transfer_buffer_length - urb->actual_length;
 874         need_empty = (urb->transfer_flags & URB_ZERO_PACKET) &&
 875             usb_pipeout(urb->pipe) && !(remaining % maxps);
 876
 877         if (unlikely(!remaining && !need_empty))
 878                 return 1;
 879
 880         return 0;
 881 }
 882
 883 /* -------------------------------------------------------------------------- */
 884
 885 /* Remove all td's from the list which come
 886  * after last_td and are meant for the same pipe.
 887  * This is used when a short packet has occurred */
 888 static inline void c67x00_clear_pipe(struct c67x00_hcd *c67x00,
 889                                      struct c67x00_td *last_td)
 890 {
 891         struct c67x00_td *td, *tmp;
 892         td = last_td;
 893         tmp = last_td;
 894         while (td->td_list.next != &c67x00->td_list) {
 895                 td = list_entry(td->td_list.next, struct c67x00_td, td_list);
 896                 if (td->pipe == last_td->pipe) {
 897                         c67x00_release_td(td);
 898                         td = tmp;
 899                 }
 900                 tmp = td;
 901         }
 902 }
 903
 904 /* -------------------------------------------------------------------------- */
 905
 906 static void c67x00_handle_successful_td(struct c67x00_hcd *c67x00,
 907                                         struct c67x00_td *td)
 908 {
 909         struct urb *urb = td->urb;
 910
 911         if (!urb)
 912                 return;
 913
 914         urb->actual_length += td_actual_bytes(td);
 915
 916         switch (usb_pipetype(td->pipe)) {
 917                 /* isochronous tds are handled separately */
 918         case PIPE_CONTROL:
 919                 switch (td->privdata) {
 920                 case SETUP_STAGE:
 921                         urb->interval =
 922                             urb->transfer_buffer_length ?
 923                             DATA_STAGE : STATUS_STAGE;
 924                         /* Don't count setup_packet with normal data: */
 925                         urb->actual_length = 0;
 926                         break;
 927
 928                 case DATA_STAGE:
 929                         if (c67x00_end_of_data(td)) {
 930                                 urb->interval = STATUS_STAGE;
 931                                 c67x00_clear_pipe(c67x00, td);
 932                         }
 933                         break;
 934
 935                 case STATUS_STAGE:
 936                         urb->interval = 0;
 937                         c67x00_giveback_urb(c67x00, urb, 0);
 938                         break;
 939                 }
 940                 break;
 941
 942         case PIPE_INTERRUPT:
 943         case PIPE_BULK:
 944                 if (unlikely(c67x00_end_of_data(td))) {
 945                         c67x00_clear_pipe(c67x00, td);
 946                         c67x00_giveback_urb(c67x00, urb, 0);
 947                 }
 948                 break;
 949         }
 950 }
 951
 952 static void c67x00_handle_isoc(struct c67x00_hcd *c67x00, struct c67x00_td *td)
 953 {
 954         struct urb *urb = td->urb;
 955         int cnt;
 956
 957         if (!urb)
 958                 return;
 959
 960         cnt = td->privdata;
 961
 962         if (td->status & TD_ERROR_MASK)
 963                 urb->error_count++;
 964
 965         urb->iso_frame_desc[cnt].actual_length = td_actual_bytes(td);
 966         urb->iso_frame_desc[cnt].status = c67x00_td_to_error(c67x00, td);
 967         if (cnt + 1 == urb->number_of_packets)  /* Last packet */
 968                 c67x00_giveback_urb(c67x00, urb, 0);
 969 }
 970
 971 /* -------------------------------------------------------------------------- */
 972
 973 /**
 974  * c67x00_check_td_list - handle tds which have been processed by the c67x00
 975  * pre: current_td == 0
 976  */
 977 static inline void c67x00_check_td_list(struct c67x00_hcd *c67x00)
 978 {
 979         struct c67x00_td *td, *tmp;
 980         struct urb *urb;
 981         int ack_ok;
 982         int clear_endpoint;
 983
 984         list_for_each_entry_safe(td, tmp, &c67x00->td_list, td_list) {
 985                 /* get the TD */
 986                 c67x00_parse_td(c67x00, td);
 987                 urb = td->urb;  /* urb can be NULL! */
 988                 ack_ok = 0;
 989                 clear_endpoint = 1;
 990
 991                 /* Handle isochronous transfers separately */
 992                 if (usb_pipeisoc(td->pipe)) {
 993                         clear_endpoint = 0;
 994                         c67x00_handle_isoc(c67x00, td);
 995                         goto cont;
 996                 }
 997
 998                 /* When an error occurs, all td's for that pipe go into an
 999                  * inactive state. This state matches successful transfers so
1000                  * we must make sure not to service them. */
1001                 if (td->status & TD_ERROR_MASK) {
1002                         c67x00_giveback_urb(c67x00, urb,
1003                                             c67x00_td_to_error(c67x00, td));
1004                         goto cont;
1005                 }
1006
1007                 if ((td->status & TD_STATUSMASK_NAK) || !td_sequence_ok(td) ||
1008                     !td_acked(td))
1009                         goto cont;
1010
1011                 /* Sequence ok and acked, don't need to fix toggle */
1012                 ack_ok = 1;
1013
1014                 if (unlikely(td->status & TD_STATUSMASK_OVF)) {
1015                         if (td_residue(td) & TD_RESIDUE_OVERFLOW) {
1016                                 /* Overflow */
1017                                 c67x00_giveback_urb(c67x00, urb, -EOVERFLOW);
1018                                 goto cont;
1019                         }
1020                 }
1021
1022                 clear_endpoint = 0;
1023                 c67x00_handle_successful_td(c67x00, td);
1024
1025 cont:
1026                 if (clear_endpoint)
1027                         c67x00_clear_pipe(c67x00, td);
1028                 if (ack_ok)
1029                         usb_settoggle(td_udev(td), usb_pipeendpoint(td->pipe),
1030                                       usb_pipeout(td->pipe),
1031                                       !(td->ctrl_reg & SEQ_SEL));
1032                 /* next in list could have been removed, due to clear_pipe! */
1033                 tmp = list_entry(td->td_list.next, typeof(*td), td_list);
1034                 c67x00_release_td(td);
1035         }
1036 }
1037
1038 /* -------------------------------------------------------------------------- */
1039
1040 static inline int c67x00_all_tds_processed(struct c67x00_hcd *c67x00)
1041 {
1042         /* If all tds are processed, we can check the previous frame (if
1043          * there was any) and start our next frame.
1044          */
1045         return !c67x00_ll_husb_get_current_td(c67x00->sie);
1046 }
1047
1048 /**
1049  * Send td to C67X00
1050  */
1051 static void c67x00_send_td(struct c67x00_hcd *c67x00, struct c67x00_td *td)
1052 {
1053         int len = td_length(td);
1054
1055         if (len && ((td->pid_ep & TD_PIDEPMASK_PID) != TD_PID_IN))
1056                 c67x00_ll_write_mem_le16(c67x00->sie->dev, td_ly_base_addr(td),
1057                                          td->data, len);
1058
1059         c67x00_ll_write_mem_le16(c67x00->sie->dev,
1060                                  td->td_addr, td, CY_TD_SIZE);
1061 }
1062
1063 static void c67x00_send_frame(struct c67x00_hcd *c67x00)
1064 {
1065         struct c67x00_td *td;
1066
1067         if (list_empty(&c67x00->td_list))
1068                 dev_warn(c67x00_hcd_dev(c67x00),
1069                          "%s: td list should not be empty here!\n",
1070                          __func__);
1071
1072         list_for_each_entry(td, &c67x00->td_list, td_list) {
1073                 if (td->td_list.next == &c67x00->td_list)
1074                         td->next_td_addr = 0;   /* Last td in list */
1075
1076                 c67x00_send_td(c67x00, td);
1077         }
1078
1079         c67x00_ll_husb_set_current_td(c67x00->sie, c67x00->td_base_addr);
1080 }
1081
1082 /* -------------------------------------------------------------------------- */
1083
1084 /**
1085  * c67x00_do_work - Schedulers state machine
1086  */
1087 static void c67x00_do_work(struct c67x00_hcd *c67x00)
1088 {
1089         spin_lock(&c67x00->lock);
1090         /* Make sure all tds are processed */
1091         if (!c67x00_all_tds_processed(c67x00))
1092                 goto out;
1093
1094         c67x00_check_td_list(c67x00);
1095
1096         /* no td's are being processed (current == 0)
1097          * and all have been "checked" */
1098         complete(&c67x00->endpoint_disable);
1099
1100         if (!list_empty(&c67x00->td_list))
1101                 goto out;
1102
1103         c67x00->current_frame = c67x00_get_current_frame_number(c67x00);
1104         if (c67x00->current_frame == c67x00->last_frame)
1105                 goto out;       /* Don't send tds in same frame */
1106         c67x00->last_frame = c67x00->current_frame;
1107
1108         /* If no urbs are scheduled, our work is done */
1109         if (!c67x00->urb_count) {
1110                 c67x00_ll_hpi_disable_sofeop(c67x00->sie);
1111                 goto out;
1112         }
1113
1114         c67x00_fill_frame(c67x00);
1115         if (!list_empty(&c67x00->td_list))
1116                 /* TD's have been added to the frame */
1117                 c67x00_send_frame(c67x00);
1118
1119  out:
1120         spin_unlock(&c67x00->lock);
1121 }
1122
1123 /* -------------------------------------------------------------------------- */
1124
1125 static void c67x00_sched_tasklet(unsigned long __c67x00)
1126 {
1127         struct c67x00_hcd *c67x00 = (struct c67x00_hcd *)__c67x00;
1128         c67x00_do_work(c67x00);
1129 }
1130
1131 void c67x00_sched_kick(struct c67x00_hcd *c67x00)
1132 {
1133         tasklet_hi_schedule(&c67x00->tasklet);
1134 }
1135
1136 int c67x00_sched_start_scheduler(struct c67x00_hcd *c67x00)
1137 {
1138         tasklet_init(&c67x00->tasklet, c67x00_sched_tasklet,
1139                      (unsigned long)c67x00);
1140         return 0;
1141 }
1142
1143 void c67x00_sched_stop_scheduler(struct c67x00_hcd *c67x00)
1144 {
1145         tasklet_kill(&c67x00->tasklet);
1146 }