qcacmn: Correct DSCP to tid progamming for QCN9224

Fix DSCP to TID table programming issue for QCN9924

Change-Id: I698beb7bf475939b8477127b4950bc0d0cf9a791

dp/wifi3.0/dp_main.c

@@ -14279,6 +14279,7 @@ static void dp_soc_cfg_init(struct dp_soc *soc)
 		soc->rxdma2sw_rings_not_supported = 1;
 		soc->ast_offload_support = AST_OFFLOAD_ENABLE_STATUS;
 		soc->mec_fw_offload = FW_MEC_FW_OFFLOAD_ENABLED;
+		soc->num_hw_dscp_tid_map = HAL_MAX_HW_DSCP_TID_V2_MAPS;
 		break;
 	default:
 		qdf_print("%s: Unknown tgt type %d\n", __func__, target_type);

hal/wifi3.0/qcn9224/hal_9224_tx.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2021 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2021 Qualcomm Innovation Center, Inc. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for
  * any purpose with or without fee is hereby granted, provided that the
@@ -25,6 +26,7 @@
 
 #define DSCP_TID_TABLE_SIZE 24
 #define NUM_WORDS_PER_DSCP_TID_TABLE (DSCP_TID_TABLE_SIZE / 4)
+#define HAL_TX_NUM_DSCP_REGISTER_SIZE 32
 
 /**
  * hal_tx_set_dscp_tid_map_9224() - Configure default DSCP to TID map table
@@ -47,7 +49,7 @@ static void hal_tx_set_dscp_tid_map_9224(struct hal_soc *hal_soc, uint8_t *map,
 
 	struct hal_soc *soc = (struct hal_soc *)hal_soc;
 
-	if (id >= HAL_MAX_HW_DSCP_TID_MAPS_11AX)
+	if (id >= HAL_MAX_HW_DSCP_TID_V2_MAPS)
 		return;
 
 	cmn_reg_addr = HWIO_TCL_R0_CONS_RING_CMN_CTRL_REG_ADDR(
@@ -105,27 +107,74 @@ static void hal_tx_set_dscp_tid_map_9224(struct hal_soc *hal_soc, uint8_t *map,
  *
  * Return: void
  */
-static void hal_tx_update_dscp_tid_9224(struct hal_soc *hal_soc, uint8_t tid,
+static void hal_tx_update_dscp_tid_9224(struct hal_soc *soc, uint8_t tid,
 					uint8_t id, uint8_t dscp)
 {
-	int index;
-	uint32_t addr;
-	uint32_t value;
+	uint32_t addr, addr1, cmn_reg_addr;
+	uint32_t start_value = 0, end_value = 0;
 	uint32_t regval;
-	struct hal_soc *soc = (struct hal_soc *)hal_soc;
+	uint8_t end_bits = 0;
+	uint8_t start_bits = 0;
+	uint32_t start_index, end_index;
+
+	cmn_reg_addr = HWIO_TCL_R0_CONS_RING_CMN_CTRL_REG_ADDR(
+					MAC_TCL_REG_REG_BASE);
 
 	addr = HWIO_TCL_R0_DSCP_TID_MAP_n_ADDR(
-			MAC_TCL_REG_REG_BASE, id);
+				MAC_TCL_REG_REG_BASE,
+				id * NUM_WORDS_PER_DSCP_TID_TABLE);
+
+	start_index = dscp * HAL_TX_BITS_PER_TID;
+	end_index = (start_index + (HAL_TX_BITS_PER_TID - 1))
+		    % HAL_TX_NUM_DSCP_REGISTER_SIZE;
+	start_index = start_index % HAL_TX_NUM_DSCP_REGISTER_SIZE;
+	addr += (4 * ((dscp * HAL_TX_BITS_PER_TID) /
+			HAL_TX_NUM_DSCP_REGISTER_SIZE));
+
+	if (end_index < start_index) {
+		end_bits = end_index + 1;
+		start_bits = HAL_TX_BITS_PER_TID - end_bits;
+		start_value = tid << start_index;
+		end_value = tid >> start_bits;
+		addr1 = addr + 4;
+	} else {
+		start_bits = HAL_TX_BITS_PER_TID - end_bits;
+		start_value = tid << start_index;
+		addr1 = 0;
+	}
+
+	/* Enable read/write access */
+	regval = HAL_REG_READ(soc, cmn_reg_addr);
+	regval |=
+	(1 << HWIO_TCL_R0_CONS_RING_CMN_CTRL_REG_DSCP_TID_MAP_PROGRAM_EN_SHFT);
 
-	index = dscp % HAL_TX_NUM_DSCP_PER_REGISTER;
-	addr += 4 * (dscp / HAL_TX_NUM_DSCP_PER_REGISTER);
-	value = tid << (HAL_TX_BITS_PER_TID * index);
+	HAL_REG_WRITE(soc, cmn_reg_addr, regval);
 
 	regval = HAL_REG_READ(soc, addr);
-	regval &= ~(HAL_TX_TID_BITS_MASK << (HAL_TX_BITS_PER_TID * index));
-	regval |= value;
+
+	if (end_index < start_index)
+		regval &= (~0) >> start_bits;
+	else
+		regval &= ~(7 << start_index);
+
+	regval |= start_value;
 
 	HAL_REG_WRITE(soc, addr, (regval & HWIO_TCL_R0_DSCP_TID_MAP_n_RMSK));
+
+	if (addr1) {
+		regval = HAL_REG_READ(soc, addr1);
+		regval &= (~0) << end_bits;
+		regval |= end_value;
+
+		HAL_REG_WRITE(soc, addr1, (regval &
+			     HWIO_TCL_R0_DSCP_TID_MAP_n_RMSK));
+	}
+
+	/* Diasble read/write access */
+	regval = HAL_REG_READ(soc, cmn_reg_addr);
+	regval &=
+	~(HWIO_TCL_R0_CONS_RING_CMN_CTRL_REG_DSCP_TID_MAP_PROGRAM_EN_BMSK);
+	HAL_REG_WRITE(soc, cmn_reg_addr, regval);
 }
 
 /**