drm/amd/display: Fix VTEM InfoPacket programming

Refactor setting bit fields. Correcting the offset of MD0.
Initializing the InfoPacket header fields. Defining the field offsets
and masks.
Signed-off-by: Reza Amini <Reza.Amini@amd.com>
Reviewed-by: Anthony Koo <Anthony.Koo@amd.com>
Acked-by: Bhawanpreet Lakha <Bhawanpreet.Lakha@amd.com>
Signed-off-by: Alex Deucher <alexander.deucher@amd.com>

drivers/gpu/drm/amd/display/modules/freesync/freesync.c

@@ -50,6 +50,93 @@ struct core_freesync {
 	struct dc *dc;
 };
 
+void setFieldWithMask(unsigned char *dest, unsigned int mask, unsigned int value)
+{
+	unsigned int shift = 0;
+
+	if (!mask || !dest)
+		return;
+
+	while (!((mask >> shift) & 1))
+		shift++;
+
+	//reset
+	*dest = *dest & ~mask;
+	//set
+	//dont let value span past mask
+	value = value & (mask >> shift);
+	//insert value
+	*dest = *dest | (value << shift);
+}
+
+// VTEM Byte Offset
+#define VRR_VTEM_PB0		0
+#define VRR_VTEM_PB1		1
+#define VRR_VTEM_PB2		2
+#define VRR_VTEM_PB3		3
+#define VRR_VTEM_PB4		4
+#define VRR_VTEM_PB5		5
+#define VRR_VTEM_PB6		6
+
+#define VRR_VTEM_MD0		7
+#define VRR_VTEM_MD1		8
+#define VRR_VTEM_MD2		9
+#define VRR_VTEM_MD3		10
+
+
+// VTEM Byte Masks
+//PB0
+#define MASK__VRR_VTEM_PB0__RESERVED0  0x01
+#define MASK__VRR_VTEM_PB0__SYNC       0x02
+#define MASK__VRR_VTEM_PB0__VFR        0x04
+#define MASK__VRR_VTEM_PB0__AFR        0x08
+#define MASK__VRR_VTEM_PB0__DS_TYPE    0x30
+	//0: Periodic pseudo-static EM Data Set
+	//1: Periodic dynamic EM Data Set
+	//2: Unique EM Data Set
+	//3: Reserved
+#define MASK__VRR_VTEM_PB0__END        0x40
+#define MASK__VRR_VTEM_PB0__NEW        0x80
+
+//PB1
+#define MASK__VRR_VTEM_PB1__RESERVED1 0xFF
+
+//PB2
+#define MASK__VRR_VTEM_PB2__ORGANIZATION_ID 0xFF
+	//0: This is a Vendor Specific EM Data Set
+	//1: This EM Data Set is defined by This Specification (HDMI 2.1 r102.clean)
+	//2: This EM Data Set is defined by CTA-861-G
+	//3: This EM Data Set is defined by VESA
+//PB3
+#define MASK__VRR_VTEM_PB3__DATA_SET_TAG_MSB    0xFF
+//PB4
+#define MASK__VRR_VTEM_PB4__DATA_SET_TAG_LSB    0xFF
+//PB5
+#define MASK__VRR_VTEM_PB5__DATA_SET_LENGTH_MSB 0xFF
+//PB6
+#define MASK__VRR_VTEM_PB6__DATA_SET_LENGTH_LSB 0xFF
+
+
+
+//PB7-27 (20 bytes):
+//PB7 = MD0
+#define MASK__VRR_VTEM_MD0__VRR_EN         0x01
+#define MASK__VRR_VTEM_MD0__M_CONST        0x02
+#define MASK__VRR_VTEM_MD0__RESERVED2      0x0C
+#define MASK__VRR_VTEM_MD0__FVA_FACTOR_M1  0xF0
+
+//MD1
+#define MASK__VRR_VTEM_MD1__BASE_VFRONT    0xFF
+
+//MD2
+#define MASK__VRR_VTEM_MD2__BASE_REFRESH_RATE_98  0x03
+#define MASK__VRR_VTEM_MD2__RB                    0x04
+#define MASK__VRR_VTEM_MD2__RESERVED3             0xF8
+
+//MD3
+#define MASK__VRR_VTEM_MD3__BASE_REFRESH_RATE_07  0xFF
+
+
 #define MOD_FREESYNC_TO_CORE(mod_freesync)\
 		container_of(mod_freesync, struct core_freesync, public)
 
@@ -489,16 +576,14 @@ static void build_vrr_infopacket_header_vtem(enum signal_type signal,
 	// HB0, HB1, HB2 indicates PacketType VTEMPacket
 	infopacket->hb0 = 0x7F;
 	infopacket->hb1 = 0xC0;
-	infopacket->hb2 = 0x00;
-	/* HB3 Bit Fields
-	 * Reserved :1 = 0
-	 * Sync     :1 = 0
-	 * VFR      :1 = 1
-	 * Ds_Type  :2 = 0
-	 * End      :1 = 0
-	 * New      :1 = 0
-	 */
-	infopacket->hb3 = 0x20;
+	infopacket->hb2 = 0x00; //sequence_index
+
+	setFieldWithMask(&infopacket->sb[VRR_VTEM_PB0], MASK__VRR_VTEM_PB0__VFR, 1);
+	setFieldWithMask(&infopacket->sb[VRR_VTEM_PB2], MASK__VRR_VTEM_PB2__ORGANIZATION_ID, 1);
+	setFieldWithMask(&infopacket->sb[VRR_VTEM_PB3], MASK__VRR_VTEM_PB3__DATA_SET_TAG_MSB, 0);
+	setFieldWithMask(&infopacket->sb[VRR_VTEM_PB4], MASK__VRR_VTEM_PB4__DATA_SET_TAG_LSB, 1);
+	setFieldWithMask(&infopacket->sb[VRR_VTEM_PB5], MASK__VRR_VTEM_PB5__DATA_SET_LENGTH_MSB, 0);
+	setFieldWithMask(&infopacket->sb[VRR_VTEM_PB6], MASK__VRR_VTEM_PB6__DATA_SET_LENGTH_LSB, 4);
 }
 
 static void build_vrr_infopacket_header_v1(enum signal_type signal,
@@ -603,45 +688,36 @@ static void build_vrr_vtem_infopacket_data(const struct dc_stream_state *stream,
 		const struct mod_vrr_params *vrr,
 		struct dc_info_packet *infopacket)
 {
-	/* dc_info_packet to VtemPacket Translation of Bit-fields,
-	 * SB[6]
-	 * unsigned char VRR_EN        :1
-	 * unsigned char M_CONST       :1
-	 * unsigned char Reserved2     :2
-	 * unsigned char FVA_Factor_M1 :4
-	 * SB[7]
-	 * unsigned char Base_Vfront   :8
-	 * SB[8]
-	 * unsigned char Base_Refresh_Rate_98 :2
-	 * unsigned char RB                   :1
-	 * unsigned char Reserved3            :5
-	 * SB[9]
-	 * unsigned char Base_RefreshRate_07  :8
-	 */
 	unsigned int fieldRateInHz;
 
 	if (vrr->state == VRR_STATE_ACTIVE_VARIABLE ||
-				vrr->state == VRR_STATE_ACTIVE_FIXED){
-		infopacket->sb[6] |= 0x01; //VRR_EN Bit = 1
+				vrr->state == VRR_STATE_ACTIVE_FIXED) {
+		setFieldWithMask(&infopacket->sb[VRR_VTEM_MD0], MASK__VRR_VTEM_MD0__VRR_EN, 1);
 	} else {
-		infopacket->sb[6] &= 0xFE; //VRR_EN Bit = 0
+		setFieldWithMask(&infopacket->sb[VRR_VTEM_MD0], MASK__VRR_VTEM_MD0__VRR_EN, 0);
 	}
 
 	if (!stream->timing.vic) {
-		infopacket->sb[7] = stream->timing.v_front_porch;
+		setFieldWithMask(&infopacket->sb[VRR_VTEM_MD1], MASK__VRR_VTEM_MD1__BASE_VFRONT,
+				stream->timing.v_front_porch);
+
 
 		/* TODO: In dal2, we check mode flags for a reduced blanking timing.
 		 * Need a way to relay that information to this function.
 		 * if("ReducedBlanking")
 		 * {
-		 *   infopacket->sb[8] |= 0x20; //Set 3rd bit to 1
+		 *   setFieldWithMask(&infopacket->sb[VRR_VTEM_MD2], MASK__VRR_VTEM_MD2__RB, 1;
 		 * }
 		 */
+
+		//TODO: DAL2 does FixPoint and rounding. Here we might need to account for that
 		fieldRateInHz = (stream->timing.pix_clk_100hz * 100)/
-				(stream->timing.h_total * stream->timing.v_total);
+			(stream->timing.h_total * stream->timing.v_total);
 
-		infopacket->sb[8] |= ((fieldRateInHz & 0x300) >> 2);
-		infopacket->sb[9] |= fieldRateInHz & 0xFF;
+		setFieldWithMask(&infopacket->sb[VRR_VTEM_MD2],  MASK__VRR_VTEM_MD2__BASE_REFRESH_RATE_98,
+				fieldRateInHz >> 8);
+		setFieldWithMask(&infopacket->sb[VRR_VTEM_MD3], MASK__VRR_VTEM_MD3__BASE_REFRESH_RATE_07,
+				fieldRateInHz);
 
 	}
 	infopacket->valid = true;
@@ -765,6 +841,8 @@ static void build_vrr_infopacket_vtem(const struct dc_stream_state *stream,
 {
 	//VTEM info packet for HdmiVrr
 
+	memset(infopacket, 0, sizeof(struct dc_info_packet));
+
 	//VTEM Packet is structured differently
 	build_vrr_infopacket_header_vtem(stream->signal, infopacket);
 	build_vrr_vtem_infopacket_data(stream, vrr, infopacket);