Fix channel selection request

Signed-off-by: Janusz Dziedzic <janusz.dziedzic@iopsys.eu>

src/cntlr_acs.c

@@ -235,7 +235,7 @@ void cntlr_acs_node_channel_recalc(struct node *node, bool skip_dfs)
 						   r->radio_el->macaddr,
 						   acs_params.best_channel,
 						   acs_params.best_opclass,
-						   15);
+						   14);
 
 		if (ret)
 			warn("acs switch failed\n");

src/cntlr_tlv.c

@@ -37,6 +37,7 @@
 
 #include <wifidefs.h>
 #include "wifi_dataelements.h"
+#include "wifi_opclass.h"
 
 #include <uci.h>
 
@@ -1483,12 +1484,37 @@ int cntlr_gen_channel_pref(struct controller *c, struct cmdu_buff *frm,
 		const uint8_t *chanlist, uint8_t pref)
 {
 	int ret, offset = 0;
-	int i, j, num_opclass = 1;
+	struct wifi_radio_opclass opclass = {};
+	struct wifi_radio_opclass_entry *entry;
+	struct wifi_radio_opclass_channel *channel;
+	struct netif_radio *radio;
 	struct tlv *t;
-	uint8_t preference = 0x00;
-	uint8_t reason_code = 0x00;
+	int opclass_num_offset;
+	int opclass_num;
+	int i, j;
 
-	t = cmdu_reserve_tlv(frm, 256);
+	/* Find radio and supported opclasses */
+	radio = find_radio_by_mac(c, radio_id);
+	if (!radio)
+		return -1;
+
+	if (!radio->radio_el)
+		return -1;
+
+	if (!radio->radio_el->supp_opclass.num_opclass)
+		return -1;
+
+	/* Build opclass we would like to send */
+	memcpy(&opclass, &radio->radio_el->supp_opclass, sizeof(opclass));
+	wifi_opclass_set_preferences(&opclass, 0x0);
+
+	/* Update requested preference */
+	for (i = 0; i < channel_nr; i++)
+		wifi_opclass_id_set_channel_preferences(&opclass, class_id, chanlist[i], pref << 4);
+
+	wifi_opclass_dump(&opclass);
+
+	t = cmdu_reserve_tlv(frm, 1024);
 	if (!t)
 		return -1;
 
@@ -1496,19 +1522,37 @@ int cntlr_gen_channel_pref(struct controller *c, struct cmdu_buff *frm,
 
 	memcpy(&t->data[offset], radio_id, 6);	/* radio id */
 	offset += 6;
-	t->data[offset++] = num_opclass;	/* num opclass */
 
-	for (i = 0; i < num_opclass; i++) {
-		t->data[offset++] = class_id;	/* class id */
-		t->data[offset++] = channel_nr;	/* num channel */
-		for (j = 0; j < channel_nr; j++)
-			t->data[offset++] = chanlist[j];
+	/* 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;
 
-		preference |= ((pref << 4) & CHANNEL_PREF_MASK);	/* preference */
-		preference |= (reason_code & CHANNEL_PREF_REASON);	/* reason code */
+                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;
+                }
+
+                for (j = 0; j < entry->num_channel; j++) {
+                        channel = &entry->channel[j];
+
+                        t->data[offset++] = entry->id;
+                        t->data[offset++] = 1;                                  /* k */
+                        t->data[offset++] = channel->channel;
+                        t->data[offset++] = channel->preference;
+
+                        opclass_num++;
+                }
         }
 
+        t->data[opclass_num_offset] = opclass_num;                              /* m */
 	t->len = offset;
 	ret = cmdu_put_tlv(frm, t);
 	if (ret) {

src/wifi_opclass.c

@@ -379,7 +379,7 @@ static const struct wifi_radio_opclass e4 = {
 				{ .channel = 167, .ctrl_channels = {161, 165, 169, 173}},
 				{ .channel = 183, .ctrl_channels = {177, 181, 185, 189}},
 				{ .channel = 199, .ctrl_channels = {193, 197, 201, 205}},
-				{ .channel = 209, .ctrl_channels = {209, 213, 217, 221}},
+				{ .channel = 215, .ctrl_channels = {209, 213, 217, 221}},
 			}
 		},
 		{
@@ -414,7 +414,7 @@ static const struct wifi_radio_opclass e4 = {
 				{ .channel = 167, .ctrl_channels = {161, 165, 169, 173}},
 				{ .channel = 183, .ctrl_channels = {177, 181, 185, 189}},
 				{ .channel = 199, .ctrl_channels = {193, 197, 201, 205}},
-				{ .channel = 209, .ctrl_channels = {209, 213, 217, 221}},
+				{ .channel = 215, .ctrl_channels = {209, 213, 217, 221}},
 			}
 		},
 	},
@@ -984,6 +984,23 @@ void wifi_opclass_id_set_preferences(struct wifi_radio_opclass *opclass, uint8_t
 		entry->channel[i].preference = preference;
 }
 
+void wifi_opclass_id_set_channel_preferences(struct wifi_radio_opclass *opclass, uint8_t id, uint8_t channel, uint8_t preference)
+{
+	struct wifi_radio_opclass_entry *entry;
+	int i;
+
+	entry = wifi_opclass_find_entry(opclass, id);
+	if (WARN_ON(!entry))
+		return;
+
+	for (i = 0; i < entry->num_channel; i++) {
+		if (entry->channel[i].channel != channel)
+			continue;
+		entry->channel[i].preference = preference;
+		break;
+	}
+}
+
 bool wifi_opclass_is_channel_supported(struct wifi_radio_opclass_channel *chan)
 {
 	uint8_t pref;

src/wifi_opclass.h

@@ -32,6 +32,7 @@ int wifi_opclass_id_num_channels_supported(struct wifi_radio_opclass *opclass, u
 int wifi_opclass_id_num_channels_unsupported(struct wifi_radio_opclass *opclass, uint8_t id);
 bool wifi_opclass_id_channel_supported(struct wifi_radio_opclass *opclass, uint8_t id, uint8_t channel);
 void wifi_opclass_id_set_preferences(struct wifi_radio_opclass *opclass, uint8_t id, uint8_t preference);
+void wifi_opclass_id_set_channel_preferences(struct wifi_radio_opclass *opclass, uint8_t id, uint8_t channel, uint8_t preference);
 bool wifi_opclass_is_channel_supported(struct wifi_radio_opclass_channel *chan);
 int wifi_opclass_get_supported_ctrl_channels(struct wifi_radio_opclass *opclass,
 			       uint8_t id,