libnexio.c

/***************************************************************************
 *                                                                         *
 *          ###########   ###########   ##########    ##########           *
 *         ############  ############  ############  ############          *
 *         ##            ##            ##   ##   ##  ##        ##          *
 *         ##            ##            ##   ##   ##  ##        ##          *
 *         ###########   ####  ######  ##   ##   ##  ##    ######          *
 *          ###########  ####  #       ##   ##   ##  ##    #    #          *
 *                   ##  ##    ######  ##   ##   ##  ##    #    #          *
 *                   ##  ##    #       ##   ##   ##  ##    #    #          *
 *         ############  ##### ######  ##   ##   ##  ##### ######          *
 *         ###########    ###########  ##   ##   ##   ##########           *
 *                                                                         *
 *            S E C U R E   M O B I L E   N E T W O R K I N G              *
 *                                                                         *
 * This file is part of NexMon.                                            *
 *                                                                         *
 * Copyright (c) 2016 NexMon Team                                          *
 *                                                                         *
 * NexMon is free software: you can redistribute it and/or modify          *
 * it under the terms of the GNU General Public License as published by    *
 * the Free Software Foundation, either version 3 of the License, or       *
 * (at your option) any later version.                                     *
 *                                                                         *
 * NexMon is distributed in the hope that it will be useful,               *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
 * GNU General Public License for more details.                            *
 *                                                                         *
 * You should have received a copy of the GNU General Public License       *
 * along with NexMon. If not, see <http://www.gnu.org/licenses/>.          *
 *                                                                         *
 **************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <byteswap.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h>

#include <sys/ioctl.h>
#ifdef BUILD_ON_RPI
#include <linux/if.h>
#else
#include <net/if.h>
#endif
#include <stdbool.h>
#include <errno.h>

#include <netinet/in.h>
#include <netinet/ip.h>

#include <linux/netlink.h>

#define WLC_IOCTL_MAGIC          0x14e46c77
#define DHD_IOCTL_MAGIC          0x00444944

#define WLC_GET_MAGIC                     0
#define DHD_GET_MAGIC                     0

#define NEXUDP_IOCTL            		  0
#define NEXUDP_INJECT_RADIOTAP  		  1

#define NETLINK_USER                     31

#define IPADDR(a,b,c,d) ((d) << 24 | (c) << 16 | (b) << 8 | (a))

#define NEXIO_TYPE_IOCTL		0
#define NEXIO_TYPE_UDP			1
#define NEXIO_TYPE_NETLINK		2

struct nexio {
	int type;
	struct ifreq *ifr;
	int sock_rx_ioctl;
	int sock_rx_frame;
	int sock_tx;
	unsigned int securitycookie;
};

struct nex_ioctl {
    unsigned int cmd;   	/* common ioctl definition */
    void *buf;  			/* pointer to user buffer */
    unsigned int len;   	/* length of user buffer */
    bool set;   			/* get or set request (optional) */
    unsigned int used;  	/* bytes read or written (optional) */
    unsigned int needed;    /* bytes needed (optional) */
    unsigned int driver;    /* to identify target driver */
};

struct nexudp_header {
    char nex[3];
    char type;
    int securitycookie;
} __attribute__((packed));

struct nexudp_ioctl_header {
    struct nexudp_header nexudphdr;
    unsigned int cmd;
    unsigned int set;
    char payload[1];
} __attribute__((packed));


static int
__nex_driver_io(struct ifreq *ifr, struct nex_ioctl *ioc)
{
    int s;
    int ret = 0;

    /* pass ioctl data */
    ifr->ifr_data = (void *) ioc;

    /* open socket to kernel */
    if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
        fprintf(stderr, "error: socket\n");

    ret = ioctl(s, SIOCDEVPRIVATE, ifr);
    if (ret < 0 && errno != EAGAIN)
        fprintf(stderr, "%s: error (%d: %s)\n", __FUNCTION__, errno, strerror(errno));

    /* cleanup */
    close(s);
    return ret;
}

static int
__nex_driver_udp(struct nexio *nexio, struct nex_ioctl *ioc)
{
	int frame_len = ioc->len + sizeof(struct nexudp_ioctl_header) - sizeof(char);
	int rx_frame_len = 0;
	struct nexudp_ioctl_header *frame = (struct nexudp_ioctl_header *) malloc(frame_len);
	int ret = 0;

	memcpy(&frame->nexudphdr.nex, "NEX", 3);
	frame->nexudphdr.type = NEXUDP_IOCTL;
	frame->nexudphdr.securitycookie = nexio->securitycookie;

	frame->cmd = ioc->cmd;
	frame->set = ioc->set;

	memcpy(frame->payload, ioc->buf, ioc->len);

	send(nexio->sock_tx, frame, frame_len, 0);

	rx_frame_len = recv(nexio->sock_rx_ioctl, frame, frame_len, 0);

	if (ioc->set == 0 && rx_frame_len > 0 && *(unsigned int *) frame == ioc->cmd) {
		memcpy(ioc->buf, ((char *) frame) + sizeof(frame->cmd) + sizeof(frame->set),
			(rx_frame_len - sizeof(frame->cmd) - sizeof(frame->set)) < ioc->len ?
            (rx_frame_len - sizeof(frame->cmd) - sizeof(frame->set)) : ioc->len);
	}

	free(frame);

	if (rx_frame_len < 0) {
		ret = -1;
		fprintf(stderr, "ERR (%s): no valid answer received\n", __FUNCTION__);
	}

	return ret;
}

static int
__nex_driver_netlink(struct nexio *nexio, struct nex_ioctl *ioc)
{
        int frame_len = ioc->len + sizeof(struct nexudp_ioctl_header) - sizeof(char);
        int rx_frame_len = 0;
        struct nexudp_ioctl_header *frame;
        int ret = 0;

        struct iovec iov = { 0 };
        struct msghdr msg = { 0 };

        struct nlmsghdr *nlh = (struct nlmsghdr *) malloc(NLMSG_SPACE(frame_len));
        memset(nlh, 0, NLMSG_SPACE(frame_len));
        nlh->nlmsg_len = NLMSG_SPACE(frame_len);
        nlh->nlmsg_pid = getpid();
        nlh->nlmsg_flags = 0;
        frame = (struct nexudp_ioctl_header *) NLMSG_DATA(nlh);

        memcpy(&frame->nexudphdr.nex, "NEX", 3);
        frame->nexudphdr.type = NEXUDP_IOCTL;
        frame->nexudphdr.securitycookie = nexio->securitycookie;

        frame->cmd = ioc->cmd;
        frame->set = ioc->set;

        memcpy(frame->payload, ioc->buf, ioc->len);

//	printf("%s: before send. nlh->nlmsg_len: %d ioc->len: %d\n", __FUNCTION__, nlh->nlmsg_len, ioc->len);
        send(nexio->sock_tx, nlh, nlh->nlmsg_len, 0);

        rx_frame_len = recv(nexio->sock_rx_ioctl, nlh, nlh->nlmsg_len, 0);

//	printf("%s: framelen %d %d %s\n", __FUNCTION__, rx_frame_len, nlh->nlmsg_len, (char *) frame);

        if (ioc->set == 0 && rx_frame_len > 0 && frame->cmd == ioc->cmd) {
                memcpy(ioc->buf, frame->payload,
                        (rx_frame_len - sizeof(struct nexudp_ioctl_header) + sizeof(char)) < ioc->len ?
            (rx_frame_len - sizeof(struct nexudp_ioctl_header) + sizeof(char)) : ioc->len);
        }

        free(nlh);

        if (rx_frame_len < 0) {
                ret = -1;
                fprintf(stderr, "ERR (%s): no valid answer received\n", __FUNCTION__);
        }

        return ret;
}

/* This function is called by ioctl_setinformation_fe or ioctl_queryinformation_fe
 * for executing  remote commands or local commands
 */
int
nex_ioctl(struct nexio *nexio, int cmd, void *buf, int len, bool set)
{
    struct nex_ioctl ioc;
    int ret = 0;

    /* do it */
    ioc.cmd = cmd;
    ioc.buf = buf;
    ioc.len = len;
    ioc.set = set;
    ioc.driver = WLC_IOCTL_MAGIC;


    if (nexio != 0) {
	switch(nexio->type) {
		case NEXIO_TYPE_IOCTL:
			ret = __nex_driver_io(nexio->ifr, &ioc);
			break;
		case NEXIO_TYPE_UDP:
			ret = __nex_driver_udp(nexio, &ioc);
			break;
		case NEXIO_TYPE_NETLINK:
			ret = __nex_driver_netlink(nexio, &ioc);
			break;
		default:
			fprintf(stderr, "%s: not initialized correctly\n", __FUNCTION__);
	}
    } else {
        fprintf(stderr, "%s: not initialized\n", __FUNCTION__);
    }

    if (ret < 0 && cmd != WLC_GET_MAGIC)
        ret = -1;

    return ret;
}

struct nexio *
nex_init_ioctl(const char *ifname)
{
	struct nexio *nexio = (struct nexio *) malloc(sizeof(struct nexio));
	memset(nexio, 0, sizeof(struct nexio));

	nexio->ifr = (struct ifreq *) malloc(sizeof(struct ifreq));
	memset(nexio->ifr, 0, sizeof(struct ifreq));
	snprintf(nexio->ifr->ifr_name, sizeof(nexio->ifr->ifr_name), "%s", ifname);

	nexio->type = NEXIO_TYPE_IOCTL;

	return nexio;
}

struct nexio *
nex_init_udp(unsigned int securitycookie, unsigned int txip)
{
	struct nexio *nexio = (struct nexio *) malloc(sizeof(struct nexio));
	memset(nexio, 0, sizeof(struct nexio));

	nexio->securitycookie = securitycookie;

	struct sockaddr_in *sin_tx = (struct sockaddr_in *) malloc(sizeof(struct sockaddr_in));
	struct sockaddr_in *sin_rx_ioctl = (struct sockaddr_in *) malloc(sizeof(struct sockaddr_in));
	struct sockaddr_in *sin_rx_frame = (struct sockaddr_in *) malloc(sizeof(struct sockaddr_in));

	memset(sin_tx, 0, sizeof(struct sockaddr_in));
	memset(sin_rx_ioctl, 0, sizeof(struct sockaddr_in));
	memset(sin_rx_frame, 0, sizeof(struct sockaddr_in));

	sin_tx->sin_family = AF_INET;
	sin_tx->sin_port = htons(5500);
	sin_tx->sin_addr.s_addr = txip;

	sin_rx_ioctl->sin_family = AF_INET;
	sin_rx_ioctl->sin_port = htons(5500);
	sin_rx_ioctl->sin_addr.s_addr = IPADDR(255,255,255,255);

	sin_rx_frame->sin_family = AF_INET;
	sin_rx_frame->sin_port = htons(5600);
	sin_rx_frame->sin_addr.s_addr = IPADDR(255,255,255,255);

    nexio->sock_tx = socket(sin_tx->sin_family, SOCK_DGRAM, IPPROTO_UDP);
    nexio->sock_rx_ioctl = socket(sin_rx_ioctl->sin_family, SOCK_DGRAM, IPPROTO_UDP);
    nexio->sock_rx_frame = socket(sin_rx_frame->sin_family, SOCK_DGRAM, IPPROTO_UDP);

    // Enable broadcasts for transmit socket
    int broadcast_enable = 1;
    setsockopt(nexio->sock_tx, SOL_SOCKET, SO_BROADCAST, &broadcast_enable, sizeof(broadcast_enable));

    // Set 1 second timeout on ioctl receive socket
    struct timeval tv = {
    	.tv_sec = 1,
    	.tv_usec = 0
    };
    setsockopt(nexio->sock_rx_ioctl, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));

    connect(nexio->sock_tx, (struct sockaddr *) sin_tx, sizeof(struct sockaddr));
    bind(nexio->sock_rx_ioctl, (struct sockaddr *) sin_rx_ioctl, sizeof(struct sockaddr));
    bind(nexio->sock_rx_frame, (struct sockaddr *) sin_rx_frame, sizeof(struct sockaddr));

	nexio->type = NEXIO_TYPE_UDP;

	return nexio;
}

struct nexio *
nex_init_netlink(void)
{
//    printf("%s: Enter\n", __FUNCTION__);
    int err = 0;
    struct nexio *nexio = (struct nexio *) malloc(sizeof(struct nexio));
    memset(nexio, 0, sizeof(struct nexio));

    struct sockaddr_nl *snl_tx = (struct sockaddr_nl *) malloc(sizeof(struct sockaddr_nl));
    struct sockaddr_nl *snl_rx_ioctl = (struct sockaddr_nl *) malloc(sizeof(struct sockaddr_nl));

    memset(snl_tx, 0, sizeof(struct sockaddr_nl));
    memset(snl_rx_ioctl, 0, sizeof(struct sockaddr_nl));

    snl_tx->nl_family = AF_NETLINK;
    snl_tx->nl_pid = 0; /* For Linux Kernel */
    snl_tx->nl_groups = 0; /* unicast */

    snl_rx_ioctl->nl_family = AF_NETLINK;
    snl_rx_ioctl->nl_pid = getpid();

    nexio->sock_tx = socket(PF_NETLINK, SOCK_RAW, NETLINK_USER);
    if (nexio->sock_tx < 0) fprintf(stderr, "%s: socket error (%d: %s)\n", __FUNCTION__, errno, strerror(errno));
    nexio->sock_rx_ioctl = socket(PF_NETLINK, SOCK_RAW, NETLINK_USER);
    if (nexio->sock_rx_ioctl < 0) fprintf(stderr, "%s: socket error (%d: %s)\n", __FUNCTION__, errno, strerror(errno));

    // Set 1 second timeout on ioctl receive socket
    struct timeval tv = {
        .tv_sec = 1,
        .tv_usec = 0
    };
    setsockopt(nexio->sock_rx_ioctl, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));

//    printf("%s: Before connect\n", __FUNCTION__);
    err = bind(nexio->sock_rx_ioctl, (struct sockaddr *) snl_rx_ioctl, sizeof(struct sockaddr));
    if (err) fprintf(stderr, "%s: bind error (%d: %s)\n", __FUNCTION__, errno, strerror(errno));

    err = connect(nexio->sock_tx, (struct sockaddr *) snl_tx, sizeof(struct sockaddr));
    if (err) fprintf(stderr, "%s: connect error (%d: %s)\n", __FUNCTION__, errno, strerror(errno));

//    printf("%s: Exit\n", __FUNCTION__);

    nexio->type = NEXIO_TYPE_NETLINK;

    return nexio;
}