static uint8_t *recv_cb(uint8_t *buf, size_t *off)
{
struct btp_hdr *cmd = (void *) buf;
uint8_t *new_buf;
uint16_t len;
if (*off < sizeof(*cmd)) {
return buf;
}
len = sys_le16_to_cpu(cmd->len);
if (len > BTP_MTU - sizeof(*cmd)) {
printk("BT tester: invalid packet length\n");
*off = 0;
return buf;
}
if (*off < sizeof(*cmd) + len) {
return buf;
}
new_buf = nano_fifo_get(&avail_queue, TICKS_NONE);
if (!new_buf) {
printk("BT tester: RX overflow\n");
*off = 0;
return buf;
}
nano_fifo_put(&cmds_queue, buf);
*off = 0;
return new_buf;
}
void *get_scratch_packet(void)
{
void *packet = nano_fifo_get(&scratch_q_packets_fifo, TICKS_NONE);
__ASSERT_NO_MSG(packet);
return packet;
}
static void rx_fiber(void)
{
BT_DBG("");
while (true) {
struct net_buf *buf;
buf = nano_fifo_get(&h5.rx_queue, TICKS_UNLIMITED);
hexdump("=> ", buf->data, buf->len);
if (!memcmp(buf->data, sync_req, sizeof(sync_req))) {
if (h5.link_state == ACTIVE) {
/* TODO Reset H5 */
}
h5_send(sync_rsp, HCI_3WIRE_LINK_PKT, sizeof(sync_rsp));
} else if (!memcmp(buf->data, sync_rsp, sizeof(sync_rsp))) {
if (h5.link_state == ACTIVE) {
/* TODO Reset H5 */
}
h5.link_state = INIT;
h5_set_txwin(conf_req);
h5_send(conf_req, HCI_3WIRE_LINK_PKT, sizeof(conf_req));
} else if (!memcmp(buf->data, conf_req, 2)) {
/*
* The Host sends Config Response messages without a
* Configuration Field.
*/
h5_send(conf_rsp, HCI_3WIRE_LINK_PKT, sizeof(conf_rsp));
/* Then send Config Request with Configuration Field */
h5_set_txwin(conf_req);
h5_send(conf_req, HCI_3WIRE_LINK_PKT, sizeof(conf_req));
} else if (!memcmp(buf->data, conf_rsp, 2)) {
h5.link_state = ACTIVE;
if (buf->len > 2) {
/* Configuration field present */
h5.tx_win = (buf->data[2] & 0x07);
}
BT_DBG("Finished H5 configuration, tx_win %u",
h5.tx_win);
} else {
BT_ERR("Not handled yet %x %x",
buf->data[0], buf->data[1]);
}
net_buf_unref(buf);
}
}
char *zephyr_getline(void)
{
static struct uart_console_input *cmd;
/* Recycle cmd buffer returned previous time */
if (cmd != NULL)
{
nano_fifo_put(&free_queue, cmd);
}
cmd = nano_fifo_get(&used_queue, TICKS_UNLIMITED);
return cmd->line;
}
int
sol_mainloop_event_post(const struct mainloop_event *me)
{
struct me_fifo_entry *mfe;
mfe = nano_fifo_get(&_sol_mainloop_free_events, TICKS_NONE);
SOL_NULL_CHECK(mfe, -ENOMEM);
mfe->me = *me;
nano_fifo_put(&_sol_mainloop_pending_events, mfe);
return 0;
}
static void conn_tx_fiber(int arg1, int arg2)
{
struct bt_conn *conn = (struct bt_conn *)arg1;
struct net_buf *buf;
BT_DBG("Started for handle %u", conn->handle);
while (conn->state == BT_CONN_CONNECTED) {
/* Get next ACL packet for connection */
buf = nano_fifo_get(&conn->tx_queue, TICKS_UNLIMITED);
if (conn->state != BT_CONN_CONNECTED) {
net_buf_unref(buf);
break;
}
if (!send_buf(conn, buf)) {
net_buf_unref(buf);
}
}
BT_DBG("handle %u disconnected - cleaning up", conn->handle);
/* Give back any allocated buffers */
while ((buf = nano_fifo_get(&conn->tx_queue, TICKS_NONE))) {
net_buf_unref(buf);
}
/* Return any unacknowledged packets */
if (conn->pending_pkts) {
while (conn->pending_pkts--) {
nano_fiber_sem_give(bt_conn_get_pkts(conn));
}
}
bt_conn_reset_rx_state(conn);
BT_DBG("handle %u exiting", conn->handle);
bt_conn_unref(conn);
}
/* Delayed fiber taking care about retransmitting packets */
static void retx_fiber(int arg1, int arg2)
{
ARG_UNUSED(arg1);
ARG_UNUSED(arg2);
BT_DBG("unack_queue_len %u", unack_queue_len);
h5.retx_to = NULL;
if (unack_queue_len) {
struct nano_fifo tmp_queue;
struct net_buf *buf;
nano_fifo_init(&tmp_queue);
/* Queue to temperary queue */
while ((buf = nano_fifo_get(&h5.tx_queue, TICKS_NONE))) {
nano_fifo_put(&tmp_queue, buf);
}
/* Queue unack packets to the beginning of the queue */
while ((buf = nano_fifo_get(&h5.unack_queue, TICKS_NONE))) {
/* include also packet type */
net_buf_push(buf, sizeof(uint8_t));
nano_fifo_put(&h5.tx_queue, buf);
h5.tx_seq = (h5.tx_seq - 1) & 0x07;
unack_queue_len--;
}
/* Queue saved packets from temp queue */
while ((buf = nano_fifo_get(&tmp_queue, TICKS_NONE))) {
nano_fifo_put(&h5.tx_queue, buf);
}
/* Analyze stack */
stack_analyze("retx_stack", retx_stack, sizeof(retx_stack));
}
}
static void process_unack(void)
{
uint8_t next_seq = h5.tx_seq;
uint8_t number_removed = unack_queue_len;
if (!unack_queue_len) {
return;
}
BT_DBG("rx_ack %u tx_ack %u tx_seq %u unack_queue_len %u",
h5.rx_ack, h5.tx_ack, h5.tx_seq, unack_queue_len);
while (unack_queue_len > 0) {
if (next_seq == h5.rx_ack) {
/* Next sequence number is the same as last received
* ack number
*/
break;
}
number_removed--;
/* Similar to (n - 1) % 8 with unsigned conversion */
next_seq = (next_seq - 1) & 0x07;
}
if (next_seq != h5.rx_ack) {
BT_ERR("Wrong sequence: rx_ack %u tx_seq %u next_seq %u",
h5.rx_ack, h5.tx_seq, next_seq);
}
BT_DBG("Need to remove %u packet from the queue", number_removed);
while (number_removed) {
struct net_buf *buf = nano_fifo_get(&h5.unack_queue, TICKS_NONE);
if (!buf) {
BT_ERR("Unack queue is empty");
break;
}
/* TODO: print or do something with packet */
BT_DBG("Remove buf from the unack_queue");
net_buf_unref(buf);
unack_queue_len--;
number_removed--;
}
}
/* Called by application when it wants to receive network data */
struct net_buf *net_receive(struct net_context *context)
{
struct nano_fifo *rx_queue = net_context_get_queue(context);
struct net_tuple *tuple;
int ret = 0;
tuple = net_context_get_tuple(context);
if (!tuple) {
return NULL;
}
switch (tuple->ip_proto) {
case IPPROTO_UDP:
if (!net_context_get_receiver_registered(context)) {
struct simple_udp_connection *udp =
net_context_get_udp_connection(context);
ret = simple_udp_register(udp, tuple->local_port,
#ifdef CONFIG_NETWORKING_WITH_IPV6
(uip_ip6addr_t *)&tuple->remote_addr->in6_addr,
#else
(uip_ip4addr_t *)&tuple->remote_addr->in_addr,
#endif
tuple->remote_port,
udp_packet_receive,
context);
if (!ret) {
NET_DBG("UDP connection listener failed\n");
ret = -ENOENT;
break;
}
}
net_context_set_receiver_registered(context);
ret = 0;
break;
case IPPROTO_TCP:
NET_DBG("TCP not yet supported\n");
ret = -EINVAL;
break;
case IPPROTO_ICMPV6:
NET_DBG("ICMPv6 not yet supported\n");
ret = -EINVAL;
break;
}
return nano_fifo_get(rx_queue);
}
static void tx_fiber(void)
{
BT_DBG("");
/* FIXME: make periodic sending */
h5_send(sync_req, HCI_3WIRE_LINK_PKT, sizeof(sync_req));
while (true) {
struct net_buf *buf;
uint8_t type;
BT_DBG("link_state %u", h5.link_state);
switch (h5.link_state) {
case UNINIT:
/* FIXME: send sync */
fiber_sleep(10);
break;
case INIT:
/* FIXME: send conf */
fiber_sleep(10);
break;
case ACTIVE:
buf = nano_fifo_get(&h5.tx_queue, TICKS_UNLIMITED);
type = h5_get_type(buf);
h5_send(buf->data, type, buf->len);
/* buf is dequeued from tx_queue and queued to unack
* queue.
*/
nano_fifo_put(&h5.unack_queue, buf);
unack_queue_len++;
if (h5.retx_to) {
fiber_delayed_start_cancel(h5.retx_to);
}
h5.retx_to = fiber_delayed_start(retx_stack,
sizeof(retx_stack),
retx_fiber, 0, 0, 7, 0,
H5_TX_ACK_TIMEOUT);
break;
}
}
}
void tester_init(void)
{
int i;
nano_fifo_init(&cmds_queue);
nano_fifo_init(&avail_queue);
for (i = 0; i < CMD_QUEUED; i++) {
nano_fifo_put(&avail_queue, &cmd_buf[i * BTP_MTU]);
}
task_fiber_start(stack, STACKSIZE, cmd_handler, 0, 0, 7, 0);
uart_pipe_register(nano_fifo_get(&avail_queue, TICKS_NONE),
BTP_MTU, recv_cb);
printk("BT tester initialized\n");
}
