97 lines · c
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 811 12u8 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 33u8 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 54u8 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 77ssize_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