1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
/*
* $Copyright:
* ----------------------------------------------------------------
* This confidential and proprietary software may be used only as
* authorised by a licensing agreement from ARM Limited
* (C) COPYRIGHT 2008-2011 ARM Limited
* ALL RIGHTS RESERVED
* The entire notice above must be reproduced on all authorised
* copies and copies may only be made to the extent permitted
* by a licensing agreement from ARM Limited.
* ----------------------------------------------------------------
* File: mem_trap.c
* ----------------------------------------------------------------
* $
*/
#include "virtualisor.h"
#include "misc.h"
#include "virt_helpers.h"
#include "mem_trap.h"
/*
* Generic call to make accesses to a peripheral trap into the
* HYP mode by invalidating its descriptor in the 2nd stage
* translation tables
*/
unsigned mem_trap_setup(unsigned periph_addr, mem_trap_data *periph_trap_data)
{
unsigned rc = 0x0, four_kb_index = 0;
unsigned one_gb_index = 0, two_mb_index = 0;
unsigned long long vtcr = 0x0, hcr = 0x0, level = 0;
unsigned long long pagetable_base = 0x0, l2_desc = 0;
unsigned long long l3_desc = 0, l3_table = 0;
/* Check if 2nd stage translations are enabled */
hcr = read_hcr();
if (!(hcr & HCR_VM)) {
printf("%s: 2nd Stage translations not enabled \n", __FUNCTION__);
rc = 0x1;
goto out;
}
/* Check what level of tables we need to start at */
vtcr = read_vtcr();
level = (vtcr >> 6) & 0x3;
/* Read the page table base address. */
pagetable_base = read_vttbr();
/* Calculate the table indices */
one_gb_index = periph_addr >> 30;
/* Each GB contains (1 << 9) or 512 2MBs */
two_mb_index = (periph_addr >> 21) - ((1 << 9) * one_gb_index);
/* Each GB contains (1 << 18) or 262144 4KBs */
four_kb_index = (periph_addr >> 12) - ((1 << 9) * (periph_addr >> 21));
/* For either starting level find out the level 2 desc */
switch (level) {
case 0x1:
{
/* Start from first level */
unsigned long long l1_desc = 0;
unsigned long long l2_table = 0;
l1_desc = ((unsigned long long *)((unsigned)(&pagetable_base)[0]))[one_gb_index];
if ((l1_desc & 0x3) != TABLE_MAPPING) {
printf("%s: Invalid 1st level desc : 0x%llu \n", __FUNCTION__, l1_desc);
rc = 0x1;
goto out;
}
l2_table = l1_desc & 0xfffffff000UL;
l2_desc = ((unsigned long long *)((unsigned)(&l2_table)[0]))[two_mb_index];
break;
}
case 0x0:
{
/* Start from second level */
l2_desc = ((unsigned long long *)((unsigned)(&pagetable_base)[0]))[two_mb_index];
break;
}
default:
printf("%s: Invalid Pagetable level \n", __FUNCTION__);
rc = 0x1;
}
/* Validate the 2nd level descriptor */
if ((l2_desc & 0x3) != TABLE_MAPPING) {
printf("%s: Invalid 2nd level desc : 0x%llu \n",
__FUNCTION__, l2_desc);
rc = 0x1;
goto out;
}
l3_table = l2_desc & 0xfffffff000UL;
l3_desc = ((unsigned long long *)((unsigned)(&l3_table)[0]))[four_kb_index];
/*
* Validate the 3rd level descriptor. This means that the mapping is
* already invalid and we have not touched it
*/
if ((l3_desc & 0x3) != VALID_MAPPING) {
printf("%s: Invalid 3rd level desc : 0x%llu \n",
__FUNCTION__, l3_desc);
rc = 0x1;
goto out;
}
/* Save the info gathered so far */
periph_trap_data->table = l3_table;
periph_trap_data->index = four_kb_index;
periph_trap_data->prev_desc = l3_desc;
periph_trap_data->cluster_id = read_clusterid();
periph_trap_data->valid = 1;
/* Invalidate the peripheral page table entry */
((unsigned long long *)((unsigned)(&l3_table)[0]))[four_kb_index] = 0x0;
out:
return rc;
}
|
