static int wpa_driver_get_power_save(void *priv, int *state)
{
struct i802_bss *bss = priv;
struct wpa_driver_nl80211_data *drv = bss->drv;
struct nl_msg *msg;
int ret = -1;
enum nl80211_ps_state ps_state;
msg = nlmsg_alloc();
if (!msg)
return -1;
genlmsg_put(msg, 0, 0, genl_family_get_id(drv->nl80211), 0, 0,
NL80211_CMD_GET_POWER_SAVE, 0);
NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, drv->ifindex);
ret = send_and_recv_msgs(drv, msg, get_power_mode_handler, state);
msg = NULL;
if (ret < 0)
wpa_printf(MSG_ERROR, "nl80211: Get power mode fail: %d", ret);
nla_put_failure:
nlmsg_free(msg);
return ret;
}
int reroute_path_selection_frames(char* ifname)
{
struct nl_msg *msg;
uint8_t cmd = NL80211_CMD_REGISTER_FRAME;
int ret;
char *pret;
char action_code[2] = { 0x20, 0x00 };
int ifindex = if_nametoindex(ifname);
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
pret = genlmsg_put(msg, 0, 0,
genl_family_get_id(nlcfg.nl80211), 0, 0, cmd, 0);
if (pret == NULL)
goto nla_put_failure;
NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ifindex);
NLA_PUT(msg, NL80211_ATTR_FRAME_MATCH, sizeof(action_code), action_code);
ret = send_and_recv_msgs(msg, receive_ps_frames, NULL);
if (ret)
printf("Registering for path selection frames failed: %d (%s)\n", ret,
strerror(-ret));
else
printf("Registering for path selection frames succeeded. Yay!\n");
return ret;
nla_put_failure:
return -ENOBUFS;
}
static int wpa_driver_set_power_save(void *priv, int state)
{
struct i802_bss *bss = priv;
struct wpa_driver_nl80211_data *drv = bss->drv;
struct nl_msg *msg;
int ret = -1;
enum nl80211_ps_state ps_state;
msg = nlmsg_alloc();
if (!msg)
return -1;
genlmsg_put(msg, 0, 0, drv->global->nl80211_id, 0, 0,
NL80211_CMD_SET_POWER_SAVE, 0);
if (state == WPA_PS_ENABLED)
ps_state = NL80211_PS_ENABLED;
else
ps_state = NL80211_PS_DISABLED;
NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, drv->ifindex);
NLA_PUT_U32(msg, NL80211_ATTR_PS_STATE, ps_state);
ret = send_and_recv_msgs(drv, msg, NULL, NULL);
msg = NULL;
if (ret < 0)
wpa_printf(MSG_ERROR, "nl80211: Set power mode fail: %d", ret);
nla_put_failure:
nlmsg_free(msg);
return ret;
}
int nl80211_get_wiphy_index(struct nl80211_data *ctx, int* phyidx)
{
struct nl_msg *msg;
struct wiphy_idx_data data = { 0 };
if (!(msg = nl80211_cmd_msg(ctx, 0, NL80211_CMD_GET_INTERFACE)))
return -1;
if (send_and_recv_msgs(ctx, msg, netdev_info_handler, &data))
return -1;
if (phyidx) *phyidx = data.wiphy_idx;
return 0;
}
static int nl80211_create_iface_once(struct nl80211_data* ctx, int(*handler)(struct nl_msg *, void *), void *arg)
{
int ret = -ENOBUFS;
struct nl_msg *msg;
struct nlattr *flags;
int ifidx;
const char *ifname = ctx->monitor_name;
fprintf(stderr, "nl80211: Create monitor interface %s\n", ifname);
msg = nl80211_cmd_msg(ctx, 0, NL80211_CMD_NEW_INTERFACE);
if (!msg)
goto fail;
if (nla_put_string(msg, NL80211_ATTR_IFNAME, ifname))
goto fail;
if (nla_put_u32(msg, NL80211_ATTR_IFTYPE, NL80211_IFTYPE_MONITOR))
goto fail;
flags = nla_nest_start(msg, NL80211_ATTR_MNTR_FLAGS);
if (!flags)
goto fail;
if (nla_put_flag(msg, NL80211_MNTR_FLAG_COOK_FRAMES))
goto fail;
nla_nest_end(msg, flags);
/*
* Tell cfg80211 that the interface belongs to the socket that created
* it, and the interface should be deleted when the socket is closed.
*/
if (nla_put_flag(msg, NL80211_ATTR_IFACE_SOCKET_OWNER))
goto fail;
ret = send_and_recv_msgs(ctx, msg, handler, arg);
msg = NULL;
if (ret) {
fail:
nlmsg_free(msg);
fprintf(stderr, "nl80211: Failed to create Monitor interface: %d (%s)\n", ret, strerror(-ret));
return -1;
}
ifidx = if_nametoindex(ifname);
fprintf(stderr, "nl80211: New interface %s created at ifindex=%d\n", ifname, ifidx);
if (ifidx <= 0)
return -1;
return ifidx;
}
int join_mesh(char* ifname, char *mesh_id, int mesh_id_len, char *vendor_ie, int vendor_ie_len)
{
struct nl_msg *msg;
uint8_t cmd = NL80211_CMD_JOIN_MESH;
int ret;
char *pret;
int ifindex = if_nametoindex(ifname);
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
if (!mesh_id || !mesh_id_len)
return -EINVAL;
printf("o11s-pathseld: Staring mesh with mesh id = %s\n", mesh_id);
pret = genlmsg_put(msg, 0, 0,
genl_family_get_id(nlcfg.nl80211), 0, 0, cmd, 0);
if (pret == NULL)
goto nla_put_failure;
if (vendor_ie) {
struct nlattr *container = nla_nest_start(msg,
NL80211_ATTR_MESH_PARAMS);
if (!container)
return -ENOBUFS;
NLA_PUT(msg, NL80211_MESHCONF_VENDOR_PATH_SEL_IE,
vendor_ie_len, vendor_ie);
NLA_PUT_U8(msg, NL80211_MESHCONF_ENABLE_VENDOR_PATH_SEL, 1);
NLA_PUT_U8(msg, NL80211_MESHCONF_ENABLE_VENDOR_METRIC, 1);
nla_nest_end(msg, container);
}
NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ifindex);
NLA_PUT(msg, NL80211_ATTR_MESH_ID, mesh_id_len, mesh_id);
ret = send_and_recv_msgs(msg, NULL, NULL);
if (ret)
printf("Mesh start failed: %d (%s)\n", ret,
strerror(-ret));
else
printf("Mesh start succeeded. Yay!\n");
return ret;
nla_put_failure:
return -ENOBUFS;
}
int nl80211_get_ifmode(struct nl80211_data *ctx, enum nl80211_iftype* nlmode)
{
struct nl_msg *msg;
struct wiphy_idx_data data = {
.nlmode = NL80211_IFTYPE_UNSPECIFIED,
.macaddr = NULL,
};
if (!(msg = nl80211_cmd_msg(ctx, 0, NL80211_CMD_GET_INTERFACE)))
return NL80211_IFTYPE_UNSPECIFIED;
if (send_and_recv_msgs(ctx, msg, netdev_info_handler, &data))
return -1;
if (nlmode) *nlmode = data.nlmode;
return 0;
}
static u32 get_nl80211_protocol_features(struct wpa_driver_nl80211_data *drv)
{
u32 feat = 0;
struct nl_msg *msg;
msg = nlmsg_alloc();
if (!msg)
return 0;
if (!nl80211_cmd(drv, msg, 0, NL80211_CMD_GET_PROTOCOL_FEATURES)) {
nlmsg_free(msg);
return 0;
}
if (send_and_recv_msgs(drv, msg, protocol_feature_handler, &feat) == 0)
return feat;
return 0;
}
int nl80211_get_macaddr(struct nl80211_data *ctx, u8* macaddr)
{
if (!macaddr) return -1;
struct nl_msg *msg;
struct wiphy_idx_data data = { 0 };
data.macaddr = macaddr;
if (!(msg = nl80211_cmd_msg(ctx, 0, NL80211_CMD_GET_INTERFACE))) {
free(data.macaddr);
return -1;
}
if (send_and_recv_msgs(ctx, msg, netdev_info_handler, &data)) {
free(data.macaddr);
return -1;
}
return 0;
}
int nl80211_set_ifmode(struct nl80211_data* ctx, enum nl80211_iftype mode)
{
struct nl_msg *msg;
msg = nl80211_cmd_msg(ctx, 0, NL80211_CMD_SET_INTERFACE);
if (!msg) {
nlmsg_free(msg);
return -1;
}
if (nla_put_u32(msg, NL80211_ATTR_IFTYPE, mode)) {
nlmsg_free(msg);
return -1;
}
if (send_and_recv_msgs(ctx, msg, NULL, NULL)) {
fprintf(stderr, "failed?\n");
return -1;
}
return 0;
}
int nl80211_unset_ap(struct nl80211_data *ctx)
{
struct nl_msg *msg = NULL;
int ret;
if (!ctx) return -1;
if (ctx->ifindex<0) return -1;
msg = nlmsg_alloc();
if (!msg) goto fail;
if (!nl80211_cmd(ctx, msg, 0, NL80211_CMD_DEL_BEACON)) goto fail;
if (nla_put_u32(msg, NL80211_ATTR_IFINDEX, ctx->ifindex)) goto fail;
ret = send_and_recv_msgs(ctx, msg, NULL, NULL);
if (ret) {
fprintf(stderr, "nl80211: advanced settings failed: %d (%s)\n", ret, strerror(-ret));
goto fail;
}
return 0;
fail:
if (msg) nlmsg_free(msg);
return -1;
}
void nl80211_remove_iface(struct nl80211_data *ctx, int ifidx)
{
struct nl_msg *msg = NULL;
fprintf(stderr, "nl80211: Remove interface ifindex=%d\n", ifidx);
msg = nlmsg_alloc();
if (!msg)
goto failed;
if (!nl80211_cmd(ctx, msg, 0, NL80211_CMD_DEL_INTERFACE))
goto failed;
if (nla_put_u32(msg, NL80211_ATTR_IFINDEX, ifidx))
goto failed;
if (send_and_recv_msgs(ctx, msg, NULL, NULL) == 0)
return;
failed:
if (msg) nlmsg_free(msg);
fprintf(stderr, "failed to remove interface (ifidx=%d)\n", ifidx);
}
int nl80211_set_ap(struct nl80211_data *ctx)
{
struct nl_msg *msg = NULL;
int ret;
u16 beacon_period = 100;
u8 beacon_DTIM = 1;
char* ssid_name = "TEST";
u8 ssid_sz = strlen(ssid_name);
int j;
u8 rates[NL80211_MAX_SUPP_RATES];
u8 nbBascis = (u8)(sizeof(BASIC_RATES)/sizeof(int));
// format basics rates
for (j = 0; j<nbBascis; j++) rates[j] = BASIC_RATE(BASIC_RATES[j]);
//portion of the beacon before the TIM IE
u8 * head = NULL;
int head_sz = 0;
//portion of the beacon after the TIM IE
u8 * tail = NULL;
int tail_sz = 0;
// BEACON HEAD
{
u8 * pos = NULL;
packet_element_t macheader = { 0 };
packet_element_t ssid = { 0 };
packet_element_t rates = { 0 };
packet_element_t ds = { 0 };
head_sz += hostapd_header_beacon(&macheader, ctx->macaddr, beacon_period);
head_sz += hostapd_eid_ssid(&ssid, ssid_name);
head_sz += hostapd_eid_supp_rates(&rates);
head_sz += hostapd_eid_ds_params(&ds);
head = (u8*)malloc(head_sz); pos = head;
pos = packet_element_concatnfree(pos, macheader);
pos = packet_element_concatnfree(pos, ssid);
pos = packet_element_concatnfree(pos, rates);
pos = packet_element_concatnfree(pos, ds);
fhexdump(stderr, "nl80211: Beacon head", head, head_sz);
}
// BEACON TAIL
{
u8 * pos = NULL;
packet_element_t rates = { 0 };
//packet_element_t emilie = { 0 };
tail_sz += hostapd_eid_ext_rates(&rates);
//tail_sz += hostapd_eid_emilie(&emilie);
tail = (u8*)malloc(tail_sz); pos = tail;
pos = packet_element_concatnfree(pos, rates);
//pos = packet_element_concatnfree(pos, emilie);
fhexdump(stderr, "nl80211: Beacon tail", tail, tail_sz);
}
// NL80211 BEACON SETTING
msg = nlmsg_alloc();
if (!msg) goto fail;
if (!nl80211_cmd(ctx, msg, 0, NL80211_CMD_NEW_BEACON)) goto fail;
if (nla_put_u32(msg, NL80211_ATTR_IFINDEX, ctx->ifindex)) goto fail;
if (nla_put(msg, NL80211_ATTR_BEACON_HEAD, head_sz, head)) goto fail;
if (nla_put(msg, NL80211_ATTR_BEACON_TAIL, tail_sz, tail)) goto fail;
if (nla_put_u32(msg, NL80211_ATTR_BEACON_INTERVAL, beacon_period)) goto fail;
if (nla_put_u32(msg, NL80211_ATTR_DTIM_PERIOD, beacon_DTIM)) goto fail;
if (nla_put(msg, NL80211_ATTR_SSID, ssid_sz, ssid_name)) goto fail;
if (nla_put_u32(msg, NL80211_ATTR_HIDDEN_SSID, NL80211_HIDDEN_SSID_NOT_IN_USE)) goto fail;
if (nla_put_flag(msg, NL80211_ATTR_PRIVACY)) goto fail;
if (nla_put_u32(msg, NL80211_ATTR_AUTH_TYPE, NL80211_AUTHTYPE_OPEN_SYSTEM)) goto fail;
if (nla_put_u32(msg, NL80211_ATTR_SMPS_MODE, NL80211_SMPS_OFF)) goto fail;
ret = send_and_recv_msgs(ctx, msg, NULL, NULL);
if (ret) {
fprintf(stderr, "nl80211: Beacon set failed: %d (%s)\n", ret, strerror(-ret));
goto fail;
}
// NL80211 BSS ADVDANCED SETTING
msg = nlmsg_alloc();
if (!msg)
goto fail;
if (!nl80211_cmd(ctx, msg, 0, NL80211_CMD_SET_BSS))
goto fail;
if (nla_put_u32(msg, NL80211_ATTR_IFINDEX, ctx->ifindex))
goto fail;
if (nla_put_u8(msg, NL80211_ATTR_BSS_CTS_PROT, 0))
goto fail;
if (nla_put_u8(msg, NL80211_ATTR_BSS_SHORT_PREAMBLE, 0))
goto fail;
if (nla_put_u8(msg, NL80211_ATTR_BSS_SHORT_SLOT_TIME, 1))
goto fail;
if (nla_put_u16(msg, NL80211_ATTR_BSS_HT_OPMODE, 0))
goto fail;
if (nla_put_u8(msg, NL80211_ATTR_AP_ISOLATE, 0))
goto fail;
if (nla_put(msg, NL80211_ATTR_BSS_BASIC_RATES, nbBascis, rates))
goto fail;;
ret = send_and_recv_msgs(ctx, msg, NULL, NULL);
if (ret) {
fprintf(stderr, "nl80211: advanced settings failed: %d (%s)\n", ret, strerror(-ret));
goto fail;
}
if (head) free(head);
if (tail) free(tail);
return 0;
fail:
if (msg) nlmsg_free(msg);
if (head) free(head);
if (tail) free(tail);
return -1;
}
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;
}
