示例#1
0
int
nm_rx_rsp(uint8_t *attr_val, uint16_t attr_len)
{
    struct os_mbuf *om;
    int rc;

    om = os_msys_get_pkthdr(attr_len, 0);
    if (om == NULL) {
        rc = 1;
        goto err;
    }

    rc = os_mbuf_append(om, attr_val, attr_len);
    if (rc != 0) {
        goto err;
    }

    bleshell_printf("received nmgr rsp: ");
    rc = shell_nlip_output(om);
    if (rc != 0) {
        goto err;
    }

    return 0;

err:
    os_mbuf_free_chain(om);
    return rc;
}
示例#2
0
static int
nmgr_send_rspfrag(struct nmgr_transport *nt, struct os_mbuf *rsp,
                  struct os_mbuf *req, uint16_t len)
{
    struct os_mbuf *rspfrag;
    int rc;

    rspfrag = NULL;

    rspfrag = os_msys_get_pkthdr(len, OS_MBUF_USRHDR_LEN(req));
    if (!rspfrag) {
        rc = MGMT_ERR_ENOMEM;
        goto err;
    }

    /* Copy the request packet header into the response. */
    memcpy(OS_MBUF_USRHDR(rspfrag), OS_MBUF_USRHDR(req),
           OS_MBUF_USRHDR_LEN(req));

    if (os_mbuf_appendfrom(rspfrag, rsp, 0, len)) {
        rc = MGMT_ERR_ENOMEM;
        goto err;
    }

    nt->nt_output(nt, rspfrag);

    return MGMT_ERR_EOK;
err:
    if (rspfrag) {
        os_mbuf_free_chain(rspfrag);
    }
    return rc;
}
示例#3
0
/**
 * Allocate a pdu (chain) for reception.
 *
 * @param len
 *
 * @return struct os_mbuf*
 */
struct os_mbuf *
ble_ll_rxpdu_alloc(uint16_t len)
{
    uint16_t mb_bytes;
    struct os_mbuf *m;
    struct os_mbuf *n;
    struct os_mbuf *p;
    struct os_mbuf_pkthdr *pkthdr;

    p = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
    if (!p) {
        goto rxpdu_alloc_exit;
    }

    /* Set packet length */
    pkthdr = OS_MBUF_PKTHDR(p);
    pkthdr->omp_len = len;

    /*
     * NOTE: first mbuf in chain will have data pre-pended to it so we adjust
     * m_data by a word.
     */
    p->om_data += 4;
    mb_bytes = (p->om_omp->omp_databuf_len - p->om_pkthdr_len - 4);

    if (mb_bytes < len) {
        n = p;
        len -= mb_bytes;
        while (len) {
            m = os_msys_get(len, 0);
            if (!m) {
                os_mbuf_free_chain(p);
                p = NULL;
                goto rxpdu_alloc_exit;
            }
            /* Chain new mbuf to existing chain */
            SLIST_NEXT(n, om_next) = m;
            n = m;
            mb_bytes = m->om_omp->omp_databuf_len;
            if (mb_bytes >= len) {
                len = 0;
            } else {
                len -= mb_bytes;
            }
        }
    }


rxpdu_alloc_exit:
    if (!p) {
        STATS_INC(ble_ll_stats, no_bufs);
    }
    return p;
}
示例#4
0
/*
 * Test wrapper to get packets. Only get a packet if we have more than half
 * left
 */
static struct os_mbuf *
bletest_get_packet(void)
{
    struct os_mbuf *om;

    om = NULL;
    if (os_msys_num_free() >= 5) {
        om = os_msys_get_pkthdr(0, sizeof(struct ble_mbuf_hdr));
    }
    return om;
}
示例#5
0
/**
 * Create a scan response PDU
 *
 * @param advsm
 */
static struct os_mbuf *
ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm)
{
    uint8_t     scan_rsp_len;
    uint8_t     *dptr;
    uint8_t     pdulen;
    uint8_t     hdr;
    struct os_mbuf *m;

    /* Obtain scan response buffer */
    m = os_msys_get_pkthdr(BLE_SCAN_RSP_DATA_MAX_LEN + BLE_DEV_ADDR_LEN,
                           sizeof(struct ble_mbuf_hdr));
    if (!m) {
        return NULL;
    }

    /* Make sure that the length is valid */
    scan_rsp_len = advsm->scan_rsp_len;
    assert(scan_rsp_len <= BLE_SCAN_RSP_DATA_MAX_LEN);

    /* Set BLE transmit header */
    pdulen = BLE_DEV_ADDR_LEN + scan_rsp_len;
    hdr = BLE_ADV_PDU_TYPE_SCAN_RSP;
    if (advsm->adv_txadd) {
        hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
    }

    ble_ll_mbuf_init(m, pdulen, hdr);

    /*
     * XXX: Am I sure this is correct? The adva in this packet will be the
     * same one that was being advertised and is based on the peer identity
     * address in the set advertising parameters. If a different peer sends
     * us a scan request (for some reason) we will reply with an adva that
     * was not generated based on the local irk of the peer sending the scan
     * request.
     */

    /* Construct scan response */
    dptr = m->om_data;
    memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN);
    if (scan_rsp_len != 0) {
        memcpy(dptr + BLE_DEV_ADDR_LEN, advsm->scan_rsp_data, scan_rsp_len);
    }

    return m;
}
示例#6
0
/*
 * Receive a character from UART.
 */
static int
nmgr_uart_rx_char(void *arg, uint8_t data)
{
    struct nmgr_uart_state *nus = (struct nmgr_uart_state *)arg;
    struct os_mbuf *m;
    int rc;

    if (!nus->nus_rx) {
        m = os_msys_get_pkthdr(SHELL_NLIP_MAX_FRAME, 0);
        if (!m) {
            return 0;
        }
        nus->nus_rx = OS_MBUF_PKTHDR(m);
        if (OS_MBUF_TRAILINGSPACE(m) < SHELL_NLIP_MAX_FRAME) {
            /*
             * mbuf is too small.
             */
            os_mbuf_free_chain(m);
            nus->nus_rx = NULL;
            return 0;
        }
    }

    m = OS_MBUF_PKTHDR_TO_MBUF(nus->nus_rx);
    if (data == '\n') {
        /*
         * Full line of input. Process it outside interrupt context.
         */
        assert(!nus->nus_rx_q);
        nus->nus_rx_q = nus->nus_rx;
        nus->nus_rx = NULL;
        os_eventq_put(g_mgmt_evq, &nus->nus_cb_ev);
        return 0;
    } else {
        rc = os_mbuf_append(m, &data, 1);
        if (rc == 0) {
            return 0;
        }
    }
    /* failed */
    nus->nus_rx->omp_len = 0;
    m->om_len = 0;
    os_mbuf_free_chain(SLIST_NEXT(m, om_next));
    SLIST_NEXT(m, om_next) = NULL;
    return 0;
}
示例#7
0
int
nm_chr_access(uint16_t conn_handle, uint16_t attr_handle,
              uint8_t op, union ble_gatt_access_ctxt *ctxt,
              void *arg)
{
    struct nmgr_transport *nt;
    struct os_mbuf *om;
    int rc;

    assert(attr_handle == nm_attr_val_handle);

    om = NULL;

    if (op != BLE_GATT_ACCESS_OP_WRITE_CHR) {
        rc = BLE_ATT_ERR_WRITE_NOT_PERMITTED;
        goto err;
    }

    om = os_msys_get_pkthdr(ctxt->chr_access.len, 2);
    if (om == NULL) {
        rc = 1;
        goto err;
    }

    /* Put the connection handle in the request packet header. */
    memcpy(OS_MBUF_USRHDR(om), &conn_handle, sizeof conn_handle);

    rc = os_mbuf_append(om, ctxt->chr_access.data, ctxt->chr_access.len);
    if (rc != 0) {
        goto err;
    }

    nt = arg;
    rc = nmgr_rx_req(nt, om);

    return rc;

err:
    os_mbuf_free_chain(om);
    return rc;
}
/**
 * Initialize the advertising functionality of a BLE device. This should
 * be called once on initialization
 */
void
ble_ll_adv_init(void)
{
    struct ble_ll_adv_sm *advsm;

    /* Set default advertising parameters */
    advsm = &g_ble_ll_adv_sm;
    memset(advsm, 0, sizeof(struct ble_ll_adv_sm));

    advsm->adv_itvl_min = BLE_HCI_ADV_ITVL_DEF;
    advsm->adv_itvl_max = BLE_HCI_ADV_ITVL_DEF;
    advsm->adv_chanmask = BLE_HCI_ADV_CHANMASK_DEF;

    /* Initialize advertising tx done event */
    advsm->adv_txdone_ev.ev_type = BLE_LL_EVENT_ADV_EV_DONE;
    advsm->adv_txdone_ev.ev_arg = advsm;

    /* Get a scan response mbuf (packet header) and attach to state machine */
    advsm->scan_rsp_pdu = os_msys_get_pkthdr(BLE_MBUF_PAYLOAD_SIZE,
                                             sizeof(struct ble_mbuf_hdr));
    assert(advsm->scan_rsp_pdu != NULL);
}
示例#9
0
/**
 * This is the scheduler callback (called from interrupt context) which
 * transmits an advertisement.
 *
 * Context: Interrupt (scheduler)
 *
 * @param sch
 *
 * @return int
 */
static int
ble_ll_adv_tx_start_cb(struct ble_ll_sched_item *sch)
{
    int rc;
    uint8_t end_trans;
    uint32_t txstart;
    struct ble_ll_adv_sm *advsm;
    struct os_mbuf *adv_pdu;

    /* Get the state machine for the event */
    advsm = (struct ble_ll_adv_sm *)sch->cb_arg;

    /* Set channel */
    rc = ble_phy_setchan(advsm->adv_chan, 0, 0);
    assert(rc == 0);

    /* Set transmit start time. */
    txstart = sch->start_time + XCVR_PROC_DELAY_USECS;
    rc = ble_phy_tx_set_start_time(txstart);
    if (rc) {
        STATS_INC(ble_ll_stats, adv_late_starts);
        goto adv_tx_done;
    }

#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1)
    /* XXX: automatically do this in the phy based on channel? */
    ble_phy_encrypt_disable();
#endif

#if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) == 1)
    advsm->adv_rpa_index = -1;
    if (ble_ll_resolv_enabled()) {
        ble_phy_resolv_list_enable();
    } else {
        ble_phy_resolv_list_disable();
    }
#endif

    /* Set phy mode based on type of advertisement */
    if (advsm->adv_type == BLE_HCI_ADV_TYPE_ADV_NONCONN_IND) {
        end_trans = BLE_PHY_TRANSITION_NONE;
        ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm);
    } else {
        end_trans = BLE_PHY_TRANSITION_TX_RX;
        ble_phy_set_txend_cb(NULL, NULL);
    }

    /* Get an advertising mbuf (packet header)  */
    adv_pdu = os_msys_get_pkthdr(BLE_ADV_MAX_PKT_LEN,
                                 sizeof(struct ble_mbuf_hdr));
    if (!adv_pdu) {
        ble_phy_disable();
        goto adv_tx_done;
    }

    ble_ll_adv_pdu_make(advsm, adv_pdu);

    /* Transmit advertisement */
    rc = ble_phy_tx(adv_pdu, end_trans);
    os_mbuf_free_chain(adv_pdu);
    if (rc) {
        goto adv_tx_done;
    }

    /* Enable/disable whitelisting based on filter policy */
    if (advsm->adv_filter_policy != BLE_HCI_ADV_FILT_NONE) {
        ble_ll_whitelist_enable();
    } else {
        ble_ll_whitelist_disable();
    }

    /* Set link layer state to advertising */
    ble_ll_state_set(BLE_LL_STATE_ADV);

    /* Count # of adv. sent */
    STATS_INC(ble_ll_stats, adv_txg);

    return BLE_LL_SCHED_STATE_RUNNING;

adv_tx_done:
    ble_ll_adv_tx_done(advsm);
    return BLE_LL_SCHED_STATE_DONE;
}
示例#10
0
static int
gatt_svr_chr_access_newtmgr(uint16_t conn_handle, uint16_t attr_handle,
                            struct ble_gatt_access_ctxt *ctxt, void *arg)
{
    int rc;
    struct os_mbuf *m_req;

    switch (ctxt->op) {
        case BLE_GATT_ACCESS_OP_WRITE_CHR:
            /* Try to reuse the BLE packet mbuf as the newtmgr request.  This
             * requires a two-byte usrhdr to hold the BLE connection handle so
             * that the newtmgr response can be sent to the correct peer.  If
             * it is not possible to reuse the mbuf, then allocate a new one
             * and copy the request contents.
             */
            if (OS_MBUF_USRHDR_LEN(ctxt->om) >= sizeof (conn_handle)) {
                /* Sufficient usrhdr space already present. */
                m_req = ctxt->om;
                ctxt->om = NULL;
            } else if (OS_MBUF_LEADINGSPACE(ctxt->om) >=
                       sizeof (conn_handle)) {

                /* Usrhdr isn't present, but there is enough leading space to
                 * add one.
                 */
                m_req = ctxt->om;
                ctxt->om = NULL;

                m_req->om_pkthdr_len += sizeof (conn_handle);
            } else {
                /* The mbuf can't be reused.  Allocate a new one and perform a
                 * copy.  Don't set ctxt->om to NULL; let the NimBLE host free
                 * it.
                 */
                m_req = os_msys_get_pkthdr(OS_MBUF_PKTLEN(ctxt->om),
                                           sizeof (conn_handle));
                if (!m_req) {
                    return BLE_ATT_ERR_INSUFFICIENT_RES;
                }
                rc = os_mbuf_appendfrom(m_req, ctxt->om, 0,
                                        OS_MBUF_PKTLEN(ctxt->om));
                if (rc) {
                    return BLE_ATT_ERR_INSUFFICIENT_RES;
                }
            }

            /* Write the connection handle to the newtmgr request usrhdr.  This
             * is necessary so that we later know who to send the newtmgr
             * response to.
             */
            memcpy(OS_MBUF_USRHDR(m_req), &conn_handle, sizeof(conn_handle));

            rc = nmgr_rx_req(&ble_nt, m_req);
            if (rc != 0) {
                return BLE_ATT_ERR_UNLIKELY;
            }
            return 0;

        default:
            assert(0);
            return BLE_ATT_ERR_UNLIKELY;
    }
}
示例#11
0
static void
nmgr_handle_req(struct nmgr_transport *nt, struct os_mbuf *req)
{
    struct os_mbuf *rsp;
    const struct mgmt_handler *handler;
    struct nmgr_hdr *rsp_hdr;
    struct nmgr_hdr hdr;
    int off;
    uint16_t len;
    int rc;

    rsp_hdr = NULL;

    rsp = os_msys_get_pkthdr(512, OS_MBUF_USRHDR_LEN(req));
    if (!rsp) {
        rc = os_mbuf_copydata(req, 0, sizeof(hdr), &hdr);
        if (rc < 0) {
            goto err_norsp;
        }
        rsp = req;
        req = NULL;
        goto err;
    }

    /* Copy the request packet header into the response. */
    memcpy(OS_MBUF_USRHDR(rsp), OS_MBUF_USRHDR(req), OS_MBUF_USRHDR_LEN(req));

    off = 0;
    len = OS_MBUF_PKTHDR(req)->omp_len;

    while (off < len) {
        rc = os_mbuf_copydata(req, off, sizeof(hdr), &hdr);
        if (rc < 0) {
            rc = MGMT_ERR_EINVAL;
            goto err_norsp;
        }

        hdr.nh_len = ntohs(hdr.nh_len);

        handler = mgmt_find_handler(ntohs(hdr.nh_group), hdr.nh_id);
        if (!handler) {
            rc = MGMT_ERR_ENOENT;
            goto err;
        }

        /* Build response header apriori.  Then pass to the handlers
         * to fill out the response data, and adjust length & flags.
         */
        rsp_hdr = nmgr_init_rsp(rsp, &hdr);
        if (!rsp_hdr) {
            rc = MGMT_ERR_ENOMEM;
            goto err_norsp;
        }

        cbor_mbuf_reader_init(&nmgr_task_cbuf.reader, req, sizeof(hdr));
        cbor_parser_init(&nmgr_task_cbuf.reader.r, 0,
                         &nmgr_task_cbuf.n_b.parser, &nmgr_task_cbuf.n_b.it);

        if (hdr.nh_op == NMGR_OP_READ) {
            if (handler->mh_read) {
                rc = handler->mh_read(&nmgr_task_cbuf.n_b);
            } else {
                rc = MGMT_ERR_ENOENT;
            }
        } else if (hdr.nh_op == NMGR_OP_WRITE) {
            if (handler->mh_write) {
                rc = handler->mh_write(&nmgr_task_cbuf.n_b);
            } else {
                rc = MGMT_ERR_ENOENT;
            }
        } else {
            rc = MGMT_ERR_EINVAL;
        }

        if (rc != 0) {
            goto err;
        }

        rsp_hdr->nh_len +=
            cbor_encode_bytes_written(&nmgr_task_cbuf.n_b.encoder);
        off += sizeof(hdr) + OS_ALIGN(hdr.nh_len, 4);

        rsp_hdr->nh_len = htons(rsp_hdr->nh_len);
        rc = nmgr_rsp_fragment(nt, rsp_hdr, rsp, req);
        if (rc) {
            goto err;
        }
    }

    os_mbuf_free_chain(rsp);
    os_mbuf_free_chain(req);
    return;
err:
    OS_MBUF_PKTHDR(rsp)->omp_len = rsp->om_len = 0;
    nmgr_send_err_rsp(nt, rsp, &hdr, rc);
    os_mbuf_free_chain(req);
    return;
err_norsp:
    os_mbuf_free_chain(rsp);
    os_mbuf_free_chain(req);
    return;
}