diff options
| -rw-r--r-- | payloads/libpayload/drivers/usb/ohci.c | 230 | ||||
| -rw-r--r-- | payloads/libpayload/drivers/usb/ohci_private.h | 1 |
2 files changed, 223 insertions, 8 deletions
diff --git a/payloads/libpayload/drivers/usb/ohci.c b/payloads/libpayload/drivers/usb/ohci.c index 6d98cc1100..0f4886fc63 100644 --- a/payloads/libpayload/drivers/usb/ohci.c +++ b/payloads/libpayload/drivers/usb/ohci.c @@ -144,6 +144,8 @@ ohci_init (pcidev_t addr) OHCI_INST (controller)->periodic_ed = periodic_ed; OHCI_INST (controller)->opreg->HcHCCA = virt_to_phys(OHCI_INST (controller)->hcca); + /* Make sure periodic schedule is enabled. */ + OHCI_INST (controller)->opreg->HcControl |= PeriodicListEnable; OHCI_INST (controller)->opreg->HcControl &= ~IsochronousEnable; // unused by this driver // disable everything, contrary to what OHCI spec says in 5.1.1.4, as we don't need IRQs OHCI_INST (controller)->opreg->HcInterruptEnable = 1<<31; @@ -477,33 +479,215 @@ ohci_bulk (endpoint_t *ep, int dalen, u8 *data, int finalize) return failure; } + +struct _intr_queue; + +struct _intrq_td { + volatile td_t td; + u8 *data; + struct _intrq_td *next; + struct _intr_queue *intrq; +}; + +struct _intr_queue { + volatile ed_t ed; + struct _intrq_td *head; + struct _intrq_td *tail; + u8 *data; + int reqsize; + endpoint_t *endp; + unsigned int remaining_tds; +}; + +typedef struct _intrq_td intrq_td_t; +typedef struct _intr_queue intr_queue_t; + +#define INTRQ_TD_FROM_TD(x) ((intrq_td_t *)x) + +static void +ohci_fill_intrq_td(intrq_td_t *const td, intr_queue_t *const intrq, + u8 *const data) +{ + memset(td, 0, sizeof(*td)); + td->td.config = TD_QUEUETYPE_INTR | + (intrq->endp->direction == IN + ? TD_DIRECTION_IN : TD_DIRECTION_OUT) | + TD_DELAY_INTERRUPT_ZERO | + TD_TOGGLE_FROM_ED | + TD_CC_NOACCESS; + td->td.current_buffer_pointer = virt_to_phys(data); + td->td.buffer_end = td->td.current_buffer_pointer + intrq->reqsize - 1; + td->intrq = intrq; + td->data = data; +} + /* create and hook-up an intr queue into device schedule */ -static void* -ohci_create_intr_queue (endpoint_t *ep, int reqsize, int reqcount, int reqtiming) +static void * +ohci_create_intr_queue(endpoint_t *const ep, const int reqsize, + const int reqcount, const int reqtiming) { - return NULL; + int i; + intrq_td_t *first_td = NULL, *last_td = NULL; + + if (reqsize > 4096) + return NULL; + + intr_queue_t *const intrq = + (intr_queue_t *)memalign(sizeof(intrq->ed), sizeof(*intrq)); + memset(intrq, 0, sizeof(*intrq)); + intrq->data = (u8 *)malloc(reqcount * reqsize); + intrq->reqsize = reqsize; + intrq->endp = ep; + + /* Create #reqcount TDs. */ + u8 *cur_data = intrq->data; + for (i = 0; i < reqcount; ++i) { + intrq_td_t *const td = memalign(sizeof(td->td), sizeof(*td)); + ++intrq->remaining_tds; + ohci_fill_intrq_td(td, intrq, cur_data); + cur_data += reqsize; + if (!first_td) + first_td = td; + else + last_td->td.next_td = virt_to_phys(&td->td); + last_td = td; + } + + /* Create last, dummy TD. */ + intrq_td_t *dummy_td = memalign(sizeof(dummy_td->td), sizeof(*dummy_td)); + memset(dummy_td, 0, sizeof(*dummy_td)); + dummy_td->intrq = intrq; + if (last_td) + last_td->td.next_td = virt_to_phys(&dummy_td->td); + last_td = dummy_td; + + /* Initialize ED. */ + intrq->ed.config = (ep->dev->address << ED_FUNC_SHIFT) | + ((ep->endpoint & 0xf) << ED_EP_SHIFT) | + (((ep->direction == IN) ? OHCI_IN : OHCI_OUT) << ED_DIR_SHIFT) | + (ep->dev->speed ? ED_LOWSPEED : 0) | + (ep->maxpacketsize << ED_MPS_SHIFT); + intrq->ed.tail_pointer = virt_to_phys(last_td); + intrq->ed.head_pointer = virt_to_phys(first_td) | + (ep->toggle ? ED_TOGGLE : 0); + + /* Insert ED into periodic table. */ + int nothing_placed = 1; + ohci_t *const ohci = OHCI_INST(ep->dev->controller); + u32 *const intr_table = ohci->hcca->HccaInterruptTable; + const u32 dummy_ptr = virt_to_phys(ohci->periodic_ed); + for (i = 0; i < 32; i += reqtiming) { + /* Advance to the next free position. */ + while ((i < 32) && (intr_table[i] != dummy_ptr)) ++i; + if (i < 32) { + intr_table[i] = virt_to_phys(&intrq->ed); + nothing_placed = 0; + } + } + if (nothing_placed) { + printf("Error: Failed to place ohci interrupt endpoint " + "descriptor into periodic table: no space left\n"); + ohci_destroy_intr_queue(ep, intrq); + return NULL; + } + + return intrq; } /* remove queue from device schedule, dropping all data that came in */ static void -ohci_destroy_intr_queue (endpoint_t *ep, void *q_) +ohci_destroy_intr_queue(endpoint_t *const ep, void *const q_) { + intr_queue_t *const intrq = (intr_queue_t *)q_; + + int i; + + /* Remove interrupt queue from periodic table. */ + ohci_t *const ohci = OHCI_INST(ep->dev->controller); + u32 *const intr_table = ohci->hcca->HccaInterruptTable; + for (i=0; i < 32; ++i) { + if (intr_table[i] == virt_to_phys(intrq)) + intr_table[i] = virt_to_phys(ohci->periodic_ed); + } + /* Wait for frame to finish. */ + mdelay(1); + + /* Free unprocessed TDs. */ + while ((intrq->ed.head_pointer & ~0x3) != intrq->ed.tail_pointer) { + td_t *const cur_td = + (td_t *)phys_to_virt(intrq->ed.head_pointer & ~0x3); + intrq->ed.head_pointer = cur_td->next_td; + free(INTRQ_TD_FROM_TD(cur_td)); + --intrq->remaining_tds; + } + /* Free final, dummy TD. */ + free(phys_to_virt(intrq->ed.head_pointer & ~0x3)); + /* Free data buffer. */ + free(intrq->data); + + /* Process done queue and free processed TDs. */ + ohci_process_done_queue(ohci, 1); + while (intrq->head) { + intrq_td_t *const cur_td = intrq->head; + intrq->head = intrq->head->next; + free(cur_td); + --intrq->remaining_tds; + } + if (intrq->remaining_tds) { + printf("error: ohci_destroy_intr_queue(): " + "freed all but %d TDs.\n", intrq->remaining_tds); + } + + free(intrq); + + /* Save data toggle. */ + ep->toggle = intrq->ed.head_pointer & ED_TOGGLE; } /* read one intr-packet from queue, if available. extend the queue for new input. return NULL if nothing new available. Recommended use: while (data=poll_intr_queue(q)) process(data); */ -static u8* -ohci_poll_intr_queue (void *q_) +static u8 * +ohci_poll_intr_queue(void *const q_) { - return NULL; + intr_queue_t *const intrq = (intr_queue_t *)q_; + + u8 *data = NULL; + + /* Process done queue first, then check if we have work to do. */ + ohci_process_done_queue(OHCI_INST(intrq->endp->dev->controller), 0); + + if (intrq->head) { + /* Save pointer to processed TD and advance. */ + intrq_td_t *const cur_td = intrq->head; + intrq->head = cur_td->next; + + /* Return data buffer of this TD. */ + data = cur_td->data; + + /* Requeue this TD (i.e. copy to dummy and requeue as dummy). */ + intrq_td_t *const dummy_td = + INTRQ_TD_FROM_TD(phys_to_virt(intrq->ed.tail_pointer)); + ohci_fill_intrq_td(dummy_td, intrq, cur_td->data); + /* Reset all but intrq pointer (i.e. init as dummy). */ + memset(cur_td, 0, sizeof(*cur_td)); + cur_td->intrq = intrq; + /* Insert into interrupt queue as dummy. */ + dummy_td->td.next_td = virt_to_phys(&cur_td->td); + intrq->ed.tail_pointer = virt_to_phys(&cur_td->td); + } + + return data; } static void ohci_process_done_queue(ohci_t *const ohci, const int spew_debug) { - int i; + int i, j; + + /* Temporary queue of interrupt queue TDs (to reverse order). */ + intrq_td_t *temp_tdq = NULL; /* Check if done head has been written. */ if (!(ohci->opreg->HcInterruptStatus & WritebackDoneHead)) @@ -527,6 +711,11 @@ ohci_process_done_queue(ohci_t *const ohci, const int spew_debug) /* Free processed async TDs. */ free((void *)done_td); break; + case TD_QUEUETYPE_INTR: + /* Save done TD if it comes from an interrupt queue. */ + INTRQ_TD_FROM_TD(done_td)->next = temp_tdq; + temp_tdq = INTRQ_TD_FROM_TD(done_td); + break; default: break; } @@ -534,5 +723,30 @@ ohci_process_done_queue(ohci_t *const ohci, const int spew_debug) } if (spew_debug) debug("Processed %d done TDs.\n", i); + + j = 0; + /* Process interrupt queue TDs in right order. */ + while (temp_tdq) { + /* Save pointer of current TD and advance. */ + intrq_td_t *const cur_td = temp_tdq; + temp_tdq = temp_tdq->next; + + /* The interrupt queue for the current TD. */ + intr_queue_t *const intrq = cur_td->intrq; + /* Append to interrupt queue. */ + if (!intrq->head) { + /* First element. */ + intrq->head = intrq->tail = cur_td; + } else { + /* Insert at tail. */ + intrq->tail->next = cur_td; + intrq->tail = cur_td; + } + /* It's always the last element. */ + cur_td->next = NULL; + ++j; + } + if (spew_debug) + debug("processed %d done tds, %d intr tds thereof.\n", i, j); } diff --git a/payloads/libpayload/drivers/usb/ohci_private.h b/payloads/libpayload/drivers/usb/ohci_private.h index 3826db08ee..a32203c582 100644 --- a/payloads/libpayload/drivers/usb/ohci_private.h +++ b/payloads/libpayload/drivers/usb/ohci_private.h @@ -231,6 +231,7 @@ #define TD_QUEUETYPE_SHIFT 0 #define TD_QUEUETYPE_MASK MASK(TD_QUEUETYPE_SHIFT, 2) #define TD_QUEUETYPE_ASYNC (0 << TD_QUEUETYPE_SHIFT) +#define TD_QUEUETYPE_INTR (1 << TD_QUEUETYPE_SHIFT) #define TD_DIRECTION_SHIFT 19 #define TD_DIRECTION_MASK MASK(TD_DIRECTION_SHIFT, 2) |