static void dhd_bta_flush_hcidata(dhd_pub_t *pub, uint16 llh) { int prec; struct pktq *q; uint count = 0; q = dhd_bus_txq(pub->bus); if (q == NULL) return; DHD_BTA(("dhd: flushing HCI ACL data for logical link %u...\n", llh)); dhd_os_sdlock_txq(pub); /* Walk through the txq and toss all HCI ACL data packets */ PKTQ_PREC_ITER(q, prec) { void *head_pkt = NULL; while (pktq_ppeek(q, prec) != head_pkt) { void *pkt = pktq_pdeq(q, prec); int ifidx; PKTPULL(pub->osh, pkt, dhd_bus_hdrlen(pub->bus)); dhd_prot_hdrpull(pub, &ifidx, pkt); if (PKTLEN(pub->osh, pkt) >= RFC1042_HDR_LEN) { struct ether_header *eh = (struct ether_header *)PKTDATA(pub->osh, pkt); if (ntoh16(eh->ether_type) < ETHER_TYPE_MIN) { struct dot11_llc_snap_header *lsh = (struct dot11_llc_snap_header *)&eh[1]; if (bcmp(lsh, BT_SIG_SNAP_MPROT, DOT11_LLC_SNAP_HDR_LEN - 2) == 0 && ntoh16(lsh->type) == BTA_PROT_L2CAP) { amp_hci_ACL_data_t *ACL_data = (amp_hci_ACL_data_t *)&lsh[1]; uint16 handle = ltoh16(ACL_data->handle); if (HCI_ACL_DATA_HANDLE(handle) == llh) { PKTFREE(pub->osh, pkt, TRUE); count ++; continue; } } } } dhd_prot_hdrpush(pub, ifidx, pkt); PKTPUSH(pub->osh, pkt, dhd_bus_hdrlen(pub->bus)); if (head_pkt == NULL) head_pkt = pkt; pktq_penq(q, prec, pkt); } }
bool dhd_prec_enq(dhd_pub_t *dhdp, struct pktq *q, void *pkt, int prec) { void *p; int eprec = -1; /* precedence to evict from */ bool discard_oldest; /* Fast case, precedence queue is not full and we are also not * exceeding total queue length */ if (!pktq_pfull(q, prec) && !pktq_full(q)) { pktq_penq(q, prec, pkt); return TRUE; } /* Determine precedence from which to evict packet, if any */ if (pktq_pfull(q, prec)) eprec = prec; else if (pktq_full(q)) { p = pktq_peek_tail(q, &eprec); ASSERT(p); if (eprec > prec) return FALSE; } /* Evict if needed */ if (eprec >= 0) { /* Detect queueing to unconfigured precedence */ ASSERT(!pktq_pempty(q, eprec)); discard_oldest = AC_BITMAP_TST(dhdp->wme_dp, eprec); if (eprec == prec && !discard_oldest) return FALSE; /* refuse newer (incoming) packet */ /* Evict packet according to discard policy */ p = discard_oldest ? pktq_pdeq(q, eprec) : pktq_pdeq_tail(q, eprec); if (p == NULL) { DHD_ERROR(("%s: pktq_penq() failed, oldest %d.", __FUNCTION__, discard_oldest)); ASSERT(p); } PKTFREE(dhdp->osh, p, TRUE); } /* Enqueue */ p = pktq_penq(q, prec, pkt); if (p == NULL) { DHD_ERROR(("%s: pktq_penq() failed.", __FUNCTION__)); ASSERT(p); } return TRUE; }