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