static struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch,
struct net_buf *buf,
size_t sdu_hdr_len)
{
struct net_buf *seg;
uint16_t headroom;
uint16_t len;
/* Segment if data (+ data headroom) is bigger than MPS */
if (buf->len + sdu_hdr_len > ch->tx.mps) {
goto segment;
}
/* Segment if there is no space in the user_data */
if (buf->pool->user_data_size < BT_BUF_USER_DATA_MIN) {
BT_WARN("Too small buffer user_data_size %u",
buf->pool->user_data_size);
goto segment;
}
headroom = sizeof(struct bt_hci_acl_hdr) +
sizeof(struct bt_l2cap_hdr) + sdu_hdr_len;
/* Check if original buffer has enough headroom and don't have any
* fragments.
*/
if (net_buf_headroom(buf) >= headroom && !buf->frags) {
if (sdu_hdr_len) {
/* Push SDU length if set */
net_buf_push_le16(buf, net_buf_frags_len(buf));
}
return net_buf_ref(buf);
}
segment:
seg = bt_l2cap_create_pdu(&le_data_pool, 0);
if (sdu_hdr_len) {
net_buf_add_le16(seg, net_buf_frags_len(buf));
}
/* Don't send more that TX MPS including SDU length */
len = min(net_buf_tailroom(seg), ch->tx.mps - sdu_hdr_len);
/* Limit if original buffer is smaller than the segment */
len = min(buf->len, len);
net_buf_add_mem(seg, buf->data, len);
net_buf_pull(buf, len);
BT_DBG("ch %p seg %p len %u", ch, seg, seg->len);
return seg;
}
/* Setup IPv4 + UDP header */
static void setup_header(struct net_buf *buf)
{
struct net_ipv4_hdr *ipv4;
struct net_udp_hdr *udp;
uint16_t len;
ipv4 = NET_IPV4_BUF(buf);
udp = NET_UDP_BUF(buf);
len = net_buf_frags_len(buf->frags);
/* Setup IPv4 header */
memset(ipv4, 0, sizeof(struct net_ipv4_hdr));
ipv4->vhl = 0x45;
ipv4->ttl = 0xFF;
ipv4->proto = IPPROTO_UDP;
ipv4->len[0] = len >> 8;
ipv4->len[1] = (uint8_t)len;
ipv4->chksum = ~net_calc_chksum_ipv4(buf);
net_ipaddr_copy(&ipv4->dst, net_ipv4_broadcast_address());
len -= NET_IPV4H_LEN;
/* Setup UDP header */
udp->src_port = htons(DHCPV4_CLIENT_PORT);
udp->dst_port = htons(DHCPV4_SERVER_PORT);
udp->len = htons(len);
udp->chksum = ~net_calc_chksum_udp(buf);
}
static enum net_verdict net_bt_recv(struct net_if *iface, struct net_buf *buf)
{
uint32_t src;
uint32_t dst;
NET_DBG("iface %p buf %p len %u", iface, buf, net_buf_frags_len(buf));
/* Uncompress will drop the current fragment. Buf ll src/dst address
* will then be wrong and must be updated according to the new fragment.
*/
src = net_nbuf_ll_src(buf)->addr ?
net_nbuf_ll_src(buf)->addr - net_nbuf_ll(buf) : 0;
dst = net_nbuf_ll_dst(buf)->addr ?
net_nbuf_ll_dst(buf)->addr - net_nbuf_ll(buf) : 0;
if (!net_6lo_uncompress(buf)) {
NET_DBG("Packet decompression failed");
return NET_DROP;
}
net_nbuf_ll_src(buf)->addr = src ? net_nbuf_ll(buf) + src : NULL;
net_nbuf_ll_dst(buf)->addr = dst ? net_nbuf_ll(buf) + dst : NULL;
return NET_CONTINUE;
}
static void ipsp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
{
struct bt_context *ctxt = CHAN_CTXT(chan);
struct net_buf *nbuf;
NET_DBG("Incoming data channel %p len %u", chan,
net_buf_frags_len(buf));
/* Get buffer for bearer / protocol related data */
nbuf = net_nbuf_get_reserve_rx(0);
/* Set destination address */
net_nbuf_ll_dst(nbuf)->addr = ctxt->src.val;
net_nbuf_ll_dst(nbuf)->len = sizeof(ctxt->src);
/* Set source address */
net_nbuf_ll_src(nbuf)->addr = ctxt->dst.val;
net_nbuf_ll_src(nbuf)->len = sizeof(ctxt->dst);
/* Add data buffer as fragment of RX buffer, take a reference while
* doing so since L2CAP will unref the buffer after return.
*/
net_buf_frag_add(nbuf, net_buf_ref(buf));
if (net_recv_data(ctxt->iface, nbuf) < 0) {
NET_DBG("Packet dropped by NET stack");
net_nbuf_unref(nbuf);
}
}
static void ipsp_recv(struct bt_l2cap_chan *chan, struct net_buf *buf)
{
struct bt_context *ctxt = CHAN_CTXT(chan);
struct net_pkt *pkt;
NET_DBG("Incoming data channel %p len %zu", chan,
net_buf_frags_len(buf));
/* Get packet for bearer / protocol related data */
pkt = net_pkt_get_reserve_rx(0, K_FOREVER);
/* Set destination address */
net_pkt_ll_dst(pkt)->addr = ctxt->src.val;
net_pkt_ll_dst(pkt)->len = sizeof(ctxt->src);
net_pkt_ll_dst(pkt)->type = NET_LINK_BLUETOOTH;
/* Set source address */
net_pkt_ll_src(pkt)->addr = ctxt->dst.val;
net_pkt_ll_src(pkt)->len = sizeof(ctxt->dst);
net_pkt_ll_src(pkt)->type = NET_LINK_BLUETOOTH;
/* Add data buffer as fragment of RX buffer, take a reference while
* doing so since L2CAP will unref the buffer after return.
*/
net_pkt_frag_add(pkt, net_buf_ref(buf));
if (net_recv_data(ctxt->iface, pkt) < 0) {
NET_DBG("Packet dropped by NET stack");
net_pkt_unref(pkt);
}
}
static void l2cap_chan_le_recv_sdu(struct bt_l2cap_le_chan *chan,
struct net_buf *buf)
{
struct net_buf *frag;
uint16_t len;
BT_DBG("chan %p len %u sdu %zu", chan, buf->len,
net_buf_frags_len(chan->_sdu));
if (net_buf_frags_len(chan->_sdu) + buf->len > chan->_sdu_len) {
BT_ERR("SDU length mismatch");
bt_l2cap_chan_disconnect(&chan->chan);
return;
}
/* Jump to last fragment */
frag = net_buf_frag_last(chan->_sdu);
while (buf->len) {
/* Check if there is any space left in the current fragment */
if (!net_buf_tailroom(frag)) {
frag = l2cap_alloc_frag(chan);
if (!frag) {
BT_ERR("Unable to store SDU");
bt_l2cap_chan_disconnect(&chan->chan);
return;
}
}
len = min(net_buf_tailroom(frag), buf->len);
net_buf_add_mem(frag, buf->data, len);
net_buf_pull(buf, len);
BT_DBG("frag %p len %u", frag, frag->len);
}
if (net_buf_frags_len(chan->_sdu) == chan->_sdu_len) {
/* Receiving complete SDU, notify channel and reset SDU buf */
chan->chan.ops->recv(&chan->chan, chan->_sdu);
net_buf_unref(chan->_sdu);
chan->_sdu = NULL;
chan->_sdu_len = 0;
}
l2cap_chan_update_credits(chan);
}
static int l2cap_chan_le_send_sdu(struct bt_l2cap_le_chan *ch,
struct net_buf *buf, int sent)
{
int ret, total_len;
struct net_buf *frag;
total_len = net_buf_frags_len(buf) + sent;
if (total_len > ch->tx.mtu) {
return -EMSGSIZE;
}
frag = buf;
if (!frag->len && frag->frags) {
frag = frag->frags;
}
if (!sent) {
/* Add SDU length for the first segment */
sent = l2cap_chan_le_send(ch, frag, BT_L2CAP_SDU_HDR_LEN);
if (sent < 0) {
if (sent == -EAGAIN) {
sent = 0;
/* Store sent data into user_data */
memcpy(net_buf_user_data(buf), &sent,
sizeof(sent));
}
return sent;
}
}
/* Send remaining segments */
for (ret = 0; sent < total_len; sent += ret) {
/* Proceed to next fragment */
if (!frag->len) {
frag = net_buf_frag_del(buf, frag);
}
ret = l2cap_chan_le_send(ch, frag, 0);
if (ret < 0) {
if (ret == -EAGAIN) {
/* Store sent data into user_data */
memcpy(net_buf_user_data(buf), &sent,
sizeof(sent));
}
return ret;
}
}
BT_DBG("ch %p cid 0x%04x sent %u", ch, ch->tx.cid, sent);
net_buf_unref(buf);
return ret;
}
int net_recv_data(struct net_if *iface, struct net_buf *buf)
{
struct net_buf *frag;
SYS_LOG_DBG("Got data, buf %p, len %d frags->len %d",
buf, buf->len, net_buf_frags_len(buf));
frag = net_buf_frag_last(buf);
/**
* Add length 1 byte, do not forget to reserve it
*/
net_buf_push_u8(frag, net_buf_frags_len(buf) - 1);
hexdump("<", frag->data, net_buf_frags_len(buf));
try_write(WPANUSB_ENDP_BULK_IN, frag->data, net_buf_frags_len(buf));
net_nbuf_unref(buf);
return 0;
}
static int bt_iface_send(struct net_if *iface, struct net_buf *buf)
{
struct bt_context *ctxt = net_if_get_device(iface)->driver_data;
int ret;
NET_DBG("iface %p buf %p len %u", iface, buf, net_buf_frags_len(buf));
ret = bt_l2cap_chan_send(&ctxt->ipsp_chan.chan, buf);
if (ret < 0) {
return ret;
}
return ret;
}
static enum net_verdict net_bt_send(struct net_if *iface, struct net_buf *buf)
{
struct bt_context *ctxt = net_if_get_device(iface)->driver_data;
NET_DBG("iface %p buf %p len %u", iface, buf, net_buf_frags_len(buf));
/* Only accept IPv6 packets */
if (net_nbuf_family(buf) != AF_INET6) {
return NET_DROP;
}
if (!net_6lo_compress(buf, true, NULL)) {
NET_DBG("Packet compression failed");
return NET_DROP;
}
net_if_queue_tx(ctxt->iface, buf);
return NET_OK;
}
static u8_t *recv_cb(u8_t *buf, size_t *off)
{
struct slip_context *slip =
CONTAINER_OF(buf, struct slip_context, buf);
size_t i;
if (!slip->init_done) {
*off = 0;
return buf;
}
for (i = 0; i < *off; i++) {
if (slip_input_byte(slip, buf[i])) {
#if SYS_LOG_LEVEL >= SYS_LOG_LEVEL_DEBUG
struct net_buf *frag = slip->rx->frags;
int bytes = net_buf_frags_len(frag);
int count = 0;
while (bytes && frag) {
char msg[8 + 1];
snprintf(msg, sizeof(msg), ">slip %2d", count);
hexdump(msg, frag->data, frag->len, 0);
frag = frag->frags;
count++;
}
SYS_LOG_DBG("[%p] received data %d bytes", slip, bytes);
#endif
process_msg(slip);
break;
}
}
*off = 0;
return buf;
}
static enum net_verdict net_dhcpv4_input(struct net_conn *conn,
struct net_buf *buf,
void *user_data)
{
struct dhcp_msg *msg;
struct net_buf *frag;
struct net_if *iface;
uint8_t msg_type;
uint8_t min;
uint16_t pos;
if (!conn) {
NET_DBG("Invalid connection");
return NET_DROP;
}
if (!buf || !buf->frags) {
NET_DBG("Invalid buffer, no fragments");
return NET_DROP;
}
iface = net_nbuf_iface(buf);
if (!iface) {
return NET_DROP;
}
frag = buf->frags;
min = NET_IPV4UDPH_LEN + sizeof(struct dhcp_msg);
/*
* If the message is not DHCP then continue passing to
* related handlers.
*/
if (net_buf_frags_len(frag) < min) {
NET_DBG("Input msg is not related to DHCPv4");
return NET_CONTINUE;
}
msg = (struct dhcp_msg *)(frag->data + NET_IPV4UDPH_LEN);
NET_DBG("received dhcp msg [op=0x%x htype=0x%x hlen=%u xid=0x%x "
"secs=%u flags=0x%x ciaddr=%d.%d.%d.%d yiaddr=%d.%d.%d.%d "
"siaddr=%d.%d.%d.%d giaddr=%d.%d.%d.%d chaddr=%s]",
msg->op, msg->htype, msg->hlen, ntohl(msg->xid),
msg->secs, msg->flags,
msg->ciaddr[0], msg->ciaddr[1], msg->ciaddr[2], msg->ciaddr[3],
msg->yiaddr[0], msg->yiaddr[1], msg->yiaddr[2], msg->yiaddr[3],
msg->siaddr[0], msg->siaddr[1], msg->siaddr[2], msg->siaddr[3],
msg->giaddr[0], msg->giaddr[1], msg->giaddr[2], msg->giaddr[3],
net_sprint_ll_addr(msg->chaddr, 6));
if (!(msg->op == DHCPV4_MSG_BOOT_REPLY &&
iface->dhcpv4.xid == ntohl(msg->xid) &&
!memcmp(msg->chaddr, iface->link_addr.addr,
iface->link_addr.len))) {
NET_DBG("Unexpected op (%d), xid (%x vs %x) or chaddr",
msg->op, iface->dhcpv4.xid, ntohl(msg->xid));
goto drop;
}
memcpy(iface->dhcpv4.requested_ip.s4_addr, msg->yiaddr,
sizeof(msg->yiaddr));
/* sname, file are not used at the moment, skip it */
frag = net_nbuf_skip(frag, min, &pos, SIZE_OF_SNAME + SIZE_OF_FILE);
if (!frag && pos) {
goto drop;
}
if (parse_options(iface, frag, pos, &msg_type) == NET_DROP) {
NET_DBG("Invalid Options");
goto drop;
}
net_nbuf_unref(buf);
handle_dhcpv4_reply(iface, msg_type);
return NET_OK;
drop:
return NET_DROP;
}
