1 // © 2022 Qualcomm Innovation Center, Inc. All rights reserved.
2 //
3 // SPDX-License-Identifier: BSD-3-Clause
4
5 #include <assert.h>
6 #include <hyptypes.h>
7
8 #include <hypregisters.h>
9
10 #include <compiler.h>
11 #include <cpulocal.h>
12 #include <platform_cpu.h>
13
14 platform_mpidr_mapping_t
platform_cpu_get_mpidr_mapping(void)15 platform_cpu_get_mpidr_mapping(void)
16 {
17 MPIDR_EL1_t real_mpidr = register_MPIDR_EL1_read();
18
19 return (platform_mpidr_mapping_t){
20 .aff_shift = { PLATFORM_MPIDR_AFF0_SHIFT,
21 PLATFORM_MPIDR_AFF1_SHIFT,
22 PLATFORM_MPIDR_AFF2_SHIFT,
23 PLATFORM_MPIDR_AFF3_SHIFT },
24 .aff_mask = { PLATFORM_MPIDR_AFF0_MASK,
25 PLATFORM_MPIDR_AFF1_MASK,
26 PLATFORM_MPIDR_AFF2_MASK,
27 PLATFORM_MPIDR_AFF3_MASK },
28 .multi_thread = MPIDR_EL1_get_MT(&real_mpidr),
29 .uniprocessor = MPIDR_EL1_get_U(&real_mpidr),
30 };
31 }
32
33 MPIDR_EL1_t
platform_cpu_map_index_to_mpidr(const platform_mpidr_mapping_t * mapping,index_t index)34 platform_cpu_map_index_to_mpidr(const platform_mpidr_mapping_t *mapping,
35 index_t index)
36 {
37 MPIDR_EL1_t mpidr = MPIDR_EL1_default();
38
39 assert(mapping->aff_shift[0] < 32U);
40 assert(mapping->aff_shift[1] < 32U);
41 assert(mapping->aff_shift[2] < 32U);
42 assert(mapping->aff_shift[3] < 32U);
43
44 MPIDR_EL1_set_Aff0(&mpidr, (uint8_t)((index >> mapping->aff_shift[0]) &
45 mapping->aff_mask[0]));
46 MPIDR_EL1_set_Aff1(&mpidr, (uint8_t)((index >> mapping->aff_shift[1]) &
47 mapping->aff_mask[1]));
48 MPIDR_EL1_set_Aff2(&mpidr, (uint8_t)((index >> mapping->aff_shift[2]) &
49 mapping->aff_mask[2]));
50 MPIDR_EL1_set_Aff3(&mpidr, (uint8_t)((index >> mapping->aff_shift[3]) &
51 mapping->aff_mask[3]));
52 MPIDR_EL1_set_MT(&mpidr, mapping->multi_thread);
53 MPIDR_EL1_set_U(&mpidr, mapping->uniprocessor);
54
55 return mpidr;
56 }
57
58 index_t
platform_cpu_map_mpidr_to_index(const platform_mpidr_mapping_t * mapping,MPIDR_EL1_t mpidr)59 platform_cpu_map_mpidr_to_index(const platform_mpidr_mapping_t *mapping,
60 MPIDR_EL1_t mpidr)
61 {
62 index_t index = 0U;
63
64 assert(mapping->aff_shift[0] < 32U);
65 assert(mapping->aff_shift[1] < 32U);
66 assert(mapping->aff_shift[2] < 32U);
67 assert(mapping->aff_shift[3] < 32U);
68
69 index |= ((index_t)MPIDR_EL1_get_Aff0(&mpidr) &
70 (index_t)mapping->aff_mask[0])
71 << mapping->aff_shift[0];
72 index |= ((index_t)MPIDR_EL1_get_Aff1(&mpidr) &
73 (index_t)mapping->aff_mask[1])
74 << mapping->aff_shift[1];
75 index |= ((index_t)MPIDR_EL1_get_Aff2(&mpidr) &
76 (index_t)mapping->aff_mask[2])
77 << mapping->aff_shift[2];
78 index |= ((index_t)MPIDR_EL1_get_Aff3(&mpidr) &
79 (index_t)mapping->aff_mask[3])
80 << mapping->aff_shift[3];
81
82 return index;
83 }
84
85 bool
platform_cpu_map_mpidr_valid(const platform_mpidr_mapping_t * mapping,MPIDR_EL1_t mpidr)86 platform_cpu_map_mpidr_valid(const platform_mpidr_mapping_t *mapping,
87 MPIDR_EL1_t mpidr)
88 {
89 bool valid = true;
90
91 assert(mapping->aff_shift[0] < 32U);
92 assert(mapping->aff_shift[1] < 32U);
93 assert(mapping->aff_shift[2] < 32U);
94 assert(mapping->aff_shift[3] < 32U);
95
96 if ((MPIDR_EL1_get_Aff0(&mpidr) & ~mapping->aff_mask[0]) != 0U) {
97 valid = false;
98 }
99 if ((MPIDR_EL1_get_Aff1(&mpidr) & ~mapping->aff_mask[1]) != 0U) {
100 valid = false;
101 }
102 if ((MPIDR_EL1_get_Aff2(&mpidr) & ~mapping->aff_mask[2]) != 0U) {
103 valid = false;
104 }
105 if ((MPIDR_EL1_get_Aff3(&mpidr) & ~mapping->aff_mask[3]) != 0U) {
106 valid = false;
107 }
108
109 return valid;
110 }
111
112 MPIDR_EL1_t
platform_cpu_index_to_mpidr(index_t index)113 platform_cpu_index_to_mpidr(index_t index)
114 {
115 platform_mpidr_mapping_t mapping = platform_cpu_get_mpidr_mapping();
116 return platform_cpu_map_index_to_mpidr(&mapping, index);
117 }
118
119 index_t
platform_cpu_mpidr_to_index(MPIDR_EL1_t mpidr)120 platform_cpu_mpidr_to_index(MPIDR_EL1_t mpidr)
121 {
122 platform_mpidr_mapping_t mapping = platform_cpu_get_mpidr_mapping();
123 return platform_cpu_map_mpidr_to_index(&mapping, mpidr);
124 }
125
126 bool
platform_cpu_mpidr_valid(MPIDR_EL1_t mpidr)127 platform_cpu_mpidr_valid(MPIDR_EL1_t mpidr)
128 {
129 platform_mpidr_mapping_t mapping = platform_cpu_get_mpidr_mapping();
130 return platform_cpu_map_mpidr_valid(&mapping, mpidr);
131 }
132