1 /* SPDX-License-Identifier: GPL-2.0-only */ 2 /* 3 * Copyright (c) 2024, The Linux Foundation. All rights reserved. 4 */ 5 6 #include <linux/types.h> 7 8 #include "dp_utils.h" 9 10 #define DP_SDP_HEADER_SIZE 8 11 12 u8 dp_utils_get_g0_value(u8 data) 13 { 14 u8 c[4]; 15 u8 g[4]; 16 u8 ret_data = 0; 17 u8 i; 18 19 for (i = 0; i < 4; i++) 20 c[i] = (data >> i) & 0x01; 21 22 g[0] = c[3]; 23 g[1] = c[0] ^ c[3]; 24 g[2] = c[1]; 25 g[3] = c[2]; 26 27 for (i = 0; i < 4; i++) 28 ret_data = ((g[i] & 0x01) << i) | ret_data; 29 30 return ret_data; 31 } 32 33 u8 dp_utils_get_g1_value(u8 data) 34 { 35 u8 c[4]; 36 u8 g[4]; 37 u8 ret_data = 0; 38 u8 i; 39 40 for (i = 0; i < 4; i++) 41 c[i] = (data >> i) & 0x01; 42 43 g[0] = c[0] ^ c[3]; 44 g[1] = c[0] ^ c[1] ^ c[3]; 45 g[2] = c[1] ^ c[2]; 46 g[3] = c[2] ^ c[3]; 47 48 for (i = 0; i < 4; i++) 49 ret_data = ((g[i] & 0x01) << i) | ret_data; 50 51 return ret_data; 52 } 53 54 u8 dp_utils_calculate_parity(u32 data) 55 { 56 u8 x0 = 0; 57 u8 x1 = 0; 58 u8 ci = 0; 59 u8 iData = 0; 60 u8 i = 0; 61 u8 parity_byte; 62 u8 num_byte = (data & 0xFF00) > 0 ? 8 : 2; 63 64 for (i = 0; i < num_byte; i++) { 65 iData = (data >> i * 4) & 0xF; 66 67 ci = iData ^ x1; 68 x1 = x0 ^ dp_utils_get_g1_value(ci); 69 x0 = dp_utils_get_g0_value(ci); 70 } 71 72 parity_byte = x1 | (x0 << 4); 73 74 return parity_byte; 75 } 76 77 ssize_t dp_utils_pack_sdp_header(struct dp_sdp_header *sdp_header, u32 *header_buff) 78 { 79 size_t length; 80 81 length = sizeof(header_buff); 82 if (length < DP_SDP_HEADER_SIZE) 83 return -ENOSPC; 84 85 header_buff[0] = FIELD_PREP(HEADER_0_MASK, sdp_header->HB0) | 86 FIELD_PREP(PARITY_0_MASK, dp_utils_calculate_parity(sdp_header->HB0)) | 87 FIELD_PREP(HEADER_1_MASK, sdp_header->HB1) | 88 FIELD_PREP(PARITY_1_MASK, dp_utils_calculate_parity(sdp_header->HB1)); 89 90 header_buff[1] = FIELD_PREP(HEADER_2_MASK, sdp_header->HB2) | 91 FIELD_PREP(PARITY_2_MASK, dp_utils_calculate_parity(sdp_header->HB2)) | 92 FIELD_PREP(HEADER_3_MASK, sdp_header->HB3) | 93 FIELD_PREP(PARITY_3_MASK, dp_utils_calculate_parity(sdp_header->HB3)); 94 95 return length; 96 } 97