bcmdhd_hdf/bcmdhd/wl_ext_genl.c

#ifdef WL_EXT_GENL
#include <bcmendian.h>
#include <wl_android.h>
#include <dhd_config.h>
#include <net/genetlink.h>

#define AGENL_ERROR(name, arg1, args...)                                       \
    do {                                                                       \
        if (android_msg_level & ANDROID_ERROR_LEVEL) {                         \
            printk(KERN_ERR DHD_LOG_PREFIX "[%s] AGENL-ERROR) %s : " arg1,     \
                   name, __func__, ##args);                                    \
        }                                                                      \
    } while (0)
#define AGENL_TRACE(name, arg1, args...)                                       \
    do {                                                                       \
        if (android_msg_level & ANDROID_TRACE_LEVEL) {                         \
            printk(KERN_INFO DHD_LOG_PREFIX "[%s] AGENL-TRACE) %s : " arg1,    \
                   name, __func__, ##args);                                    \
        }                                                                      \
    } while (0)
#define AGENL_INFO(name, arg1, args...)                                        \
    do {                                                                       \
        if (android_msg_level & ANDROID_INFO_LEVEL) {                          \
            printk(KERN_INFO DHD_LOG_PREFIX "[%s] AGENL-INFO) %s : " arg1,     \
                   name, __func__, ##args);                                    \
        }                                                                      \
    } while (0)

#define htod32(i) i
#define htod16(i) i
#define dtoh32(i) i
#define dtoh16(i) i

#ifdef SENDPROB
#define MGMT_PROBE_REQ 0x40
#define MGMT_PROBE_RES 0x50
#endif

enum {
    __GENL_CUSTOM_ATTR_INVALID,
    GENL_CUSTOM_ATTR_MSG, /* message */
    __GENL_CUSTOM_ATTR_MAX,
};

enum {
    __GENLL_CUSTOM_COMMAND_INVALID,
    GENL_CUSTOM_COMMAND_BIND, /* bind */
    GENL_CUSTOM_COMMAND_SEND, /* user -> kernel */
    GENL_CUSTOM_COMMAND_RECV, /* kernel -> user */
    __GENL_CUSTOM_COMMAND_MAX,
};

#if defined(ALIBABA_ZEROCONFIG)
#define GENL_FAMILY_NAME "WIFI_NL_CUSTOM"
#define PROBE_RSP_DST_MAC_OFFSET 4
#define PROBE_RSP_VNDR_ID_OFFSET 55
#else
#define GENL_FAMILY_NAME "WLAN_NL_CUSTOM"
#define PROBE_RSP_DST_MAC_OFFSET 4
#define PROBE_RSP_VNDR_ID_OFFSET DOT11_MGMT_HDR_LEN
#endif
#define PROBE_RSP_VNDR_LEN_OFFSET (PROBE_RSP_VNDR_ID_OFFSET + 1)
#define PROBE_RSP_VNDR_OUI_OFFSET (PROBE_RSP_VNDR_ID_OFFSET + 2)
#define MAX_CUSTOM_PKT_LENGTH 2048
#define GENL_CUSTOM_ATTR_MAX (__GENL_CUSTOM_ATTR_MAX - 1)
#define GENLMSG_UNICAST_RETRY_LIMIT 5

typedef struct genl_params {
    struct net_device *dev;
    bool bind;
    int pm;
    int bind_pid;
    int send_retry_cnt;
} genl_params_t;

struct genl_params *g_zconf = NULL;

static int wl_ext_genl_bind(struct sk_buff *skb, struct genl_info *info);
static int wl_ext_genl_recv(struct sk_buff *skb, struct genl_info *info);
static int wl_ext_genl_send(struct genl_params *zconf, struct net_device *dev,
                            char *buf, int buf_len);

static struct nla_policy wl_ext_genl_policy[GENL_CUSTOM_ATTR_MAX + 1] = {
    [GENL_CUSTOM_ATTR_MSG] = {.type = NLA_NUL_STRING},
};

static struct genl_ops wl_ext_genl_ops[] = {
    {
        .cmd = GENL_CUSTOM_COMMAND_BIND,
        .flags = 0,
        .policy = wl_ext_genl_policy,
        .doit = wl_ext_genl_bind,
        .dumpit = NULL,
    },
    {
        .cmd = GENL_CUSTOM_COMMAND_SEND,
        .flags = 0,
        .policy = wl_ext_genl_policy,
        .doit = wl_ext_genl_recv,
        .dumpit = NULL,
    },
};

static struct genl_family wl_ext_genl_family = {
    .id = GENL_ID_GENERATE,
    .hdrsize = 0,
    .name = GENL_FAMILY_NAME,
    .version = 1,
    .maxattr = GENL_CUSTOM_ATTR_MAX,
};

#ifdef SENDPROB
static int wl_ext_add_del_ie_hex(struct net_device *dev, uint pktflag,
                                 char *ie_data, int ie_len,
                                 const char *add_del_cmd)
{
    vndr_ie_setbuf_t *vndr_ie = NULL;
    char iovar_buf[WLC_IOCTL_SMLEN] = "\0";
    int tot_len = 0, iecount;
    int err = -1;

    if (!ie_len) {
        AGENL_ERROR(dev->name, "wrong ie_len %d\n", ie_len);
        goto exit;
    }

    tot_len = (int)(sizeof(vndr_ie_setbuf_t) + (ie_len));
    vndr_ie = (vndr_ie_setbuf_t *)kzalloc(tot_len, GFP_KERNEL);
    if (!vndr_ie) {
        AGENL_ERROR(dev->name, "IE memory alloc failed\n");
        err = -ENOMEM;
        goto exit;
    }

    /* Copy the vndr_ie SET command ("add"/"del") to the buffer */
    strncpy(vndr_ie->cmd, add_del_cmd, VNDR_IE_CMD_LEN - 1);
    vndr_ie->cmd[VNDR_IE_CMD_LEN - 1] = '\0';

    /* Set the IE count - the buffer contains only 1 IE */
    iecount = htod32(1);
    memcpy((void *)&vndr_ie->vndr_ie_buffer.iecount, &iecount, sizeof(s32));

    /* Set packet flag to indicate that BEACON's will contain this IE */
    pktflag = htod32(pktflag);
    memcpy((void *)&vndr_ie->vndr_ie_buffer.vndr_ie_list[0].pktflag, &pktflag,
           sizeof(u32));

    /* Set the IE ID */
    vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.id =
        (uchar)DOT11_MNG_VS_ID;

    /* Set the IE LEN */
    vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.len = ie_len;

    /* Set the IE OUI and DATA */
    memcpy((char *)vndr_ie->vndr_ie_buffer.vndr_ie_list[0].vndr_ie_data.oui,
           ie_data, ie_len);

    err = wldev_iovar_setbuf(dev, "vndr_ie", vndr_ie, tot_len, iovar_buf,
                             sizeof(iovar_buf), NULL);
    if (err != 0) {
        AGENL_ERROR(dev->name, "vndr_ie, ret=%d\n", err);
    }

exit:
    if (vndr_ie) {
        kfree(vndr_ie);
    }
    return err;
}

static int wl_ext_send_probersp(struct net_device *dev, char *buf, int buf_len)
{
    char addr[ETHER_ADDR_LEN], *pVndrOUI;
    char iovar_buf[WLC_IOCTL_SMLEN] = "\0";
    int err = -1, ie_len;

    if (buf == NULL || buf_len <= 0) {
        AGENL_ERROR(dev->name, "buf is NULL or buf_len <= 0\n");
        return -1;
    }

    AGENL_TRACE(dev->name, "Enter\n");

    memcpy(addr, (buf + PROBE_RSP_DST_MAC_OFFSET), ETHER_ADDR_LEN);
    pVndrOUI = (buf + PROBE_RSP_VNDR_OUI_OFFSET);
    ie_len = *(buf + PROBE_RSP_VNDR_LEN_OFFSET);

    if (ie_len > (buf_len - PROBE_RSP_VNDR_OUI_OFFSET)) {
        AGENL_ERROR(dev->name, "wrong vendor ie len %d\n", ie_len);
        return -1;
    }

    err = wl_ext_add_del_ie_hex(dev, VNDR_IE_PRBRSP_FLAG, pVndrOUI, ie_len,
                                "add");
    if (err) {
        goto exit;
    }

    err = wldev_iovar_setbuf(dev, "send_probresp", addr, ETHER_ADDR_LEN,
                             iovar_buf, sizeof(iovar_buf), NULL);
    if (err != 0) {
        AGENL_ERROR(dev->name, "vndr_ie, ret=%d\n", err);
    }

    OSL_SLEEP(0x64);
    wl_ext_add_del_ie_hex(dev, VNDR_IE_PRBRSP_FLAG, pVndrOUI, ie_len, "del");

exit:
    return err;
}

static int wl_ext_set_probreq(struct net_device *dev, bool set)
{
    int bytes_written = 0;
    char recv_probreq[32];

    AGENL_TRACE(dev->name, "Enter\n");

    if (set) {
        sprintf(recv_probreq, "wl recv_probreq 1");
        wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
    } else {
        sprintf(recv_probreq, "wl recv_probreq 0");
        wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
    }

    return 0;
}

void wl_ext_probreq_event(struct net_device *dev, void *argu,
                          const wl_event_msg_t *e, void *data)
{
    struct genl_params *zconf = (struct genl_params *)argu;
    int i, ret = 0, num_ie = 0, totlen;
    uint32 event_len = 0;
    char *buf, *pbuf;
    uint rem_len, buflen = MAX_CUSTOM_PKT_LENGTH;
    uint32 event_id[] = {DOT11_MNG_VS_ID};
    uint32 datalen = ntoh32(e->datalen);
    bcm_tlv_t *ie;

    AGENL_TRACE(dev->name, "Enter\n");

    rem_len = buflen;
    buf = kzalloc(MAX_CUSTOM_PKT_LENGTH, GFP_KERNEL);
    if (unlikely(!buf)) {
        AGENL_ERROR(dev->name, "Could not allocate buf\n");
        return;
    }

    // copy mgmt header
    pbuf = buf;
    memcpy(pbuf, data, DOT11_MGMT_HDR_LEN);
    rem_len -= (DOT11_MGMT_HDR_LEN + 1);
    datalen -= DOT11_MGMT_HDR_LEN;
    data += DOT11_MGMT_HDR_LEN;

    // copy IEs
    pbuf = buf + DOT11_MGMT_HDR_LEN;
#if 1 // non-sort by id
    ie = (bcm_tlv_t *)data;
    totlen = datalen;
    while (ie && totlen >= TLV_HDR_LEN) {
        int ie_id = -1;
        int ie_len = ie->len + TLV_HDR_LEN;
        for (i = 0; i < sizeof(event_id) / sizeof(event_id[0]); i++) {
            if (ie->id == event_id[i]) {
                ie_id = ie->id;
                break;
            }
        }
        if ((ie->id == ie_id) && (totlen >= ie_len) && (rem_len >= ie_len)) {
            memcpy(pbuf, ie, ie_len);
            pbuf += ie_len;
            rem_len -= ie_len;
            num_ie++;
        }
        ie = (bcm_tlv_t *)((uint8 *)ie + ie_len);
        totlen -= ie_len;
    }
#else // sort by id
    for (i = 0; i < sizeof(event_id) / sizeof(event_id[0]); i++) {
        void *pdata = data;
        int data_len = datalen;
        while (rem_len > 0) {
            ie = bcm_parse_tlvs(pdata, data_len, event_id[i]);
            if (!ie) {
                break;
            }
            if (rem_len < (ie->len + TLV_HDR_LEN)) {
                ANDROID_TRACE(("%s: buffer is not enough\n", __FUNCTION__));
                break;
            }
            memcpy(pbuf, ie, min(ie->len + TLV_HDR_LEN, rem_len));
            pbuf += (ie->len + TLV_HDR_LEN);
            rem_len -= (ie->len + TLV_HDR_LEN);
            data_len -= (((void *)ie - pdata) + (ie->len + TLV_HDR_LEN));
            pdata = (char *)ie + (ie->len + TLV_HDR_LEN);
            num_ie++;
        }
    }
#endif
    if (num_ie) {
        event_len = buflen - rem_len;
        AGENL_INFO(dev->name, "num_ie=%d\n", num_ie);
        if (android_msg_level & ANDROID_INFO_LEVEL) {
            prhex("buf", buf, event_len);
        }
        ret = wl_ext_genl_send(zconf, dev, buf, event_len);
    }

    if (buf) {
        kfree(buf);
    }
    return;
}
#endif

static int wl_ext_genl_recv(struct sk_buff *skb, struct genl_info *info)
{
    struct genl_params *zconf = g_zconf;
    struct net_device *dev;
    struct nlattr *na;
    char *pData = NULL;
    int DataLen = 0;

    if (info == NULL) {
        AGENL_ERROR(dev->name, "genl_info is NULL\n");
        return -1;
    }

    if (zconf == NULL) {
        AGENL_ERROR("wlan", "g_zconf is NULL\n");
        return -1;
    }
    dev = zconf->dev;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
    AGENL_TRACE(dev->name, "Enter snd_portid=%d\n", info->snd_portid);
#else
    AGENL_TRACE(dev->name, "Enter\n");
#endif
    na = info->attrs[GENL_CUSTOM_ATTR_MSG];

    if (na) {
        pData = (char *)nla_data(na);
        DataLen = nla_len(na);
        AGENL_INFO(dev->name, "nla_len(na) : %d\n", DataLen);
        if (android_msg_level & ANDROID_INFO_LEVEL) {
            prhex("nla_data(na)", pData, DataLen);
        }
    }

#ifdef SENDPROB
    if (*pData == MGMT_PROBE_RES) {
        wl_ext_send_probersp(dev, pData, DataLen);
    } else if (*pData == MGMT_PROBE_REQ) {
        AGENL_ERROR(dev->name, "probe req\n");
    } else {
        AGENL_ERROR(dev->name, "Unexpected pkt %d\n", *pData);
        if (android_msg_level & ANDROID_INFO_LEVEL) {
            prhex("nla_data(na)", pData, DataLen);
        }
    }
#endif

    return 0;
}

static int wl_ext_genl_send(struct genl_params *zconf, struct net_device *dev,
                            char *buf, int buf_len)
{
    struct sk_buff *skb = NULL;
    char *msg_head = NULL;
    int ret = -1;
    int bytes_written = 0;
    char recv_probreq[32];

    if (zconf->bind_pid == -1) {
        AGENL_ERROR(dev->name, "There is no binded process\n");
        return -1;
    }

    if (buf == NULL || buf_len <= 0) {
        AGENL_ERROR(dev->name, "buf is NULL or buf_len : %d\n", buf_len);
        return -1;
    }

    skb = genlmsg_new(MAX_CUSTOM_PKT_LENGTH, GFP_KERNEL);
    if (skb) {
        msg_head = genlmsg_put(skb, 0, 0, &wl_ext_genl_family, 0,
                               GENL_CUSTOM_COMMAND_RECV);
        if (msg_head == NULL) {
            nlmsg_free(skb);
            AGENL_ERROR(dev->name, "genlmsg_put fail\n");
            return -1;
        }

        ret = nla_put(skb, GENL_CUSTOM_ATTR_MSG, buf_len, buf);
        if (ret != 0) {
            nlmsg_free(skb);
            AGENL_ERROR(dev->name, "nla_put fail : %d\n", ret);
            return ret;
        }

        genlmsg_end(skb, msg_head);

        /* sending message */
        AGENL_TRACE(dev->name, "send to process %d\n", zconf->bind_pid);
        ret = genlmsg_unicast(&init_net, skb, zconf->bind_pid);
        if (ret != 0) {
            AGENL_ERROR(dev->name, "genlmsg_unicast fail : %d\n", ret);
            zconf->send_retry_cnt++;
            if (zconf->send_retry_cnt >= GENLMSG_UNICAST_RETRY_LIMIT) {
                AGENL_ERROR(dev->name,
                            "Exceeding retry cnt %d, Unbind pid : %d\n",
                            zconf->send_retry_cnt, zconf->bind_pid);
                zconf->bind_pid = -1;
                sprintf(recv_probreq, "wl recv_probreq 0");
                wl_android_ext_priv_cmd(dev, recv_probreq, 0, &bytes_written);
            }
            return ret;
        }
    } else {
        AGENL_ERROR(dev->name, "genlmsg_new fail\n");
        return -1;
    }

    zconf->send_retry_cnt = 0;

    return 0;
}

static int wl_ext_genl_bind(struct sk_buff *skb, struct genl_info *info)
{
    struct genl_params *zconf = g_zconf;
    struct net_device *dev;
    struct dhd_pub *dhd;
    struct nlattr *na;
    bool bind;
    char *pData = NULL;
    int DataLen = 0;

    if (info == NULL) {
        AGENL_ERROR("wlan", "genl_info is NULL\n");
        return -1;
    }

    if (zconf == NULL) {
        AGENL_ERROR("wlan", "zconf is NULL\n");
        return -1;
    }
    dev = zconf->dev;
    dhd = dhd_get_pub(dev);

    AGENL_TRACE(dev->name, "Enter\n");

    na = info->attrs[GENL_CUSTOM_ATTR_MSG];
    if (na) {
        pData = (char *)nla_data(na);
        DataLen = nla_len(na);
        AGENL_INFO(dev->name, "nla_len(na) : %d\n", DataLen);
        if (android_msg_level & ANDROID_INFO_LEVEL) {
            prhex("nla_data(na)", pData, DataLen);
        }
    }

    if (strcmp(pData, "BIND") == 0) {
        bind = TRUE;
    } else if (strcmp(pData, "UNBIND") == 0) {
        bind = FALSE;
    } else {
        AGENL_ERROR(dev->name, "Unknown cmd %s\n", pData);
        return -1;
    }

    if (bind == zconf->bind) {
        AGENL_TRACE(dev->name, "Already %s\n", bind ? "BIND" : "UNBIND");
        return 0;
    }

    if (bind) {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
        zconf->bind_pid = info->snd_portid;
#endif
        AGENL_TRACE(dev->name, "BIND pid = %d\n", zconf->bind_pid);
#ifdef SENDPROB
        wl_ext_set_probreq(dev, TRUE);
#endif
        zconf->bind = TRUE;
        zconf->pm = dhd->conf->pm;
        dhd->conf->pm = PM_OFF;
    } else {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 7, 0)
        AGENL_TRACE(dev->name, "UNBIND snd_portid = %d\n", info->snd_portid);
#else
        AGENL_TRACE(dev->name, "UNBIND pid = %d\n", zconf->bind_pid);
#endif
        zconf->bind_pid = -1;
#ifdef SENDPROB
        wl_ext_set_probreq(dev, FALSE);
#endif
        dhd->conf->pm = zconf->pm;
        zconf->bind = FALSE;
    }

    return 0;
}

int wl_ext_genl_init(struct net_device *net)
{
    struct dhd_pub *dhd = dhd_get_pub(net);
    struct genl_params *zconf = dhd->zconf;
    int ret = 0;

    AGENL_TRACE(net->name, "Enter falimy name: \"%s\"\n",
                wl_ext_genl_family.name);

    zconf = kzalloc(sizeof(struct genl_params), GFP_KERNEL);
    if (unlikely(!zconf)) {
        AGENL_ERROR(net->name, "Could not allocate zconf\n");
        return -ENOMEM;
    }
    dhd->zconf = (void *)zconf;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 10, 0)
    ret = genl_register_family(&wl_ext_genl_family);
    // fix me: how to attach wl_ext_genl_ops
    ret = -1;
#elif LINUX_VERSION_CODE >= KERNEL_VERSION(3, 13, 0)
    ret = genl_register_family_with_ops(&wl_ext_genl_family, wl_ext_genl_ops);
#else
    ret = genl_register_family_with_ops(&wl_ext_genl_family, wl_ext_genl_ops,
                                        ARRAY_SIZE(wl_ext_genl_ops));
#endif
    if (ret != 0) {
        AGENL_ERROR(net->name, "GE_NELINK family registration fail\n");
        goto err;
    }
    zconf->bind_pid = -1;
#ifdef SENDPROB
    ret = wl_ext_event_register(net, dhd, WLC_E_PROBREQ_MSG,
                                wl_ext_probreq_event, zconf, PRIO_EVENT_IAPSTA);
    if (ret) {
        goto err;
    }
#endif
    zconf->dev = net;
    g_zconf = zconf;

    return ret;
err:
    if (zconf) {
        kfree(zconf);
    }
    return ret;
}

void wl_ext_genl_deinit(struct net_device *net)
{
    struct dhd_pub *dhd = dhd_get_pub(net);
    struct genl_params *zconf = dhd->zconf;

    AGENL_TRACE(net->name, "Enter\n");

#ifdef SENDPROB
    wl_ext_event_deregister(net, dhd, WLC_E_PROBREQ_MSG, wl_ext_probreq_event);
#endif

    genl_unregister_family(&wl_ext_genl_family);
    if (zconf != NULL) {
        kfree(dhd->zconf);
        dhd->zconf = NULL;
    }
    g_zconf = NULL;
}
#endif