1/*
2 * Copyright 2014, General Dynamics C4 Systems
3 *
4 * This software may be distributed and modified according to the terms of
5 * the GNU General Public License version 2. Note that NO WARRANTY is provided.
6 * See "LICENSE_GPLv2.txt" for details.
7 *
8 * @TAG(GD_GPL)
9 */
10
11
12#include <config.h>
13#include <object/structures.h>
14#include <object/tcb.h>
15#include <model/statedata.h>
16#include <machine/capdl.h>
17#include <arch/machine/capdl.h>
18#include <machine/io.h>
19#include <plat/machine/hardware.h>
20
21#ifdef CONFIG_DEBUG_BUILD
22
23#define ARCH 0xe0
24
25#define PD_READ_SIZE         BIT(PD_INDEX_BITS)
26#define PT_READ_SIZE         BIT(PT_INDEX_BITS)
27#define ASID_POOL_READ_SIZE  BIT(ASID_POOL_INDEX_BITS)
28
29static int getDecodedChar(unsigned char *result)
30{
31    unsigned char c;
32    c = getDebugChar();
33    if (c == START) {
34        return 1;
35    }
36    if (c == ESCAPE) {
37        c = getDebugChar();
38        if (c == START) {
39            return 1;
40        }
41        switch (c) {
42        case ESCAPE_ESCAPE:
43            *result = ESCAPE;
44            break;
45        case START_ESCAPE:
46            *result = START;
47            break;
48        case END_ESCAPE:
49            *result = END;
50            break;
51        default:
52            if (c >= 20 && c < 40) {
53                *result = c - 20;
54            }
55        }
56        return 0;
57    } else {
58        *result = c;
59        return 0;
60    }
61}
62
63static void putEncodedChar(unsigned char c)
64{
65    switch (c) {
66    case ESCAPE:
67        putDebugChar(ESCAPE);
68        putDebugChar(ESCAPE_ESCAPE);
69        break;
70    case START:
71        putDebugChar(ESCAPE);
72        putDebugChar(START_ESCAPE);
73        break;
74    case END:
75        putDebugChar(ESCAPE);
76        putDebugChar(END_ESCAPE);
77        break;
78    default:
79        if (c < 20) {
80            putDebugChar(ESCAPE);
81            putDebugChar(c + 20);
82        } else {
83            putDebugChar(c);
84        }
85    }
86}
87
88static int getArg32(unsigned int *res)
89{
90    unsigned char b1 = 0;
91    unsigned char b2 = 0;
92    unsigned char b3 = 0;
93    unsigned char b4 = 0;
94    if (getDecodedChar(&b1)) {
95        return 1;
96    }
97    if (getDecodedChar(&b2)) {
98        return 1;
99    }
100    if (getDecodedChar(&b3)) {
101        return 1;
102    }
103    if (getDecodedChar(&b4)) {
104        return 1;
105    }
106    *res = (b1 << 24 ) | (b2 << 16) | (b3 << 8) | b4;
107    return 0;
108}
109
110static void sendWord(unsigned int word)
111{
112    putEncodedChar(word & 0xff);
113    putEncodedChar((word >> 8) & 0xff);
114    putEncodedChar((word >> 16) & 0xff);
115    putEncodedChar((word >> 24) & 0xff);
116}
117
118static cte_t *getMDBParent(cte_t *slot)
119{
120    cte_t *oldSlot = CTE_PTR(mdb_node_get_mdbPrev(slot->cteMDBNode));
121
122    while (oldSlot != 0 && !isMDBParentOf(oldSlot, slot)) {
123        oldSlot = CTE_PTR(mdb_node_get_mdbPrev(oldSlot->cteMDBNode));
124    }
125
126    return oldSlot;
127}
128
129static void sendPD(unsigned int address)
130{
131    word_t i, exists;
132    pde_t *start = (pde_t *)address;
133    for (i = 0; i < PD_READ_SIZE; i++) {
134        pde_t pde = start[i];
135        exists = 0;
136        if (pde_get_pdeType(pde) == pde_pde_coarse && pde_pde_coarse_get_address(pde) != 0) {
137            exists = 1;
138        } else if (pde_get_pdeType(pde) == pde_pde_section && (pde_pde_section_get_address(pde) != 0 ||
139#ifdef CONFIG_ARM_HYPERVISOR_SUPPORT
140                                                               pde_pde_section_get_HAP(pde))) {
141#else
142                                                               pde_pde_section_get_AP(pde))) {
143#endif
144            exists = 1;
145        }
146        if (exists != 0 && i < kernelBase >> pageBitsForSize(ARMSection)) {
147            sendWord(i);
148            sendWord(pde.words[0]);
149        }
150    }
151}
152
153static void sendPT(unsigned int address)
154{
155    word_t i, exists;
156    pte_t *start = (pte_t *)address;
157    for (i = 0; i < PT_READ_SIZE; i++) {
158        pte_t pte = start[i];
159        exists = 0;
160#ifdef CONFIG_ARM_HYPERVISOR_SUPPORT
161        if (pte_get_pteType(pte) == pte_pte_small && (pte_pte_small_get_address(pte) != 0 ||
162                                                      pte_pte_small_get_HAP(pte))) {
163            exists = 1;
164        }
165#else
166        if (pte_get_pteType(pte) == pte_pte_large && (pte_pte_large_get_address(pte) != 0 ||
167                                                      pte_pte_large_get_AP(pte))) {
168            exists = 1;
169        } else if (pte_get_pteType(pte) == pte_pte_small && (pte_pte_small_get_address(pte) != 0 ||
170                                                             pte_pte_small_get_AP(pte))) {
171            exists = 1;
172        }
173#endif
174        if (exists != 0) {
175            sendWord(i);
176            sendWord(pte.words[0]);
177        }
178    }
179}
180
181static void sendASIDPool(unsigned int address)
182{
183    word_t i;
184    pde_t **start = (pde_t **)address;
185    for (i = 0; i < ASID_POOL_READ_SIZE; i++) {
186        pde_t *pde = start[i];
187        if (pde != 0) {
188            sendWord(i);
189            sendWord((unsigned int)pde);
190        }
191    }
192}
193
194static void sendRunqueues(void)
195{
196    word_t i;
197    sendWord((unsigned int) NODE_STATE(ksCurThread));
198    for (i = 0; i < NUM_READY_QUEUES; i++) {
199        tcb_t *current = NODE_STATE(ksReadyQueues[i]).head;
200        if (current != 0) {
201            while (current != NODE_STATE(ksReadyQueues[i]).end) {
202                sendWord((unsigned int)current);
203                current = current -> tcbSchedNext;
204            }
205            sendWord((unsigned int)current);
206        }
207    }
208}
209
210static void sendEPQueue(unsigned int epptr)
211{
212    tcb_t *current = (tcb_t *)endpoint_ptr_get_epQueue_head((endpoint_t *)epptr);
213    tcb_t *tail = (tcb_t *)endpoint_ptr_get_epQueue_tail((endpoint_t *)epptr);
214    if (current == 0) {
215        return;
216    }
217    while (current != tail) {
218        sendWord((unsigned int)current);
219        current = current->tcbEPNext;
220    }
221    sendWord((unsigned int)current);
222}
223
224static void sendCNode(unsigned int address, unsigned int sizebits)
225{
226    word_t i;
227    cte_t *start = (cte_t *)address;
228    for (i = 0; i < (1 << sizebits); i++) {
229        cap_t cap = start[i].cap;
230        if (cap_get_capType(cap) != cap_null_cap) {
231            cte_t *parent = getMDBParent(&start[i]);
232            sendWord(i);
233            sendWord(cap.words[0]);
234            sendWord(cap.words[1]);
235            sendWord((unsigned int)parent);
236        }
237    }
238}
239
240static void sendIRQNode(void)
241{
242    sendCNode((unsigned int)intStateIRQNode, 8);
243}
244
245static void sendVersion(void)
246{
247    sendWord(ARCH);
248    sendWord(CAPDL_VERSION);
249}
250
251void capDL(void)
252{
253    int result;
254    int done = 0;
255    while (done == 0) {
256        unsigned char c;
257        do {
258            c = getDebugChar();
259        } while (c != START);
260        do {
261            result = getDecodedChar(&c);
262            if (result) {
263                continue;
264            }
265            switch (c) {
266            case PD_COMMAND: {
267                /*pgdir */
268                unsigned int arg;
269                result = getArg32(&arg);
270                if (result) {
271                    continue;
272                }
273                sendPD(arg);
274                putDebugChar(END);
275            }
276            break;
277            case PT_COMMAND: {
278                /*pg table */
279                unsigned int arg;
280                result = getArg32(&arg);
281                if (result) {
282                    continue;
283                }
284                sendPT(arg);
285                putDebugChar(END);
286            }
287            break;
288            case ASID_POOL_COMMAND: {
289                /*asid pool */
290                unsigned int arg;
291                result = getArg32(&arg);
292                if (result) {
293                    continue;
294                }
295                sendASIDPool(arg);
296                putDebugChar(END);
297            }
298            break;
299            case RQ_COMMAND: {
300                /*runqueues */
301                sendRunqueues();
302                putDebugChar(END);
303                result = 0;
304            }
305            break;
306            case EP_COMMAND: {
307                /*endpoint waiters */
308                unsigned int arg;
309                result = getArg32(&arg);
310                if (result) {
311                    continue;
312                }
313                sendEPQueue(arg);
314                putDebugChar(END);
315            }
316            break;
317            case CN_COMMAND: {
318                /*cnode */
319                unsigned int address, sizebits;
320                result = getArg32(&address);
321                if (result) {
322                    continue;
323                }
324                result = getArg32(&sizebits);
325                if (result) {
326                    continue;
327                }
328
329                sendCNode(address, sizebits);
330                putDebugChar(END);
331            }
332            break;
333            case IRQ_COMMAND: {
334                sendIRQNode();
335                putDebugChar(END);
336                result = 0;
337            }
338            break;
339            case VERSION_COMMAND: {
340                sendVersion();
341                putDebugChar(END);
342            }
343            break;
344            case DONE: {
345                done = 1;
346                putDebugChar(END);
347            }
348            default:
349                result = 0;
350                break;
351            }
352        } while (result);
353    }
354}
355
356#endif
357