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
dp_utils_get_g0_value(u8 data)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
dp_utils_get_g1_value(u8 data)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
dp_utils_calculate_parity(u32 data)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
dp_utils_pack_sdp_header(struct dp_sdp_header * sdp_header,u32 * header_buff)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