static struct nl_msg *gen_msg(int iface, char* ssid, int chan) {
struct nl_msg *msg, *ssids, *freqs;
msg = nlmsg_alloc();
ssids = nlmsg_alloc();
freqs = nlmsg_alloc();
if (!msg || !ssids || !freqs) {
fprintf(stderr, "Failed to allocate netlink message");
if(msg)
nlmsg_free(msg);
if(ssids)
nlmsg_free(ssids);
if(freqs)
nlmsg_free(freqs);
return NULL;
}
genlmsg_put(msg, 0, 0, handle_id, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0);
NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, iface);
NLA_PUT(ssids, 1, strlen(ssid), ssid);
nla_put_nested(msg, NL80211_ATTR_SCAN_SSIDS, ssids);
NLA_PUT_U32(freqs, 1, chan*5 + 2407);
nla_put_nested(msg, NL80211_ATTR_SCAN_FREQUENCIES, freqs);
return msg;
nla_put_failure:
nlmsg_free(msg);
return NULL;
}
static int iw_cqm_rssi(struct nl80211_state *state, struct nl_cb *cb,
struct nl_msg *msg, int argc, char **argv)
{
struct nl_msg *cqm = NULL;
int thold = 0;
int hyst = 0;
int ret = -ENOSPC;
/* get the required args */
if (argc < 1 || argc > 2)
return 1;
if (strcmp(argv[0], "off")) {
thold = atoi(argv[0]);
if (thold == 0)
return -EINVAL;
if (argc == 2)
hyst = atoi(argv[1]);
}
/* connection quality monitor attributes */
cqm = nlmsg_alloc();
NLA_PUT_U32(cqm, NL80211_ATTR_CQM_RSSI_THOLD, thold);
NLA_PUT_U32(cqm, NL80211_ATTR_CQM_RSSI_HYST, hyst);
nla_put_nested(msg, NL80211_ATTR_CQM, cqm);
ret = 0;
nla_put_failure:
nlmsg_free(cqm);
return ret;
}
static int parse_mntr_flags(int *_argc, char ***_argv,
struct nl_msg *msg)
{
struct nl_msg *flags;
int err = -ENOBUFS;
enum nl80211_mntr_flags flag;
int argc = *_argc;
char **argv = *_argv;
flags = nlmsg_alloc();
if (!flags)
return -ENOMEM;
while (argc) {
int ok = 0;
for (flag = __NL80211_MNTR_FLAG_INVALID;
flag <= NL80211_MNTR_FLAG_MAX; flag++) {
if (strcmp(*argv, mntr_flags[flag]) == 0) {
ok = 1;
/*
* This shouldn't be adding "flag" if that is
* zero, but due to a problem in the kernel's
* nl80211 code (using NLA_NESTED policy) it
* will reject an empty nested attribute but
* not one that contains an invalid attribute
*/
NLA_PUT_FLAG(flags, flag);
break;
}
}
if (!ok) {
err = -EINVAL;
goto out;
}
argc--;
argv++;
}
nla_put_nested(msg, NL80211_ATTR_MNTR_FLAGS, flags);
err = 0;
nla_put_failure:
out:
nlmsg_free(flags);
*_argc = argc;
*_argv = argv;
return err;
}
// Has to be a separate function because of gotos, ew
void nl80211_parseflags(int nflags, int *in_flags, struct nl_msg *msg) {
#ifdef HAVE_LINUX_NETLINK
struct nl_msg *flags;
unsigned int x;
enum nl80211_mntr_flags flag = NL80211_MNTR_FLAG_MAX;
if ((flags = nlmsg_alloc()) == NULL) {
return;
}
for (x = 0; x < nflags; x++) {
switch (in_flags[x]) {
case nl80211_mntr_flag_none:
continue;
break;
case nl80211_mntr_flag_fcsfail:
flag = NL80211_MNTR_FLAG_FCSFAIL;
break;
case nl80211_mntr_flag_plcpfail:
flag = NL80211_MNTR_FLAG_PLCPFAIL;
break;
case nl80211_mntr_flag_control:
flag = NL80211_MNTR_FLAG_CONTROL;
break;
case nl80211_mntr_flag_otherbss:
flag = NL80211_MNTR_FLAG_OTHER_BSS;
break;
case nl80211_mntr_flag_cookframe:
flag = NL80211_MNTR_FLAG_COOK_FRAMES;
break;
}
NLA_PUT_FLAG(flags, flag);
}
nla_put_nested(msg, NL80211_ATTR_MNTR_FLAGS, flags);
nla_put_failure:
nlmsg_free(flags);
#endif
}
static int handle_wowlan_enable(struct nl80211_state *state, struct nl_cb *cb,
struct nl_msg *msg, int argc, char **argv,
enum id_input id)
{
struct nlattr *wowlan, *pattern;
struct nl_msg *patterns = NULL;
enum {
PS_REG,
PS_PAT,
} parse_state = PS_REG;
int err = -ENOBUFS;
unsigned char *pat, *mask;
size_t patlen;
int patnum = 0;
wowlan = nla_nest_start(msg, NL80211_ATTR_WOWLAN_TRIGGERS);
if (!wowlan)
return -ENOBUFS;
while (argc) {
switch (parse_state) {
case PS_REG:
if (strcmp(argv[0], "any") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_ANY);
else if (strcmp(argv[0], "disconnect") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_DISCONNECT);
else if (strcmp(argv[0], "magic-packet") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_MAGIC_PKT);
else if (strcmp(argv[0], "gtk-rekey-failure") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE);
else if (strcmp(argv[0], "eap-identity-request") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST);
else if (strcmp(argv[0], "4way-handshake") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE);
else if (strcmp(argv[0], "rfkill-release") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_RFKILL_RELEASE);
else if (strcmp(argv[0], "patterns") == 0) {
parse_state = PS_PAT;
patterns = nlmsg_alloc();
if (!patterns) {
err = -ENOMEM;
goto nla_put_failure;
}
} else {
err = 1;
goto nla_put_failure;
}
break;
case PS_PAT:
if (parse_hex_mask(argv[0], &pat, &patlen, &mask)) {
err = 1;
goto nla_put_failure;
}
pattern = nla_nest_start(patterns, ++patnum);
NLA_PUT(patterns, NL80211_WOWLAN_PKTPAT_MASK,
DIV_ROUND_UP(patlen, 8), mask);
NLA_PUT(patterns, NL80211_WOWLAN_PKTPAT_PATTERN,
patlen, pat);
nla_nest_end(patterns, pattern);
free(mask);
free(pat);
break;
}
argv++;
argc--;
}
if (patterns)
nla_put_nested(msg, NL80211_WOWLAN_TRIG_PKT_PATTERN,
patterns);
nla_nest_end(msg, wowlan);
err = 0;
nla_put_failure:
nlmsg_free(patterns);
return err;
}
static int cls_request_update(struct nl_cache *cache, struct nl_sock *sk)
{
struct tcmsg tchdr = {
.tcm_family = AF_UNSPEC,
.tcm_ifindex = cache->c_iarg1,
.tcm_parent = cache->c_iarg2,
};
return nl_send_simple(sk, RTM_GETTFILTER, NLM_F_DUMP, &tchdr,
sizeof(tchdr));
}
static int cls_build(struct rtnl_cls *cls, int type, int flags,
struct nl_msg **result)
{
struct rtnl_cls_ops *cops;
int err, prio, proto;
struct tcmsg *tchdr;
err = tca_build_msg((struct rtnl_tca *) cls, type, flags, result);
if (err < 0)
return err;
tchdr = nlmsg_data(nlmsg_hdr(*result));
prio = rtnl_cls_get_prio(cls);
proto = rtnl_cls_get_protocol(cls);
tchdr->tcm_info = TC_H_MAKE(prio << 16, htons(proto));
cops = rtnl_cls_lookup_ops(cls);
if (cops && cops->co_get_opts) {
struct nl_msg *opts;
if (!(opts = nlmsg_alloc())) {
err = -NLE_NOMEM;
goto errout;
}
if (!(err = cops->co_get_opts(cls, opts)))
err = nla_put_nested(*result, TCA_OPTIONS, opts);
nlmsg_free(opts);
if (err < 0)
goto errout;
}
return 0;
errout:
nlmsg_free(*result);
return err;
}
/**
* @name Classifier Addition/Modification/Deletion
* @{
*/
/**
* Build a netlink message to add a new classifier
* @arg cls classifier to add
* @arg flags additional netlink message flags
* @arg result Pointer to store resulting message.
*
* Builds a new netlink message requesting an addition of a classifier
* The netlink message header isn't fully equipped with all relevant
* fields and must be sent out via nl_send_auto_complete() or
* supplemented as needed. \a classifier must contain the attributes of
* the new classifier set via \c rtnl_cls_set_* functions. \a opts
* may point to the clsasifier specific options.
*
* @return 0 on success or a negative error code.
*/
int rtnl_cls_build_add_request(struct rtnl_cls *cls, int flags,
struct nl_msg **result)
{
return cls_build(cls, RTM_NEWTFILTER, NLM_F_CREATE | flags, result);
}
/**
* Add a new classifier
* @arg sk Netlink socket.
* @arg cls classifier to add
* @arg flags additional netlink message flags
*
* Builds a netlink message by calling rtnl_cls_build_add_request(),
* sends the request to the kernel and waits for the next ACK to be
* received and thus blocks until the request has been processed.
*
* @return 0 on sucess or a negative error if an error occured.
*/
int rtnl_cls_add(struct nl_sock *sk, struct rtnl_cls *cls, int flags)
{
struct nl_msg *msg;
int err;
if ((err = rtnl_cls_build_add_request(cls, flags, &msg)) < 0)
return err;
err = nl_send_auto_complete(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return nl_wait_for_ack(sk);
}
/**
* Build a netlink message to change classifier attributes
* @arg cls classifier to change
* @arg flags additional netlink message flags
* @arg result Pointer to store resulting message.
*
* Builds a new netlink message requesting a change of a neigh
* attributes. The netlink message header isn't fully equipped with
* all relevant fields and must thus be sent out via nl_send_auto_complete()
* or supplemented as needed.
*
* @return 0 on success or a negative error code.
*/
int rtnl_cls_build_change_request(struct rtnl_cls *cls, int flags,
struct nl_msg **result)
{
return cls_build(cls, RTM_NEWTFILTER, NLM_F_REPLACE | flags, result);
}
/**
* Change a classifier
* @arg sk Netlink socket.
* @arg cls classifier to change
* @arg flags additional netlink message flags
*
* Builds a netlink message by calling rtnl_cls_build_change_request(),
* sends the request to the kernel and waits for the next ACK to be
* received and thus blocks until the request has been processed.
*
* @return 0 on sucess or a negative error if an error occured.
*/
int rtnl_cls_change(struct nl_sock *sk, struct rtnl_cls *cls, int flags)
{
struct nl_msg *msg;
int err;
if ((err = rtnl_cls_build_change_request(cls, flags, &msg)) < 0)
return err;
err = nl_send_auto_complete(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return nl_wait_for_ack(sk);
}
/**
* Build a netlink request message to delete a classifier
* @arg cls classifier to delete
* @arg flags additional netlink message flags
* @arg result Pointer to store resulting message.
*
* Builds a new netlink message requesting a deletion of a classifier.
* The netlink message header isn't fully equipped with all relevant
* fields and must thus be sent out via nl_send_auto_complete()
* or supplemented as needed.
*
* @return 0 on success or a negative error code.
*/
int rtnl_cls_build_delete_request(struct rtnl_cls *cls, int flags,
struct nl_msg **result)
{
return cls_build(cls, RTM_DELTFILTER, flags, result);
}
/**
* Delete a classifier
* @arg sk Netlink socket.
* @arg cls classifier to delete
* @arg flags additional netlink message flags
*
* Builds a netlink message by calling rtnl_cls_build_delete_request(),
* sends the request to the kernel and waits for the next ACK to be
* received and thus blocks until the request has been processed.
*
* @return 0 on sucess or a negative error if an error occured.
*/
int rtnl_cls_delete(struct nl_sock *sk, struct rtnl_cls *cls, int flags)
{
struct nl_msg *msg;
int err;
if ((err = rtnl_cls_build_delete_request(cls, flags, &msg)) < 0)
return err;
err = nl_send_auto_complete(sk, msg);
nlmsg_free(msg);
if (err < 0)
return err;
return nl_wait_for_ack(sk);
}
/** @} */
/**
* @name Cache Management
* @{
*/
/**
* Build a classifier cache including all classifiers attached to the
* specified class/qdisc on eht specified interface.
* @arg sk Netlink socket.
* @arg ifindex interface index of the link the classes are
* attached to.
* @arg parent parent qdisc/class
* @arg result Pointer to store resulting cache.
*
* Allocates a new cache, initializes it properly and updates it to
* include all classes attached to the specified interface.
*
* @note The caller is responsible for destroying and freeing the
* cache after using it.
* @return 0 on success or a negative error code.
*/
int rtnl_cls_alloc_cache(struct nl_sock *sk, int ifindex, uint32_t parent, struct nl_cache **result)
{
struct nl_cache * cache;
int err;
if (!(cache = nl_cache_alloc(&rtnl_cls_ops)))
return -NLE_NOMEM;
cache->c_iarg1 = ifindex;
cache->c_iarg2 = parent;
if (sk && (err = nl_cache_refill(sk, cache)) < 0) {
nl_cache_free(cache);
return err;
}
*result = cache;
return 0;
}
/** @} */
static struct nl_cache_ops rtnl_cls_ops = {
.co_name = "route/cls",
.co_hdrsize = sizeof(struct tcmsg),
.co_msgtypes = {
{ RTM_NEWTFILTER, NL_ACT_NEW, "new" },
{ RTM_DELTFILTER, NL_ACT_DEL, "del" },
{ RTM_GETTFILTER, NL_ACT_GET, "get" },
END_OF_MSGTYPES_LIST,
},
.co_protocol = NETLINK_ROUTE,
.co_request_update = cls_request_update,
.co_msg_parser = cls_msg_parser,
.co_obj_ops = &cls_obj_ops,
};
static void __init cls_init(void)
{
nl_cache_mngt_register(&rtnl_cls_ops);
}
static void __exit cls_exit(void)
{
nl_cache_mngt_unregister(&rtnl_cls_ops);
}
static int handle_wowlan_enable(struct nl80211_state *state,
struct nl_msg *msg, int argc, char **argv,
enum id_input id)
{
struct nlattr *wowlan, *pattern;
struct nl_msg *patterns = NULL;
enum {
PS_REG,
PS_PAT,
} parse_state = PS_REG;
int err = -ENOBUFS;
unsigned char *pat, *mask;
size_t patlen;
int patnum = 0, pkt_offset;
char *eptr, *value1, *value2, *sptr = NULL;
wowlan = nla_nest_start(msg, NL80211_ATTR_WOWLAN_TRIGGERS);
if (!wowlan)
return -ENOBUFS;
while (argc) {
switch (parse_state) {
case PS_REG:
if (strcmp(argv[0], "any") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_ANY);
else if (strcmp(argv[0], "disconnect") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_DISCONNECT);
else if (strcmp(argv[0], "magic-packet") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_MAGIC_PKT);
else if (strcmp(argv[0], "gtk-rekey-failure") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_GTK_REKEY_FAILURE);
else if (strcmp(argv[0], "eap-identity-request") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_EAP_IDENT_REQUEST);
else if (strcmp(argv[0], "4way-handshake") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_4WAY_HANDSHAKE);
else if (strcmp(argv[0], "rfkill-release") == 0)
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_RFKILL_RELEASE);
else if (strcmp(argv[0], "tcp") == 0) {
argv++;
argc--;
if (!argc) {
err = 1;
goto nla_put_failure;
}
err = wowlan_parse_tcp_file(msg, argv[0]);
if (err)
goto nla_put_failure;
} else if (strcmp(argv[0], "patterns") == 0) {
parse_state = PS_PAT;
patterns = nlmsg_alloc();
if (!patterns) {
err = -ENOMEM;
goto nla_put_failure;
}
} else if (strcmp(argv[0], "net-detect") == 0) {
argv++;
argc--;
if (!argc) {
err = 1;
goto nla_put_failure;
}
err = wowlan_parse_net_detect(msg, &argc, &argv);
if (err)
goto nla_put_failure;
continue;
} else {
err = 1;
goto nla_put_failure;
}
break;
case PS_PAT:
value1 = strtok_r(argv[0], "+", &sptr);
value2 = strtok_r(NULL, "+", &sptr);
if (!value2) {
pkt_offset = 0;
value2 = value1;
} else {
pkt_offset = strtoul(value1, &eptr, 10);
if (eptr != value1 + strlen(value1)) {
err = 1;
goto nla_put_failure;
}
}
if (parse_hex_mask(value2, &pat, &patlen, &mask)) {
err = 1;
goto nla_put_failure;
}
pattern = nla_nest_start(patterns, ++patnum);
NLA_PUT(patterns, NL80211_PKTPAT_MASK,
DIV_ROUND_UP(patlen, 8), mask);
NLA_PUT(patterns, NL80211_PKTPAT_PATTERN, patlen, pat);
NLA_PUT_U32(patterns, NL80211_PKTPAT_OFFSET,
pkt_offset);
nla_nest_end(patterns, pattern);
free(mask);
free(pat);
break;
}
argv++;
argc--;
}
if (patterns)
nla_put_nested(msg, NL80211_WOWLAN_TRIG_PKT_PATTERN,
patterns);
nla_nest_end(msg, wowlan);
err = 0;
nla_put_failure:
nlmsg_free(patterns);
return err;
}
static int nl80211_set_wowlan_triggers(struct i802_bss *bss, int enable)
{
struct nl_msg *msg, *pats = NULL;
struct wpa_driver_nl80211_data *drv = bss->drv;
struct nlattr *wowtrig, *pat;
int i, ret = -1;
int filters;
bss->drv->wowlan_enabled = !!enable;
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
genlmsg_put(msg, 0, 0, drv->global->nl80211_id, 0,
0, NL80211_CMD_SET_WOWLAN, 0);
NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, bss->drv->first_bss.ifindex);
wowtrig = nla_nest_start(msg, NL80211_ATTR_WOWLAN_TRIGGERS);
if (!wowtrig) {
ret = -ENOBUFS;
goto nla_put_failure;
}
if (!enable) {
NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_ANY);
} else {
pats = nlmsg_alloc();
if (!pats) {
ret = -ENOMEM;
goto nla_put_failure;
}
/* In ginger filter 0 and 1 are always set but in ICS we
* only enable unicast. Make sure to always set it, otherwise
* unicast packets will be dropped.
* bcast packets are dropped and handled by the firmware */
filters = bss->drv->wowlan_triggers |= 1;
for (i = 0; i < NR_RX_FILTERS; i++) {
if (filters & (1 << i)) {
struct rx_filter *rx_filter = &rx_filters[i];
int patnr = 1;
u8 *pattern = nl80211_rx_filter_get_pattern(rx_filter,bss);
if (!pattern)
continue;
pat = nla_nest_start(pats, patnr++);
NLA_PUT(pats, NL80211_WOWLAN_PKTPAT_MASK,
rx_filter->mask_len,
rx_filter->mask);
NLA_PUT(pats, NL80211_WOWLAN_PKTPAT_PATTERN,
rx_filter->pattern_len,
pattern);
nla_nest_end(pats, pat);
}
}
}
if (pats)
nla_put_nested(msg, NL80211_WOWLAN_TRIG_PKT_PATTERN, pats);
nla_nest_end(msg, wowtrig);
ret = send_and_recv_msgs(bss->drv, msg, NULL, NULL);
if (ret < 0)
wpa_printf(MSG_ERROR, "Failed to set WoWLAN trigger:%d\n", ret);
if (pats)
nlmsg_free(pats);
return 0;
nla_put_failure:
nlmsg_free(msg);
return ret;
}
