|
@@ -431,6 +431,77 @@ ble_phy_get_cur_phy(void)
|
|
|
* lengths of the individual mbufs are not set prior to calling.
|
|
* lengths of the individual mbufs are not set prior to calling.
|
|
|
*
|
|
*
|
|
|
*/
|
|
*/
|
|
|
|
|
+
|
|
|
|
|
+#ifdef __ARMCC_VERSION
|
|
|
|
|
+void
|
|
|
|
|
+ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
|
|
|
|
|
+{
|
|
|
|
|
+ uint16_t rem_bytes;
|
|
|
|
|
+ uint16_t mb_bytes;
|
|
|
|
|
+ uint16_t copylen;
|
|
|
|
|
+ uint32_t *dst;
|
|
|
|
|
+ uint32_t *src;
|
|
|
|
|
+ struct os_mbuf *m;
|
|
|
|
|
+ struct ble_mbuf_hdr *ble_hdr;
|
|
|
|
|
+ struct os_mbuf_pkthdr *pkthdr;
|
|
|
|
|
+
|
|
|
|
|
+ /* Better be aligned */
|
|
|
|
|
+ assert(((uint32_t)dptr & 3) == 0);
|
|
|
|
|
+
|
|
|
|
|
+ pkthdr = OS_MBUF_PKTHDR(rxpdu);
|
|
|
|
|
+ rem_bytes = pkthdr->omp_len;
|
|
|
|
|
+
|
|
|
|
|
+ /* Fill in the mbuf pkthdr first. */
|
|
|
|
|
+ dst = (uint32_t *)(rxpdu->om_data);
|
|
|
|
|
+ src = (uint32_t *)dptr;
|
|
|
|
|
+
|
|
|
|
|
+ mb_bytes = (rxpdu->om_omp->omp_databuf_len - rxpdu->om_pkthdr_len - 4);
|
|
|
|
|
+ copylen = min(mb_bytes, rem_bytes);
|
|
|
|
|
+ copylen &= 0xFFFC;
|
|
|
|
|
+ rem_bytes -= copylen;
|
|
|
|
|
+ mb_bytes -= copylen;
|
|
|
|
|
+ rxpdu->om_len = copylen;
|
|
|
|
|
+ while (copylen > 0) {
|
|
|
|
|
+ *dst = *src;
|
|
|
|
|
+ ++dst;
|
|
|
|
|
+ ++src;
|
|
|
|
|
+ copylen -= 4;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ /* Copy remaining bytes */
|
|
|
|
|
+ m = rxpdu;
|
|
|
|
|
+ while (rem_bytes > 0) {
|
|
|
|
|
+ /* If there are enough bytes in the mbuf, copy them and leave */
|
|
|
|
|
+ if (rem_bytes <= mb_bytes) {
|
|
|
|
|
+ memcpy(m->om_data + m->om_len, src, rem_bytes);
|
|
|
|
|
+ m->om_len += rem_bytes;
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ m = SLIST_NEXT(m, om_next);
|
|
|
|
|
+ assert(m != NULL);
|
|
|
|
|
+
|
|
|
|
|
+ mb_bytes = m->om_omp->omp_databuf_len;
|
|
|
|
|
+ copylen = min(mb_bytes, rem_bytes);
|
|
|
|
|
+ copylen &= 0xFFFC;
|
|
|
|
|
+ rem_bytes -= copylen;
|
|
|
|
|
+ mb_bytes -= copylen;
|
|
|
|
|
+ m->om_len = copylen;
|
|
|
|
|
+ dst = (uint32_t *)m->om_data;
|
|
|
|
|
+ while (copylen > 0) {
|
|
|
|
|
+ *dst = *src;
|
|
|
|
|
+ ++dst;
|
|
|
|
|
+ ++src;
|
|
|
|
|
+ copylen -= 4;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ /* Copy ble header */
|
|
|
|
|
+ ble_hdr = BLE_MBUF_HDR_PTR(rxpdu);
|
|
|
|
|
+ memcpy(ble_hdr, &g_ble_phy_data.rxhdr, sizeof(struct ble_mbuf_hdr));
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+#else
|
|
|
void
|
|
void
|
|
|
ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
|
|
ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
|
|
|
{
|
|
{
|
|
@@ -512,6 +583,7 @@ ble_phy_rxpdu_copy(uint8_t *dptr, struct os_mbuf *rxpdu)
|
|
|
memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
|
|
memcpy(BLE_MBUF_HDR_PTR(rxpdu), &g_ble_phy_data.rxhdr,
|
|
|
sizeof(struct ble_mbuf_hdr));
|
|
sizeof(struct ble_mbuf_hdr));
|
|
|
}
|
|
}
|
|
|
|
|
+#endif
|
|
|
|
|
|
|
|
/**
|
|
/**
|
|
|
* Called when we want to wait if the radio is in either the rx or tx
|
|
* Called when we want to wait if the radio is in either the rx or tx
|