linux-kernel-module-cheat/lkmc.c at master · cirosantilli/linux-kernel-module-cheat · GitHub

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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
/* https://cirosantilli.com/linux-kernel-module-cheat#lkmc-c */

#include <math.h>
#include <stdio.h>
#include <stdio.h>
#include <stdlib.h>

#include <lkmc.h>

#define LKMC_ASSERT_EQ_DEFINE(bits) \
    LKMC_ASSERT_EQ_DECLARE(bits) \
    { \
        if (val1 != val2) { \
            printf("%s failed\n", __func__); \
            printf("val1 0x%" PRIX ## bits "\n", val1); \
            printf("val2 0x%" PRIX ## bits "\n", val2); \
            lkmc_assert_fail(line); \
        } \
    }
LKMC_ASSERT_EQ_DEFINE(32)
LKMC_ASSERT_EQ_DEFINE(64)
#undef ASSERT_EQ_DEFINE

void lkmc_assert_fail(uint32_t line) {
    printf("error: assertion failed at line: %" PRIu32 "\n", line);
    fflush(stdout);
    abort();
}

void lkmc_assert_memcmp(
    const void *s1,
    const void *s2,
    size_t n,
    uint32_t line
) {
    size_t i;
    uint8_t *s1b, *s2b;
    uint8_t b1, b2;

    s1b = (uint8_t *)s1;
    s2b = (uint8_t *)s2;
    for (i = 0; i < n; ++i) {
        b1 = s1b[i];
        b2 = s2b[i];
        if (b1 != b2) {
            printf(
                "%s failed: "
                "byte1, byte2, index: "
                "0x%02" PRIX8 " 0x%02" PRIX8 " 0x%zX\n",
                __func__,
                b1,
                b2,
                i
            );
            lkmc_assert_fail(line);
        }
    }
}

void __attribute__ ((noinline)) lkmc_busy_loop(
    unsigned long long max,
    unsigned long long max2
) {
    for (unsigned long long i = 0; i < max2; i++) {
        for (unsigned long long j = 0; j < max; j++) {
            __asm__ __volatile__ ("" : "+g" (i), "+g" (j) : :);
        }
    }
}

void lkmc_print_hex_32(uint32_t x) {
    printf("0x%08" PRIX32, x);
}

void lkmc_print_hex_64(uint64_t x) {
    printf("0x%016" PRIX64, x);
}

void lkmc_print_newline() {
    printf("\n");
}

#if defined(__arm__)

void lkmc_arm_psci_cpu_on(
    uint32_t target_cpu,
    uint32_t entry_point_address,
    uint32_t context_id
) {
    register int r0 __asm__ ("r0") = 0x84000003;
    register int r1 __asm__ ("r1") = target_cpu;
    register int r2 __asm__ ("r2") = entry_point_address;
    register int r3 __asm__ ("r3") = context_id;
    __asm__ __volatile__(
        "hvc 0\n"
        :
        : "r" (r0),
          "r" (r1),
          "r" (r2),
          "r" (r3)
        :
    );
}

#elif defined(__aarch64__)

#define LKMC_SYSREG_READ_WRITE(nbits, name) \
    LKMC_CONCAT(LKMC_CONCAT(uint, nbits), _t) LKMC_CONCAT(LKMC_CONCAT(LKMC_SYSREG_SYMBOL_PREFIX, read_), name)(void) { \
        LKMC_CONCAT(LKMC_CONCAT(uint, nbits), _t) name; \
        __asm__ __volatile__("mrs %0, " #name : "=r" (name) : : ); \
        return name; \
    } \
    void LKMC_CONCAT(LKMC_CONCAT(LKMC_SYSREG_SYMBOL_PREFIX, write_), name)(LKMC_CONCAT(LKMC_CONCAT(uint, nbits), _t) name) { \
        __asm__ __volatile__("msr " #name ", %0" : : "r" (name) : ); \
    } \
    void LKMC_CONCAT(LKMC_CONCAT(LKMC_SYSREG_SYMBOL_PREFIX, print_), name)(void) { \
        printf(#name " 0x%" PRIX ## nbits "\n", LKMC_CONCAT(LKMC_CONCAT(LKMC_SYSREG_SYMBOL_PREFIX, read_), name)()); \
    }
LKMC_SYSREG_OPS
#undef LKMC_SYSREG_READ_WRITE

uint64_t lkmc_aarch64_cpu_id() {
    /* TODO: cores beyond 4th?
     * Mnemonic: Main Processor ID Register
     */
    return lkmc_sysreg_read_mpidr_el1() & 3;
}

void lkmc_aarch64_psci_cpu_on(
    uint64_t target_cpu,
    uint64_t entry_point_address,
    uint64_t context_id
) {
    register int w0 __asm__ ("w0") = 0xc4000003;
    register int x1 __asm__ ("x1") = target_cpu;
    register int x2 __asm__ ("x2") = entry_point_address;
    register int x3 __asm__ ("x3") = context_id;
    __asm__ __volatile__(
        "hvc 0\n"
        :
        : "r" (w0),
          "r" (x1),
          "r" (x2),
          "r" (x3)
        :
    );
}
#endif