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 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(data: ci); |
69 | x0 = dp_utils_get_g0_value(data: ci); |
70 | } |
71 | |
72 | parity_byte = x1 | (x0 << 4); |
73 | |
74 | return parity_byte; |
75 | } |
76 | |
77 | ssize_t (struct dp_sdp_header *, u32 *) |
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 | |