Hi Grzegorz, On Thursday 12 March 2015 18:07:32 Grzegorz Kolodziejczyk wrote: > This patch adds general functionality of bnep connect and listen. > --- > tools/bneptest.c | 711 > ++++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 707 > insertions(+), 4 deletions(-) > > diff --git a/tools/bneptest.c b/tools/bneptest.c > index 619427b..b694055 100644 > --- a/tools/bneptest.c > +++ b/tools/bneptest.c > @@ -26,33 +26,570 @@ > #endif > > #include <stdio.h> > +#include <signal.h> > #include <stdlib.h> > #include <getopt.h> > +#include <stdbool.h> > +#include <errno.h> > +#include <unistd.h> > +#include <sys/ioctl.h> > +#include <net/if.h> > +#include <linux/sockios.h> > +#include <netinet/in.h> > +#include <linux/if_bridge.h> > + > +#include <bluetooth/bluetooth.h> > +#include <bluetooth/hci.h> > +#include <bluetooth/hci_lib.h> > > #include <glib.h> > > #include "src/log.h" > +#include "btio/btio.h" > +#include "lib/bnep.h" > +#include "profiles/network/bnep.h" > + > +enum { > + MODE_NONE, > + MODE_CONNECT, > + MODE_LISTEN, just connect/listen mode should be enough > +}; > > static GMainLoop *mloop; > +static GIOChannel *bnep_io; > + > +static int mode; > +static bool no_close_after_disconn; > +static int send_frame_timeout; > + > +static bdaddr_t src_addr, dst_addr; > +static char iface[16]; > +static char bridge[16]; > +static bool send_ctrl_msg_type_set; > +static uint8_t ctrl_msg_type; > +static bool send_bnep_msg_type_set; > +static uint8_t bnep_msg_type; > +static int ctrl_msg_retransmition_nb; > +static int bnep_msg_retransmission_nb; > +static uint16_t local_role = BNEP_SVC_PANU; > +static uint16_t remote_role = BNEP_SVC_NAP; > +static uint16_t ntw_proto_down_range; > +static uint16_t ntw_proto_up_range = 0xdc05; > +static uint16_t ntw_proto_type; > +static uint8_t mcast_addr_down_range[6]; > +static uint8_t mcast_addr_up_range[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, > 0xff }; +static uint8_t src_hw_addr[6]; > +static uint8_t dst_hw_addr[6]; > +static uint8_t general_frame_payload[] = "abcdef0123456789_bnep_test_data"; > +static struct bnep *session; statics should be explicitly initialized to zero/null as well (not really needed but we have this convention in userspace code). > + > + > +static int set_forward_delay(int sk) > +{ > + unsigned long args[4] = { BRCTL_SET_BRIDGE_FORWARD_DELAY, 0, 0, 0 }; > + struct ifreq ifr; > + > + memset(&ifr, 0, sizeof(ifr)); > + strncpy(ifr.ifr_name, bridge, IFNAMSIZ); > + ifr.ifr_data = (char *) args; > + > + if (ioctl(sk, SIOCDEVPRIVATE, &ifr) < 0) { > + error("setting forward delay failed: %d (%s)", > + errno, strerror(errno)); > + return -1; > + } > + > + return 0; > +} > + > +static int nap_create_bridge(void) > +{ > + int sk, err; > + > + sk = socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0); > + if (sk < 0) > + return -EOPNOTSUPP; > + > + if (ioctl(sk, SIOCBRADDBR, bridge) < 0) { > + err = -errno; > + if (err != -EEXIST) { just check errno here. > + close(sk); > + return -EOPNOTSUPP; > + } > + } > + > + err = set_forward_delay(sk); > + if (err < 0) { > + printf("failed to set forward delay\n"); > + ioctl(sk, SIOCBRDELBR, bridge); > + } > + > + close(sk); > + > + return err; > +} > + > +static int cleanup(void) > +{ > + bnep_cleanup(); > + > + if (mode == MODE_LISTEN) > + bnep_server_delete(bridge, iface, &dst_addr); > + > + if (bnep_io) { > + g_io_channel_shutdown(bnep_io, TRUE, NULL); > + g_io_channel_unref(bnep_io); bnep_io = NULL here to avoid dangling pointer. > + } > + > + return 0; > +} > + > +static gboolean bnep_watchdog_cb(GIOChannel *chan, GIOCondition cond, > + gpointer user_data) > +{ > + printf("%s\n", __func__); > + > + if (no_close_after_disconn) > + return FALSE; > + > + /* Cleanup since it's called when disconnected l2cap */ > + if (cleanup() < 0) { > + printf("cleanup went wrong...\n"); > + return FALSE; > + } > + > + g_main_loop_quit(mloop); > + return FALSE; > +} > + > +static ssize_t send_compressed_frame(int sk, uint8_t type) > +{ > + uint8_t frame[100]; > + > + printf("%s\n", __func__); > + > + if (send_frame_timeout > 0) { > + printf("waiting %d seconds before sending msg\n", > + send_frame_timeout); > + sleep(send_frame_timeout); > + } > + > + frame[0] = type; > + memcpy(&frame[1], dst_hw_addr, sizeof(dst_hw_addr)); > + memcpy(&frame[7], src_hw_addr, sizeof(src_hw_addr)); > + frame[13] = ntw_proto_type & 0xff; > + frame[14] = (ntw_proto_type >> 8); > + memcpy(&frame[15], general_frame_payload, > + sizeof(general_frame_payload)); > + > + /* TODO - set frame payload by user */ > + return send(sk, frame, 15 + sizeof(general_frame_payload), 0); > +} > + > +static ssize_t send_general_frame(int sk) > +{ > + uint8_t frame[100]; > + > + printf("%s\n", __func__); > + > + if (send_frame_timeout > 0) { > + printf("waiting %d seconds before sending msg\n", > + send_frame_timeout); > + sleep(send_frame_timeout); > + } > + > + frame[0] = BNEP_GENERAL; > + memcpy(&frame[1], dst_hw_addr, sizeof(dst_hw_addr)); > + memcpy(&frame[7], src_hw_addr, sizeof(src_hw_addr)); > + frame[13] = ntw_proto_type & 0xff; > + frame[14] = (ntw_proto_type >> 8); > + memcpy(&frame[15], general_frame_payload, > + sizeof(general_frame_payload)); > + > + /* TODO - set frame payload by user */ > + return send(sk, frame, 15 + sizeof(general_frame_payload), 0); > +} > + > +static ssize_t send_ctrl_frame(int sk) > +{ > + /* Max buff size = type(1byte) + ctrl(1byte) + len(2byte) + > + * mcast_addr_down(6byte) + mcast_addr_up(6byte) */ /* * foo * bar */ > + uint8_t buff[16]; > + int err; > + > + printf("%s\n", __func__); > + > + if (send_frame_timeout > 0) { > + printf("waiting %d seconds before sending msg\n", > + send_frame_timeout); > + sleep(send_frame_timeout); > + } > + > + switch (ctrl_msg_type) { > + case BNEP_FILTER_NET_TYPE_SET: { > + struct bnep_set_filter_req *frame = (void *)buff; just move struct bnep_set_filter_req *frame = (void *)buff; to the top of function. Also there should be space after (void *) > + > + frame->type = BNEP_CONTROL; > + frame->ctrl = ctrl_msg_type; > + frame->len = htons(sizeof(ntw_proto_down_range) + > + sizeof(ntw_proto_up_range)); > + memcpy(frame->list, &ntw_proto_down_range, > + sizeof(ntw_proto_down_range)); > + memcpy(frame->list + sizeof(ntw_proto_down_range), > + &ntw_proto_up_range, sizeof(ntw_proto_up_range)); > + > + err = send(sk, frame, sizeof(*frame) + > + sizeof(ntw_proto_down_range) + > + sizeof(ntw_proto_up_range), 0); > + > + break; > + > + } > + case BNEP_FILTER_MULT_ADDR_SET: { > + struct bnep_set_filter_req *frame = (void *)buff; > + > + frame->type = BNEP_CONTROL; > + frame->ctrl = ctrl_msg_type; > + frame->len = htons(sizeof(mcast_addr_down_range) + > + sizeof(mcast_addr_up_range)); > + memcpy(frame->list, mcast_addr_down_range, > + sizeof(mcast_addr_down_range)); > + memcpy(frame->list + sizeof(mcast_addr_down_range), > + mcast_addr_up_range, sizeof(mcast_addr_up_range)); > + > + err = send(sk, frame, sizeof(*frame) + > + sizeof(mcast_addr_down_range) + > + sizeof(mcast_addr_up_range), 0); > + > + break; > + > + } > + default: > + err = -1; > + } > + > + return err; > +} > + > +static int send_bnep_frame(int sk) > +{ > + int err = 0; just set err in default case. > + > + switch (bnep_msg_type) { > + case BNEP_GENERAL: > + err = send_general_frame(sk); > + those empty lines are not needed. > + break; > + > + case BNEP_COMPRESSED: > + err = send_compressed_frame(sk, BNEP_COMPRESSED); > + > + break; > + > + case BNEP_COMPRESSED_SRC_ONLY: > + err = send_compressed_frame(sk, > + BNEP_COMPRESSED_SRC_ONLY); > + > + break; > + > + case BNEP_COMPRESSED_DST_ONLY: > + err = send_compressed_frame(sk, > + BNEP_COMPRESSED_DST_ONLY); > + > + break; > + > + default: > + printf("wrong bnep_msg_type 0x%02x\n", bnep_msg_type); > + } > + > + return err; > +} > + > +static void handle_bnep_msg_send(int sk) > +{ > + if (send_ctrl_msg_type_set) { > + do { > + if (send_ctrl_frame(sk) < 0) > + printf("sending ctrl frame error: %s (%d)\n", > + strerror(errno), errno); > + } while (!ctrl_msg_retransmition_nb--); This ! seems bogus. > + } > + > + if (send_bnep_msg_type_set) { > + do { > + if (send_bnep_frame(sk) < 0) > + printf("sending bnep frame error: %s (%d)\n", > + strerror(errno), errno); > + } while (!bnep_msg_retransmission_nb--); ditto. > + } > +} > + > +static gboolean setup_bnep_cb(GIOChannel *chan, GIOCondition cond, > + gpointer user_data) > +{ > + uint8_t packet[BNEP_MTU]; > + int sk, n, err; > + > + printf("%s\n", __func__); > + > + if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL)) { > + error("hangup or error or inval on BNEP socket"); > + return FALSE; > + } > + > + sk = g_io_channel_unix_get_fd(chan); > + > + /* Reading BNEP_SETUP_CONNECTION_REQUEST_MSG */ > + n = read(sk, packet, sizeof(packet)); > + if (n < 0) { double space before < > + error("read(): %s(%d)", strerror(errno), errno); > + return FALSE; > + } > + > + err = nap_create_bridge(); > + if (err < 0) { > + error("failed to create bridge: %s (%d)", strerror(-err), err); > + return FALSE; > + } > + > + if (bnep_server_add(sk, (err < 0) ? NULL : bridge, iface, &dst_addr, > + packet, n) < 0) { > + printf("server_connadd failed\n"); > + cleanup(); > + return FALSE; > + } > + > + g_io_add_watch(chan, G_IO_HUP | G_IO_ERR | G_IO_NVAL, bnep_watchdog_cb, > + NULL); > + > + handle_bnep_msg_send(sk); > + > + g_io_channel_unref(bnep_io); > + bnep_io = NULL; > + > + return FALSE; > +} > + > +static void connect_cb(GIOChannel *chan, GError *err, gpointer user_data) > +{ > + printf("%s\n", __func__); > + > + if (err) { > + error("%s", err->message); > + return; > + } > + > + g_io_add_watch(chan, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, > + setup_bnep_cb, NULL); > +} > + > +static void connected_client_cb(char *iface, int err, void *data) > +{ > + int sk = *((int *)data); Use PTR_TO_INT macro instead. > + > + printf("%s\n", __func__); > + > + free(data); > + > + handle_bnep_msg_send(sk); > +} > + > +static void disconnected_client_cb(void *data) > +{ > + printf("%s\n", __func__); > + > + if (no_close_after_disconn) > + return; > + > + /* Cleanup since it's called when disconnected l2cap */ > + if (cleanup() < 0) { > + printf("cleanup went wrong...\n"); > + return; > + } > + > + g_main_loop_quit(mloop); > +} > + > +static void connect_client_cb(GIOChannel *chan, GError *err, gpointer > user_data) +{ > + int perr; > + int *sk; > + > + sk = malloc(sizeof(*sk)); just use INT_TO_PTR to pass this as userdata. > + > + *sk = g_io_channel_unix_get_fd(bnep_io); > + > + session = bnep_new(*sk, local_role, remote_role, bridge); > + if (!session) { > + printf("cannot create bnep session\n"); > + free(sk); > + return; > + } > + > + perr = bnep_connect(session, connected_client_cb, > + disconnected_client_cb, sk, NULL); > + if (perr < 0) > + printf("cannot initiate bnep connection\n"); > +} > + > +static void confirm_cb(GIOChannel *chan, gpointer data) > +{ > + GError *err = NULL; > + char address[18]; > + > + printf("%s\n", __func__); > + > + bt_io_get(chan, &err, BT_IO_OPT_DEST_BDADDR, &dst_addr, BT_IO_OPT_DEST, > + address, BT_IO_OPT_INVALID); > + if (err) { > + error("%s", err->message); > + g_error_free(err); > + return; > + } > + > + printf("incoming connection from: %s\n", address); > + > + bnep_io = g_io_channel_ref(chan); > + g_io_channel_set_close_on_unref(bnep_io, TRUE); > + > + if (!bt_io_accept(bnep_io, connect_cb, NULL, NULL, &err)) { > + error("bt_io_accept: %s", err->message); > + g_error_free(err); missing bnep_io unref? > + } or you can just return on error and ref bnep_io here ? > +} > + > +static int bnep_server_listen(void) > +{ > + GError *gerr = NULL; > + > + printf("%s\n", __func__); > + > + bnep_io = bt_io_listen(NULL, confirm_cb, NULL, NULL, &gerr, > + BT_IO_OPT_SOURCE_BDADDR, &src_addr, > + BT_IO_OPT_PSM, BNEP_PSM, > + BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW, > + BT_IO_OPT_OMTU, BNEP_MTU, > + BT_IO_OPT_IMTU, BNEP_MTU, > + BT_IO_OPT_INVALID); > + > + if (!bnep_io) { > + printf("can't start server listening: err %s\n", gerr->message); > + g_error_free(gerr); > + return -1; > + } > + > + return 0; > +} > + > +static int bnep_client_connect(void) > +{ > + GError *gerr = NULL; > + char *dst_addr_str = batostr(&dst_addr); use ba2str and stack variable. > + > + printf("%s\n", __func__); > + > + printf("connecting %s\n", dst_addr_str); > + free(dst_addr_str); > + > + bnep_io = bt_io_connect(connect_client_cb, NULL, NULL, &gerr, > + BT_IO_OPT_SOURCE_BDADDR, &src_addr, > + BT_IO_OPT_DEST_BDADDR, &dst_addr, > + BT_IO_OPT_PSM, BNEP_PSM, > + BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_MEDIUM, > + BT_IO_OPT_OMTU, BNEP_MTU, > + BT_IO_OPT_IMTU, BNEP_MTU, > + BT_IO_OPT_INVALID); > + > + if (!bnep_io) { > + printf("cannot connect: err %s\n", gerr->message); > + g_error_free(gerr); > + return -1; > + } > + > + return 0; > +} > + > +static void exit_handler(int sig) > +{ > + printf("got sig = %d, cleaning up...\n", sig); > + > + if (cleanup() < 0) > + printf("cleanup failure...\n"); > + else > + printf("cleanup successful - exit\n"); > + > + exit(0); > +} > > static void usage(void) > { > printf("bneptest - BNEP testing ver %s\n", VERSION); > printf("Usage:\n" > - "\tbneptest [options]\n"); > + "\tbneptest [-i] -b <bridge name> -n <iface name>" > + " <connection mode> [send_ctrl_cmd] [options]\n" > + "\t-i hci dev number <hci number>, def. 0\n" > + "\t-b bridge name <string>\n" > + "\t-n interface name <string>\n"); > + printf("Connect Mode:\n" > + "\t-c connect <dst_addr>\n" > + "\t-r remote role <16 bit svc value>\n" > + "\t-l local role <16 bit svc valu>\n"); > + printf("Listen Mode:\n" > + "\t-s start server listening\n"); > + printf("Send control command:\n" > + "\t-t send message type <control msg type>, def. 0\n" > + "\t-e start network protocol type range <16 bit val>, def. 0\n" > + "\t-d end network protocol type range <16 bit val>, def. 1500\n" > + "\t-g start multicast addr range <xx:xx:xx:xx:xx:xx>, def. 0\n" > + "\t-j end multicast addr range <xx:xx:xx:xx:xx:xx>, def. f\n" > + "\t-y number of ctrl frame retransmission <integer>, def. 0\n" > + "\t-u number of bnep frame retransmission <integer>, def. 0\n"); > + printf("Send bnep generic frame:\n" > + "\t-w send bnep generic frame <bnep generic type>, def. 0\n" > + "\t-k set src mac addr <xx:xx:xx:xx:xx:xx>, def. 0\n" > + "\t-f set dst mac addr <xx:xx:xx:xx:xx:xx>, def. 0\n"); > + printf("Options:\n" > + "\t-T send message timeout after setup <seconds>\n" > + "\t-N don't close bneptest after disconnect\n"); > } > > static struct option main_options[] = { > - { "help", 0, 0, 'h' }, > + { "device", 1, 0, 'i' }, > + { "listen", 0, 0, 's' }, > + { "connect", 1, 0, 'c' }, > + { "snd_ctrl_msg_type", 1, 0, 't' }, > + { "snd_bnep_msg_type", 1, 0, 'w' }, > + { "src_hw_addr", 1, 0, 'k' }, > + { "dst_hw_addr", 1, 0, 'f' }, > + { "send_timeout", 1, 0, 'T' }, > + { "ntw_proto_down_range", 1, 0, 'd' }, > + { "ntw_proto_up_range", 1, 0, 'e' }, > + { "mcast_addr_down_range", 1, 0, 'g' }, > + { "mcast_addr_up_range", 1, 0, 'j' }, > + { "local_role", 1, 0, 'l' }, > + { "remote_role", 1, 0, 'r' }, > + { "bridge name", 1, 0, 'b' }, > + { "iface name", 1, 0, 'n' }, > + { "no_close", 0, 0, 'N' }, > + { "retrans_ctrl_nb", 0, 0, 'y' }, > + { "retrans_bnep_nb", 0, 0, 'u' }, > + { "help", 0, 0, 'h' }, > { 0, 0, 0, 0 } > }; > > int main(int argc, char *argv[]) > { > int opt; > + int err; > + bool is_set_b_name = false, is_set_i_name = false; > > DBG(""); > > + signal(SIGINT, exit_handler); > + > + hci_devba(0, &src_addr); > + bacpy(&src_addr, BDADDR_ANY); > + > mloop = g_main_loop_new(NULL, FALSE); > if (!mloop) { > printf("cannot create main loop\n"); > @@ -60,9 +597,133 @@ int main(int argc, char *argv[]) > exit(1); > } > > - while ((opt = getopt_long(argc, argv, "h", main_options, NULL)) > - != EOF) { > + while ((opt = getopt_long(argc, argv, > + "+i:c:b:n:t:T:d:e:g:j:k:f:w:l:r:y:u:Nsh", > + main_options, NULL)) != EOF) { > switch (opt) { > + case 'i': > + if (!strncmp(optarg, "hci", 3)) > + hci_devba(atoi(optarg + 3), &src_addr); > + else > + str2ba(optarg, &src_addr); > + > + break; > + Those empty lines are not needed. > + case 's': > + mode = MODE_LISTEN; > + break; > + > + case 'c': > + str2ba(optarg, &dst_addr); > + mode = MODE_CONNECT; > + > + break; > + > + case 't': > + send_ctrl_msg_type_set = true; > + ctrl_msg_type = atoi(optarg); > + > + break; > + > + case 'w': > + send_bnep_msg_type_set = true; > + bnep_msg_type = atoi(optarg); > + > + break; > + > + case 'k': { > + int i = 0; declare it at the top > + > + for (; i <= 5; i++, optarg += 3) initialize i in for loop. for (i = 0; ...) > + src_hw_addr[i] = strtol(optarg, NULL, 16); > + > + break; > + } > + > + case 'f': { > + int i = 0; ditto. > + > + for (; i <= 5; i++, optarg += 3) > + dst_hw_addr[i] = strtol(optarg, NULL, 16); > + > + break; > + } > + > + case 'T': > + send_frame_timeout = atoi(optarg); > + > + break; > + > + case 'd': > + ntw_proto_down_range = htons(atoi(optarg)); > + > + break; > + > + case 'e': > + ntw_proto_up_range = htons(atoi(optarg)); > + > + break; > + > + case 'g': { > + int i = 0; > + ditto. > + for (i = 5; i >= 0; i--, optarg += 3) > + mcast_addr_down_range[i] = > + strtol(optarg, NULL, 16); > + > + break; > + } > + > + case 'j': { > + int i = 0; ditto. > + > + for (i = 5; i >= 0; i--, optarg += 3) > + mcast_addr_up_range[i] = > + strtol(optarg, NULL, 16); > + > + break; > + } > + > + case 'l': > + local_role = atoi(optarg); > + > + break; > + > + case 'r': > + remote_role = atoi(optarg); > + > + break; > + > + case 'b': > + strncpy(bridge, optarg, 16); > + bridge[15] = '\0'; > + is_set_b_name = true; > + > + break; > + > + case 'n': > + strncpy(iface, optarg, 14); > + strcat(iface, "\%d"); > + iface[15] = '\0'; > + is_set_i_name = true; > + > + break; > + > + case 'N': > + no_close_after_disconn = true; > + > + break; > + > + case 'y': > + ctrl_msg_retransmition_nb = atoi(optarg); > + > + break; > + > + case 'u': > + bnep_msg_retransmission_nb = atoi(optarg); > + > + break; > + > case 'h': > default: > usage(); > @@ -70,5 +731,47 @@ int main(int argc, char *argv[]) > } > } > > + if (!is_set_b_name || !is_set_i_name) { > + printf("bridge, interface name must be set!\n"); > + exit(1); > + } > + > + switch (mode) { > + case MODE_CONNECT: > + err = bnep_init(); > + if (err < 0) { > + printf("cannot initialize bnep\n"); > + exit(1); > + } > + err = bnep_client_connect(); > + if (err < 0) > + exit(1); > + > + break; > + > + case MODE_LISTEN: > + err = bnep_init(); > + if (err < 0) { > + printf("cannot initialize bnep\n"); > + exit(1); > + } > + err = bnep_server_listen(); > + if (err < 0) > + exit(1); > + > + break; > + > + case MODE_NONE: > + default: > + printf("connect/listen mode not set, exit...\n"); > + exit(1); > + } > + > + g_main_loop_run(mloop); > + > + printf("Done\n"); > + > + g_main_loop_unref(mloop); > + > return 0; > } -- Szymon K. Janc szymon.janc@xxxxxxxxx -- To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in the body of a message to majordomo@xxxxxxxxxxxxxxx More majordomo info at http://vger.kernel.org/majordomo-info.html