qcacmn: Add ie cap set and get APIs

Add ie capability set and get APIs

Change-Id: I25e9f8a5ee0cd049f1fe7abddc151217ff7668d3
CRs-Fixed: 3347566

umac/cmn_services/utils/inc/wlan_utility.h

@@ -154,6 +154,38 @@ const uint8_t *wlan_get_ext_ie_ptr_from_ext_id(const uint8_t *oui,
 					       const uint8_t *ie,
 					       uint16_t ie_len);
 
+/**
+ * wlan_iecap_set() - Set the capability in the IE
+ * @iecap: pointer to capability IE
+ * @bit_pos: bit position of capability from start of capability field
+ * @tot_bits: total bits of capability
+ * @value: value to be set
+ *
+ * This function sets the value in capability IE at the bit position
+ * specified for specified number of bits in byte order.
+ *
+ * Return: void
+ */
+void wlan_iecap_set(uint8_t *iecap,
+		    uint8_t bit_pos,
+		    uint8_t tot_bits,
+		    uint32_t value);
+
+/**
+ * wlan_iecap_get() - Get the capability in the IE
+ * @iecap: pointer to capability IE
+ * @bit_pos: bit position of capability from start of capability field
+ * @tot_bits: total bits of capability
+ *
+ * This function gets the value at bit position for specified bits
+ * from start of capability field.
+ *
+ * Return: capability value
+ */
+uint32_t wlan_iecap_get(uint8_t *iecap,
+			uint8_t bit_pos,
+			uint32_t tot_bits);
+
 /**
  * wlan_get_elem_fragseq_requirements() - Get requirements related to generation
  * of element fragment sequence.

umac/cmn_services/utils/src/wlan_utility.c

@@ -227,6 +227,83 @@ static const uint8_t *wlan_get_ie_ptr_from_eid_n_oui(uint8_t eid,
 	return NULL;
 }
 
+void wlan_iecap_set(uint8_t *iecap,
+		    uint8_t bit_pos,
+		    uint8_t tot_bits,
+		    uint32_t value)
+{
+	uint8_t fit_bits;
+	uint8_t byte_cnt;
+	uint8_t prev_fit_bits = 0;
+	uint32_t shift_value;
+
+	/* calculate byte position of the field in IE capability */
+	byte_cnt = bit_pos / 8;
+	/* calculate the bit position in the start byte that needs to be set */
+	bit_pos = bit_pos % 8;
+	fit_bits = 8 - bit_pos;
+	fit_bits = (tot_bits > fit_bits) ? 8 - bit_pos : tot_bits;
+
+	while ((bit_pos + tot_bits) > 8) {
+		/* clear the target bit */
+		QDF_SET_BITS(iecap[byte_cnt], bit_pos, fit_bits, value);
+		tot_bits = tot_bits - fit_bits;
+		bit_pos = bit_pos + fit_bits;
+		if (bit_pos == 8) {
+			bit_pos = 0;
+			byte_cnt++;
+		}
+		prev_fit_bits = prev_fit_bits + fit_bits;
+		fit_bits = 8 - bit_pos;
+		fit_bits = (tot_bits > fit_bits) ? 8 - bit_pos : tot_bits;
+	}
+
+	if ((bit_pos + tot_bits) <= 8) {
+		/* clear the target bit */
+		shift_value = value >> prev_fit_bits;
+		QDF_SET_BITS(iecap[byte_cnt], bit_pos, fit_bits, shift_value);
+	}
+}
+
+uint32_t wlan_iecap_get(uint8_t *iecap,
+			uint8_t bit_pos,
+			uint32_t tot_bits)
+{
+	uint8_t fit_bits;
+	uint8_t byte_cnt;
+	uint8_t temp_val;
+	uint8_t cur_bit_pos = 0;
+	uint32_t val = 0;
+
+	/* calculate byte position of the field in IE capability */
+	byte_cnt = bit_pos / 8;
+	temp_val = *(iecap + byte_cnt);
+	/* calculate the bit position in the start byte */
+	bit_pos = bit_pos % 8;
+	fit_bits = 8 - bit_pos;
+	fit_bits = (tot_bits > fit_bits) ? 8 - bit_pos : tot_bits;
+
+	while ((tot_bits + bit_pos) > 8) {
+		val |= QDF_GET_BITS(temp_val, bit_pos, fit_bits) << cur_bit_pos;
+		tot_bits = tot_bits - fit_bits;
+		bit_pos = bit_pos + fit_bits;
+		if (bit_pos == 8) {
+			bit_pos = 0;
+			byte_cnt++;
+			temp_val = *(iecap + byte_cnt);
+		}
+		cur_bit_pos = cur_bit_pos + fit_bits;
+
+		fit_bits = 8 - bit_pos;
+		fit_bits = (tot_bits > fit_bits) ? 8 - bit_pos : tot_bits;
+	}
+
+	if ((bit_pos + tot_bits) <= 8)
+		val |= QDF_GET_BITS(temp_val, bit_pos, fit_bits) << cur_bit_pos;
+
+	return val;
+}
+
 const uint8_t *wlan_get_ie_ptr_from_eid(uint8_t eid,
 					const uint8_t *ie,
 					int ie_len)