1/*
2 * Copyright 2019, Data61, CSIRO (ABN 41 687 119 230)
3 *
4 * SPDX-License-Identifier: BSD-2-Clause
5 */
6
7#include <stdlib.h>
8#include <string.h>
9
10#include <sel4vm/guest_vm.h>
11#include <sel4vm/guest_vcpu_fault.h>
12#include <sel4vmmplatsupport/device.h>
13#include <sel4vmmplatsupport/plat/device_map.h>
14#include <sel4vmmplatsupport/plat/vgpio.h>
15
16#include <platsupport/gpio.h>
17#include <platsupport/plat/gpio.h>
18
19#define GPIOREG_CON_OFFSET    0x00
20#define GPIOREG_DAT_OFFSET    0x04
21#define GPIOREG_PUD_OFFSET    0x08
22#define GPIOREG_DRV_OFFSET    0x0C
23#define GPIOREG_CONPDN_OFFSET 0x10
24#define GPIOREG_PUDPDN_OFFSET 0x14
25#define GPIOREG_SIZE          0x20
26#define GPIOREG_OFFSET_MASK   ((GPIOREG_SIZE - 1) & ~MASK(2))
27
28#define GPIOREG_CON_BITS    4
29#define GPIOREG_DAT_BITS    1
30#define GPIOREG_PUD_BITS    2
31#define GPIOREG_DRV_BITS    2
32#define GPIOREG_CONPDN_BITS 2
33#define GPIOREG_PUDPDN_BITS 2
34
35#define NPORTS_PER_BANK (0x1000 / GPIOREG_SIZE)
36#define NPINS_PER_PORT  (8)
37#define NPINS_PER_BANK (NPORTS_PER_BANK * NPINS_PER_PORT)
38
39static memory_fault_result_t handle_vgpio_right_fault(vm_t *vm, vm_vcpu_t *vcpu, uintptr_t fault_addr,
40                                                      size_t fault_length, void *cookie);
41static memory_fault_result_t handle_vgpio_left_fault(vm_t *vm, vm_vcpu_t *vcpu, uintptr_t fault_addr,
42                                                     size_t fault_length, void *cookie);
43
44struct gpio_device {
45    vm_t *vm;
46    enum vacdev_action action;
47    void *regs[GPIO_NBANKS];
48    uint8_t granted_bf[GPIO_NBANKS][((NPINS_PER_BANK - 1 / 8) + 1)];
49};
50
51const struct device dev_gpio_left = {
52    .name = "gpio.left",
53    .pstart = GPIO_LEFT_PADDR,
54    .size = 0x1000,
55    .priv = NULL
56};
57
58const struct device dev_gpio_right = {
59    .name = "gpio.right",
60    .pstart = GPIO_RIGHT_PADDR,
61    .size = 0x1000,
62    .priv = NULL
63};
64
65static const struct device *gpio_devices[] = {
66    [GPIO_LEFT_BANK]  = &dev_gpio_left,
67    [GPIO_RIGHT_BANK] = &dev_gpio_right,
68    [GPIO_C2C_BANK]   = NULL,
69    [GPIO_AUDIO_BANK] = NULL,
70};
71
72static uint32_t _create_mask(uint8_t ac, int bits)
73{
74    uint32_t mask = 0;
75    while (ac) {
76        int pin = CTZ(ac);
77        ac &= ~BIT(pin);
78        mask |= MASK(bits) << (bits * pin);
79    }
80    return mask;
81}
82
83static memory_fault_result_t handle_vgpio_fault(vm_t *vm, vm_vcpu_t *vcpu, struct device *dev, uintptr_t addr,
84                                                size_t len,  int bank)
85{
86    struct gpio_device *gpio_device = (struct gpio_device *)dev->priv;
87    volatile uint32_t *reg;
88    int offset;
89    if (gpio_device->regs[bank] == NULL) {
90        /* We could not map the device. Lets return SBZ */
91        if (is_vcpu_read_fault(vcpu)) {
92            set_vcpu_fault_data(vcpu, 0);
93        }
94        advance_vcpu_fault(vcpu);
95        return FAULT_HANDLED;
96    }
97
98    /* Gather fault information */
99    offset = addr - dev->pstart;
100    reg = (volatile uint32_t *)(gpio_device->regs[bank] + offset);
101    if (is_vcpu_read_fault(vcpu)) {
102        set_vcpu_fault_data(vcpu, *reg);
103        ZF_LOGD("[%s] pc 0x%08x | r 0x%08x:0x%08x\n", gpio_devices[bank]->name,
104                get_vcpu_fault_ip(vcpu), addr,
105                get_vcpu_fault_data(vcpu));
106    } else {
107        uint32_t mask;
108        uint32_t change;
109        ZF_LOGD("[%s] pc 0x%08x | w 0x%08x:0x%08x\n", gpio_devices[bank]->name,
110                get_vcpu_fault_ip(vcpu), addr,
111                get_vcpu_fault_data(vcpu));
112        if ((offset >= 0x700 && offset < 0xC00) || offset >= 0xE00) {
113            /* Not implemented */
114            mask = 0xFFFFFFFF;
115        } else {
116            /* GPIO and MUX general registers */
117            uint32_t ac;
118            int port;
119            port = offset / GPIOREG_SIZE;
120            assert(port < sizeof(gpio_device->granted_bf[bank]));
121            ac = gpio_device->granted_bf[bank][port];
122            switch (offset & GPIOREG_OFFSET_MASK) {
123            case GPIOREG_CON_OFFSET:
124                mask = _create_mask(ac, GPIOREG_CON_BITS);
125                break;
126            case GPIOREG_DAT_OFFSET:
127                mask = _create_mask(ac, GPIOREG_DAT_BITS);
128                break;
129            case GPIOREG_PUD_OFFSET:
130                mask = _create_mask(ac, GPIOREG_PUD_BITS);
131                break;
132            case GPIOREG_DRV_OFFSET:
133                mask = _create_mask(ac, GPIOREG_DRV_BITS);
134                break;
135            case GPIOREG_CONPDN_OFFSET:
136                mask = _create_mask(ac, GPIOREG_CONPDN_BITS);
137                break;
138            case GPIOREG_PUDPDN_OFFSET:
139                mask = _create_mask(ac, GPIOREG_PUDPDN_BITS);
140                break;
141            default:  /* reserved */
142                ZF_LOGE("reserved GPIO 0x%x\n", offset);
143                mask = 0;
144            }
145        }
146        change = emulate_vcpu_fault(vcpu, *reg);
147        if ((change ^ *reg) & ~mask) {
148            switch (gpio_device->action) {
149            case VACDEV_REPORT_AND_MASK:
150                change = (change & mask) | (*reg & ~mask);
151            case VACDEV_REPORT_ONLY:
152                /* Fallthrough */
153                printf("[%s] pc 0x%08x| w @ 0x%08x: 0x%08x -> 0x%08x\n",
154                       gpio_devices[bank]->name,  get_vcpu_fault_ip(vcpu),
155                       addr, *reg, change);
156                break;
157            default:
158                break;
159            }
160        }
161        *reg = change;
162    }
163    advance_vcpu_fault(vcpu);
164    return FAULT_HANDLED;
165}
166
167static memory_fault_result_t handle_vgpio_right_fault(vm_t *vm, vm_vcpu_t *vcpu, uintptr_t fault_addr,
168                                                      size_t fault_length, void *cookie)
169{
170    struct device *dev = (struct device *)cookie;
171    return handle_vgpio_fault(vm, vcpu, dev, fault_addr, fault_length, GPIO_RIGHT_BANK);
172}
173
174static memory_fault_result_t handle_vgpio_left_fault(vm_t *vm, vm_vcpu_t *vcpu, uintptr_t fault_addr,
175                                                     size_t fault_length, void *cookie)
176{
177    struct device *dev = (struct device *)cookie;
178    return handle_vgpio_fault(vm, vcpu, dev, fault_addr, fault_length, GPIO_LEFT_BANK);
179}
180
181
182struct gpio_device *
183vm_install_ac_gpio(vm_t *vm, enum vacdev_default default_ac, enum vacdev_action action)
184{
185    struct gpio_device *gpio_device;
186    vspace_t *vmm_vspace;
187    uint8_t ac = (default_ac == VACDEV_DEFAULT_ALLOW) ? 0xff : 0x00;
188    int i;
189
190    gpio_device = (struct gpio_device *)calloc(1, sizeof(*gpio_device));
191    if (gpio_device == NULL) {
192        return NULL;
193    }
194    gpio_device->vm = vm;
195    gpio_device->action = action;
196    memset(gpio_device->granted_bf, ac, sizeof(gpio_device->granted_bf));
197    for (i = 0; i < GPIO_NBANKS; i++) {
198        /* Map the device locally */
199        if (gpio_devices[i] != NULL) {
200            struct device dev;
201            int err;
202            dev = *gpio_devices[i];
203            dev.priv = gpio_device;
204            gpio_device->regs[i] = ps_io_map(&vm->io_ops->io_mapper, dev.pstart, PAGE_SIZE_4K, 0, PS_MEM_NORMAL);
205            /* Ignore failues. We will check regs for NULL on access */
206            if (gpio_device->regs[i] != NULL) {
207                memory_fault_callback_fn callback = (i == GPIO_LEFT_BANK) ? handle_vgpio_left_fault : handle_vgpio_right_fault;
208                vm_memory_reservation_t *reservation = vm_reserve_memory_at(vm, dev.pstart, dev.size,
209                                                                            callback, (void *)&dev);
210            } else {
211                LOG_INFO("Failed to provide device region 0x%08x->0x%08x", dev.pstart, dev.pstart + dev.size);
212            }
213        }
214    }
215    return gpio_device;
216}
217
218static int vm_gpio_config(struct gpio_device *gpio_device, gpio_id_t gpio_id, int provide)
219{
220    int gpioport;
221    int port, pin, bank;
222    gpioport = GPIOID_PORT(gpio_id);
223    bank = GPIOPORT_GET_BANK(gpioport);
224    port = GPIOPORT_GET_PORT(gpioport);
225    pin = GPIOID_PIN(gpio_id);
226
227    if (provide) {
228        gpio_device->granted_bf[bank][port] |= BIT(pin);
229    } else {
230        gpio_device->granted_bf[bank][port] &= ~BIT(pin);
231    }
232    return 0;
233}
234
235int vm_gpio_provide(struct gpio_device *gpio_device, gpio_id_t gpio_id)
236{
237    return vm_gpio_config(gpio_device, gpio_id, 1);
238}
239
240int vm_gpio_restrict(struct gpio_device *gpio_device, gpio_id_t gpio_id)
241{
242    return vm_gpio_config(gpio_device, gpio_id, 0);
243}
244