@@ -19568,6 +19568,265 @@ nl80211_epcs_cfg(struct sk_buff *skb, struct genl_info *info)
return rdev_set_epcs(rdev, dev, val);
}
+static size_t _nl80211_smd_ctx_nl_ba_param_size(void)
+{
+ size_t n = 0;
+
+ n += nla_total_size(0); /* ba_params nest header */
+ n += n_dl_tids * (nla_total_size(0) + /* tid nest header */
+ nla_total_size(sizeof(u16)) + /* buff_size */
+ nla_total_size(sizeof(u8)) + /* policy */
+ nla_total_size(0) + /* amsdu_support */
+ nla_total_size(sizeof(u16)) + /* timeout */
+ nla_total_size(0) + /* ext_no_frag */
+ nla_total_size(sizeof(u8)) + /* ext_frag_lvl */
+ nla_total_size(sizeof(u16))); /* ext_buff_size */
+
+ return n;
+}
+
+static size_t
+nl80211_smd_ctx_nl_size(struct cfg80211_smd_transition_info *st_info)
+{
+ const struct ieee80211_smd_ctx *ctx = st_info->ctx;
+ int n_dl_tids = 0, n_ul_tids = 0;
+ u8 pn_len = ctx->pn_len;
+ size_t n = 0;
+
+ /* count valid TIDs */
+ n_dl_tids = bitmap_weight(ctx->dl.valid_tid_bmap,
+ IEEE80211_SMD_CTX_NUM_TIDS);
+ n_ul_tids = bitmap_weight(ctx->ul.valid_tid_bmap,
+ IEEE80211_SMD_CTX_NUM_TIDS);
+
+ n += nla_total_size(sizeof(u8)); /* ST type */
+
+ /* outer NL80211_ATTR_SMD_CTX nest */
+ n += nla_total_size(0); /* nest header */
+
+ /* DL nest */
+ n += nla_total_size(0); /* DL nest header */
+
+ n += nla_total_size(sizeof(u8)); /* valid_tid_bitmap */
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_DL_SN, ctx->valid_ctx_bmap)) {
+ n += nla_total_size(0); /* DL SN nest header */
+ n += n_dl_tids * nla_total_size(sizeof(u16)); /* DL SN */
+ }
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_PN, ctx->valid_ctx_bmap))
+ n += nla_total_size(pn_len); /* DL PN */
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_BA_PARAMS, ctx->valid_ctx_bmap))
+ n += _nl80211_smd_ctx_nl_ba_param_size(); /* DL BA params */
+
+ /* UL nest */
+ n += nla_total_size(0); /* UL nest header */
+
+ n += nla_total_size(sizeof(u8)); /* valid_tid_bitmap */
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_UL_SN, ctx->valid_ctx_bmap)) {
+ n += nla_total_size(0); /* UL SN nest header */
+ n += n_ul_tids * nla_total_size(sizeof(u16)); /* UL SN */
+ }
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_PN, ctx->valid_ctx_bmap)) {
+ n += nla_total_size(0); /* UL PN nest header */
+ n += n_ul_tids * nla_total_size(pn_len); /* UL PN */
+ }
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_BA_PARAMS, ctx->valid_ctx_bmap))
+ n += _nl80211_smd_ctx_nl_ba_param_size(); /* UL BA params */
+
+ if (ctx->drv_ctx_size)
+ n += nla_total_size(ctx->drv_ctx_size); /* Driver data */
+
+ return n;
+}
+
+static int
+nl80211_put_smd_ctx_ba_params(struct sk_buff *msg,
+ const unsigned long *tid_bmap,
+ const struct ieee80211_smd_ctx_ba *ba_tids,
+ bool dl)
+{
+ u32 ba_params_attr = NL80211_SMD_CTX_DL_ATTR_BA_PARAMS;
+ struct nlattr *dl_ul_ba = NULL, *tid_ba = NULL;
+ u8 tid;
+
+ if (!dl)
+ ba_params_attr = NL80211_SMD_CTX_UL_ATTR_BA_PARAMS;
+
+ dl_ul_ba = nla_nest_start(msg, ba_params_attr);
+ if (!dl_ul_ba)
+ return -ENOBUFS;
+
+ for_each_set_bit(tid, tid_bmap, IEEE80211_SMD_CTX_NUM_TIDS) {
+ const struct ieee80211_smd_ctx_ba *ba = &ba_tids[tid];
+
+ tid_ba = nla_nest_start(msg, tid + 1);
+ if (!tid_ba)
+ goto nla_fail;
+
+ if (nla_put_u16(msg, NL80211_SMD_CTX_BA_ATTR_BUFF_SIZE,
+ ba->buffer_size) ||
+ nla_put_u8(msg, NL80211_SMD_CTX_BA_ATTR_POLICY,
+ ba->ba_policy) ||
+ (ba->amsdu_supported &&
+ nla_put_flag(msg,
+ NL80211_SMD_CTX_BA_ATTR_AMSDU_SUPPORT)) ||
+ nla_put_u16(msg, NL80211_SMD_CTX_BA_ATTR_TIMEOUT,
+ ba->timeout) ||
+ (ba->ext_no_frag &&
+ nla_put_flag(msg, NL80211_SMD_CTX_BA_ATTR_EXT_NO_FRAG)) ||
+ nla_put_u8(msg, NL80211_SMD_CTX_BA_ATTR_EXT_FRAG_LEVEL,
+ ba->extfrag_level) ||
+ nla_put_u16(msg, NL80211_SMD_CTX_BA_ATTR_EXT_BUFF_SIZE,
+ ba->ext_buffer_size)) {
+ nla_nest_cancel(msg, tid_ba);
+ goto nla_fail;
+ }
+
+ nla_nest_end(msg, tid_ba);
+ }
+
+ nla_nest_end(msg, dl_ul_ba);
+ return 0;
+
+nla_fail:
+ nla_nest_cancel(msg, dl_ul_ba);
+ return -ENOBUFS;
+}
+
+static int nl80211_put_smd_ctx(struct sk_buff *msg,
+ struct cfg80211_smd_transition_info *st_info)
+{
+ struct nlattr *dl_sn = NULL, *ul_sn = NULL, *ul_pn = NULL;
+ struct nlattr *smd_ctx = NULL, *dl = NULL, *ul = NULL;
+ const struct ieee80211_smd_ctx *ctx;
+ u8 dl_tid_bmap = 0, ul_tid_bmap = 0;
+ u8 tid;
+
+ /* ST Type */
+ if (nla_put_u8(msg, NL80211_ATTR_SMD_CTX_TYPE, st_info->type))
+ return -ENOBUFS;
+
+ smd_ctx = nla_nest_start(msg, NL80211_ATTR_SMD_CTX);
+ if (!smd_ctx)
+ return -ENOBUFS;
+
+ ctx = st_info->ctx;
+
+ /* Build DL context */
+ dl_tid_bmap = bitmap_read(ctx->dl.valid_tid_bmap, 0,
+ IEEE80211_SMD_CTX_NUM_TIDS);
+
+ dl = nla_nest_start(msg, NL80211_SMD_CTX_ATTR_DL);
+ if (!dl)
+ goto nla_fail;
+
+ if (nla_put_u8(msg, NL80211_SMD_CTX_DL_ATTR_VALID_TID_BITMAP,
+ dl_tid_bmap))
+ goto nla_fail_dl;
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_DL_SN, ctx->valid_ctx_bmap)) {
+ dl_sn = nla_nest_start(msg, NL80211_SMD_CTX_DL_ATTR_SN);
+ if (!dl_sn)
+ goto nla_fail_dl;
+
+ for_each_set_bit(tid, ctx->dl.valid_tid_bmap,
+ IEEE80211_SMD_CTX_NUM_TIDS) {
+ if (nla_put_u16(msg, tid + 1, ctx->dl.sn[tid])) {
+ nla_nest_cancel(msg, dl_sn);
+ goto nla_fail_dl;
+ }
+ }
+ nla_nest_end(msg, dl_sn);
+ }
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_PN, ctx->valid_ctx_bmap) &&
+ nla_put(msg, NL80211_SMD_CTX_DL_ATTR_PN, ctx->pn_len, ctx->dl.pn))
+ goto nla_fail_dl;
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_BA_PARAMS, ctx->valid_ctx_bmap) &&
+ nl80211_put_smd_ctx_ba_params(msg, ctx->dl.valid_tid_bmap,
+ ctx->dl.ba, true))
+ goto nla_fail_dl;
+
+ nla_nest_end(msg, dl);
+
+ /* Build UL context */
+ ul_tid_bmap = bitmap_read(ctx->ul.valid_tid_bmap, 0,
+ IEEE80211_SMD_CTX_NUM_TIDS);
+
+ ul = nla_nest_start(msg, NL80211_SMD_CTX_ATTR_UL);
+ if (!ul)
+ goto nla_fail;
+
+ if (nla_put_u8(msg, NL80211_SMD_CTX_UL_ATTR_VALID_TID_BITMAP,
+ ul_tid_bmap))
+ goto nla_fail_ul;
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_UL_SN, ctx->valid_ctx_bmap)) {
+ ul_sn = nla_nest_start(msg, NL80211_SMD_CTX_UL_ATTR_SN);
+ if (!ul_sn)
+ goto nla_fail_ul;
+
+ for_each_set_bit(tid, ctx->ul.valid_tid_bmap,
+ IEEE80211_SMD_CTX_NUM_TIDS) {
+ if (nla_put_u16(msg, tid + 1, ctx->ul.sn[tid])) {
+ nla_nest_cancel(msg, ul_sn);
+ goto nla_fail_ul;
+ }
+ }
+ nla_nest_end(msg, ul_sn);
+ }
+
+ if (ctx->pn_len &&
+ test_bit(IEEE80211_SMD_CTX_VALID_PN, ctx->valid_ctx_bmap)) {
+ ul_pn = nla_nest_start(msg, NL80211_SMD_CTX_UL_ATTR_PN);
+ if (!ul_pn)
+ goto nla_fail_ul;
+
+ for_each_set_bit(tid, ctx->ul.valid_tid_bmap,
+ IEEE80211_SMD_CTX_NUM_TIDS) {
+ if (nla_put(msg, tid + 1, ctx->pn_len,
+ ctx->ul.pn[tid])) {
+ nla_nest_cancel(msg, ul_pn);
+ goto nla_fail_ul;
+ }
+ }
+ nla_nest_end(msg, ul_pn);
+ }
+
+ if (test_bit(IEEE80211_SMD_CTX_VALID_BA_PARAMS, ctx->valid_ctx_bmap) &&
+ nl80211_put_smd_ctx_ba_params(msg, ctx->ul.valid_tid_bmap,
+ ctx->ul.ba, false))
+ goto nla_fail_ul;
+
+ nla_nest_end(msg, ul);
+
+ /* Driver data blob */
+ if (ctx->drv_ctx_size &&
+ nla_put(msg, NL80211_SMD_CTX_ATTR_DRV_DATA, ctx->drv_ctx_size,
+ ctx->drv_ctx))
+ goto nla_fail;
+
+ nla_nest_end(msg, smd_ctx);
+
+ return 0;
+
+nla_fail_ul:
+ nla_nest_cancel(msg, ul);
+
+nla_fail_dl:
+ nla_nest_cancel(msg, dl);
+
+nla_fail:
+ nla_nest_cancel(msg, smd_ctx);
+ return -EINVAL;
+}
+
#define NL80211_FLAG_NEED_WIPHY 0x01
#define NL80211_FLAG_NEED_NETDEV 0x02
#define NL80211_FLAG_NEED_RTNL 0x04