diff --git a/src/cntlr_commands_impl.c b/src/cntlr_commands_impl.c index f3b08577b18e717cb6d00a35898838c688c7e078..2c113d7e9c6d0c584fa7b95fec2b8b88f14feaa3 100644 --- a/src/cntlr_commands_impl.c +++ b/src/cntlr_commands_impl.c @@ -855,10 +855,15 @@ int COMMAND(send_channel_sel)(void *priv, void *args, void *out) struct controller *c = (struct controller *)priv; struct blob_buf *bb = (struct blob_buf *)out; struct blob_attr *tb[NUM_ATTRS_CHANNEL_SEL]; + struct wifi_radio_opclass opclass = {}; uint8_t agent_mac[6] = { 0 }; char agent[18] = { 0 }; + uint8_t radio_mac[6] = { 0 }; + char radio[18] = { 0 }; struct cmdu_buff *cmdu; uint16_t mid = 0xffff; + uint32_t channel, bandwidth; + struct netif_radio *r; int ret; ret = controller_command_parse("send_channel_sel", args, tb); @@ -867,18 +872,43 @@ int COMMAND(send_channel_sel)(void *priv, void *args, void *out) return -EINVAL; } + /* Get agent */ strncpy(agent, blobmsg_data(tb[CHANNEL_SEL_ATTR_AGENT]), sizeof(agent) - 1); if (!hwaddr_aton(agent, agent_mac)) return -EINVAL; -#if 0 //TODO - cmdu = cntlr_gen_channel_sel_request(c, agent_mac, ...); + /* Get radio */ + strncpy(radio, blobmsg_data(tb[CHANNEL_SEL_ATTR_RADIO]), sizeof(radio) - 1); + if (!hwaddr_aton(radio, radio_mac)) + return -EINVAL; + + r = find_radio_by_mac(c, radio_mac); + if (!r) + return -EINVAL; + + /* Get channel/bandwidth */ + channel = blobmsg_get_u32(tb[CHANNEL_SEL_ATTR_CHANNEL]); + + if (tb[CHANNEL_SEL_ATTR_BANDWIDTH]) + bandwidth = blobmsg_get_u32(tb[CHANNEL_SEL_ATTR_BANDWIDTH]); + else + bandwidth = 0; + + /* Build opclass we would like to send */ + memcpy(&opclass, &r->radio_el->supp_opclass, sizeof(opclass)); + wifi_opclass_set_preferences(&opclass, 0x0); + + if (wifi_radio_opclass_update_channel(&opclass, r->radio_el->band, channel, bandwidth, 15 << 4)) + return -EINVAL; + + wifi_opclass_dump(&opclass); + + cmdu = cntrl_gen_channel_sel_request(c, agent_mac, radio_mac, &opclass); if (!cmdu) return -1; mid = send_cmdu(c, cmdu); cmdu_free(cmdu); -#endif /* reply with mid */ blobmsg_add_string(bb, "status", mid != 0xffff ? "ok" : "fail"); diff --git a/src/cntlr_tlv.c b/src/cntlr_tlv.c index 8e6f93ebe21b95e462563b3622b9f4aa1ec50695..b2b195c14f4ad5fb0d77f19e39b710409bfb4972 100644 --- a/src/cntlr_tlv.c +++ b/src/cntlr_tlv.c @@ -1477,6 +1477,99 @@ int cntlr_gen_channel_scan_req(struct controller *c, struct cmdu_buff *frm, return 0; } +struct cmdu_buff * cntrl_gen_channel_sel_request(struct controller *c, + uint8_t *agent, + uint8_t *radio_id, + struct wifi_radio_opclass *opclass) +{ + struct wifi_radio_opclass_entry *entry; + struct wifi_radio_opclass_channel *channel; + int ret, offset = 0; + struct cmdu_buff *cmdu; + struct netif_radio *radio; + int opclass_num_offset; + int opclass_num; + uint16_t mid = 0; + struct tlv *t; + int i, j; + + /* Find radio and supported opclasses */ + radio = find_radio_by_mac(c, radio_id); + if (!radio) + return NULL; + + if (!radio->radio_el) + return NULL; + + cmdu = cmdu_alloc_simple(CMDU_CHANNEL_SELECTION_REQ, &mid); + if (!cmdu) + return NULL; + + memcpy(cmdu->origin, agent, 6); + + t = cmdu_reserve_tlv(cmdu, 1024); + if (!t) { + cmdu_free(cmdu); + return NULL; + } + + t->type = MAP_TLV_CHANNEL_PREFERENCE; + + memcpy(&t->data[offset], radio_id, 6); /* radio id */ + offset += 6; + + /* Now update prefered */ + opclass_num_offset = offset; + t->data[offset++] = 0; /* m */ + + opclass_num = 0; + for (i = 0; i < opclass->num_opclass; i++) { + entry = &opclass->opclass[i]; + uint8_t preference; + int channel_offset; + int channel_num = 0; + + if (wifi_opclass_id_same_preference(opclass, entry->id, &preference)) { + t->data[offset++] = entry->id; + t->data[offset++] = 0; /* k */ + t->data[offset++] = preference; + opclass_num++; + continue; + } + + /* Group disabled channels - assume other with pref=15 */ + t->data[offset++] = entry->id; + channel_offset = offset; + t->data[offset++] = 0; /* k */ + + for (j = 0; j < entry->num_channel; j++) { + channel = &entry->channel[j]; + + if (((channel->preference & CHANNEL_PREF_MASK) >> 4) != 0) + continue; + + t->data[offset++] = channel->channel; + channel_num++; + } + + t->data[offset++] = 0; + t->data[channel_offset] = channel_num; + opclass_num++; + } + + t->data[opclass_num_offset] = opclass_num; /* m */ + t->len = offset; + ret = cmdu_put_tlv(cmdu, t); + if (ret) { + dbg("%s: error: cmdu_put_tlv()\n", __func__); + cmdu_free(cmdu); + return NULL; + } + + cmdu_put_eom(cmdu); + return cmdu; +} + int cntlr_gen_channel_pref(struct controller *c, struct cmdu_buff *frm, uint8_t *radio_id, uint8_t class_id, uint8_t channel_nr, const uint8_t *chanlist, uint8_t pref) diff --git a/src/cntlr_tlv.h b/src/cntlr_tlv.h index 3fdd6e41d5fdcb83a2c9f9b821dcecaa7ff186b5..f0d42edcd6b1b512de2a4f757ecb9635fc394de6 100644 --- a/src/cntlr_tlv.h +++ b/src/cntlr_tlv.h @@ -181,4 +181,8 @@ uint8_t *cntlr_gen_qos_mgmt_elem_desc(struct controller *c, uint32_t *out_len); #endif +struct cmdu_buff * cntrl_gen_channel_sel_request(struct controller *c, + uint8_t *agent, + uint8_t *radio_id, + struct wifi_radio_opclass *opclass); #endif /* CNTLR_TLV_GEN_H */