Signed-off-by: Sergey Kambalin <sergey.kambalin@auriga.com>
---
hw/gpio/bcm2838_gpio.c | 193 ++++++++++++++++++++++++++++++++++++++++-
1 file changed, 190 insertions(+), 3 deletions(-)
diff --git a/hw/gpio/bcm2838_gpio.c b/hw/gpio/bcm2838_gpio.c
index a312490bbd..69d15dbb49 100644
--- a/hw/gpio/bcm2838_gpio.c
+++ b/hw/gpio/bcm2838_gpio.c
@@ -18,6 +18,7 @@
#include "hw/sysbus.h"
#include "migration/vmstate.h"
#include "hw/gpio/bcm2838_gpio.h"
+#include "hw/irq.h"
#define GPFSEL0 0x00
#define GPFSEL1 0x04
@@ -56,14 +57,139 @@
#define RESET_VAL_CNTRL_REG2 0x50AAA95A
#define RESET_VAL_CNTRL_REG3 0x00055555
+#define NUM_FSELN_IN_GPFSELN 10
+#define NUM_BITS_FSELN 3
+#define MASK_FSELN 0x7
+
#define BYTES_IN_WORD 4
+static uint32_t gpfsel_get(BCM2838GpioState *s, uint8_t reg)
+{
+ int i;
+ uint32_t value = 0;
+ for (i = 0; i < NUM_FSELN_IN_GPFSELN; i++) {
+ uint32_t index = NUM_FSELN_IN_GPFSELN * reg + i;
+ if (index < sizeof(s->fsel)) {
+ value |= (s->fsel[index] & MASK_FSELN) << (NUM_BITS_FSELN * i);
+ }
+ }
+ return value;
+}
+
+static void gpfsel_set(BCM2838GpioState *s, uint8_t reg, uint32_t value)
+{
+ int i;
+ for (i = 0; i < NUM_FSELN_IN_GPFSELN; i++) {
+ uint32_t index = NUM_FSELN_IN_GPFSELN * reg + i;
+ if (index < sizeof(s->fsel)) {
+ int fsel = (value >> (NUM_BITS_FSELN * i)) & MASK_FSELN;
+ s->fsel[index] = fsel;
+ }
+ }
+}
+
+static int gpfsel_is_out(BCM2838GpioState *s, int index)
+{
+ if (index >= 0 && index < BCM2838_GPIO_NUM) {
+ return s->fsel[index] == 1;
+ }
+ return 0;
+}
+
+static void gpset(BCM2838GpioState *s, uint32_t val, uint8_t start,
+ uint8_t count, uint32_t *lev)
+{
+ uint32_t changes = val & ~*lev;
+ uint32_t cur = 1;
+
+ int i;
+ for (i = 0; i < count; i++) {
+ if ((changes & cur) && (gpfsel_is_out(s, start + i))) {
+ qemu_set_irq(s->out[start + i], 1);
+ }
+ cur <<= 1;
+ }
+
+ *lev |= val;
+}
+
+static void gpclr(BCM2838GpioState *s, uint32_t val, uint8_t start,
+ uint8_t count, uint32_t *lev)
+{
+ uint32_t changes = val & *lev;
+ uint32_t cur = 1;
+
+ int i;
+ for (i = 0; i < count; i++) {
+ if ((changes & cur) && (gpfsel_is_out(s, start + i))) {
+ qemu_set_irq(s->out[start + i], 0);
+ }
+ cur <<= 1;
+ }
+
+ *lev &= ~val;
+}
+
static uint64_t bcm2838_gpio_read(void *opaque, hwaddr offset, unsigned size)
{
+ BCM2838GpioState *s = (BCM2838GpioState *)opaque;
uint64_t value = 0;
- qemu_log_mask(LOG_UNIMP, "%s: %s: not implemented for %"HWADDR_PRIx"\n",
- TYPE_BCM2838_GPIO, __func__, offset);
+ switch (offset) {
+ case GPFSEL0:
+ case GPFSEL1:
+ case GPFSEL2:
+ case GPFSEL3:
+ case GPFSEL4:
+ case GPFSEL5:
+ value = gpfsel_get(s, offset / BYTES_IN_WORD);
+ break;
+ case GPSET0:
+ case GPSET1:
+ case GPCLR0:
+ case GPCLR1:
+ /* Write Only */
+ qemu_log_mask(LOG_GUEST_ERROR, "%s: %s: Attempt reading from write only"
+ " register. 0x%"PRIx64" will be returned."
+ " Address 0x%"HWADDR_PRIx", size %u\n",
+ TYPE_BCM2838_GPIO, __func__, value, offset, size);
+ break;
+ case GPLEV0:
+ value = s->lev0;
+ break;
+ case GPLEV1:
+ value = s->lev1;
+ break;
+ case GPEDS0:
+ case GPEDS1:
+ case GPREN0:
+ case GPREN1:
+ case GPFEN0:
+ case GPFEN1:
+ case GPHEN0:
+ case GPHEN1:
+ case GPLEN0:
+ case GPLEN1:
+ case GPAREN0:
+ case GPAREN1:
+ case GPAFEN0:
+ case GPAFEN1:
+ /* Not implemented */
+ qemu_log_mask(LOG_UNIMP, "%s: %s: not implemented for %"HWADDR_PRIx"\n",
+ TYPE_BCM2838_GPIO, __func__, offset);
+ break;
+ case GPIO_PUP_PDN_CNTRL_REG0:
+ case GPIO_PUP_PDN_CNTRL_REG1:
+ case GPIO_PUP_PDN_CNTRL_REG2:
+ case GPIO_PUP_PDN_CNTRL_REG3:
+ value = s->pup_cntrl_reg[(offset - GPIO_PUP_PDN_CNTRL_REG0)
+ / sizeof(s->pup_cntrl_reg[0])];
+ break;
+ default:
+ qemu_log_mask(LOG_GUEST_ERROR, "%s: %s: bad offset %"HWADDR_PRIx"\n",
+ TYPE_BCM2838_GPIO, __func__, offset);
+ break;
+ }
return value;
}
@@ -71,14 +197,75 @@ static uint64_t bcm2838_gpio_read(void *opaque, hwaddr offset, unsigned size)
static void bcm2838_gpio_write(void *opaque, hwaddr offset, uint64_t value,
unsigned size)
{
- qemu_log_mask(LOG_UNIMP, "%s: %s: not implemented for %"HWADDR_PRIx"\n",
+ BCM2838GpioState *s = (BCM2838GpioState *)opaque;
+
+ switch (offset) {
+ case GPFSEL0:
+ case GPFSEL1:
+ case GPFSEL2:
+ case GPFSEL3:
+ case GPFSEL4:
+ case GPFSEL5:
+ gpfsel_set(s, offset / BYTES_IN_WORD, value);
+ break;
+ case GPSET0:
+ gpset(s, value, 0, 32, &s->lev0);
+ break;
+ case GPSET1:
+ gpset(s, value, 32, 22, &s->lev1);
+ break;
+ case GPCLR0:
+ gpclr(s, value, 0, 32, &s->lev0);
+ break;
+ case GPCLR1:
+ gpclr(s, value, 32, 22, &s->lev1);
+ break;
+ case GPLEV0:
+ case GPLEV1:
+ /* Read Only */
+ qemu_log_mask(LOG_GUEST_ERROR, "%s: %s: Attempt writing 0x%"PRIx64""
+ " to read only register. Ignored."
+ " Address 0x%"HWADDR_PRIx", size %u\n",
+ TYPE_BCM2838_GPIO, __func__, value, offset, size);
+ break;
+ case GPEDS0:
+ case GPEDS1:
+ case GPREN0:
+ case GPREN1:
+ case GPFEN0:
+ case GPFEN1:
+ case GPHEN0:
+ case GPHEN1:
+ case GPLEN0:
+ case GPLEN1:
+ case GPAREN0:
+ case GPAREN1:
+ case GPAFEN0:
+ case GPAFEN1:
+ /* Not implemented */
+ qemu_log_mask(LOG_UNIMP, "%s: %s: not implemented for %"HWADDR_PRIx"\n",
+ TYPE_BCM2838_GPIO, __func__, offset);
+ break;
+ case GPIO_PUP_PDN_CNTRL_REG0:
+ case GPIO_PUP_PDN_CNTRL_REG1:
+ case GPIO_PUP_PDN_CNTRL_REG2:
+ case GPIO_PUP_PDN_CNTRL_REG3:
+ s->pup_cntrl_reg[(offset - GPIO_PUP_PDN_CNTRL_REG0)
+ / sizeof(s->pup_cntrl_reg[0])] = value;
+ break;
+ default:
+ qemu_log_mask(LOG_GUEST_ERROR, "%s: %s: bad offset %"HWADDR_PRIx"\n",
TYPE_BCM2838_GPIO, __func__, offset);
+ }
+ return;
}
static void bcm2838_gpio_reset(DeviceState *dev)
{
BCM2838GpioState *s = BCM2838_GPIO(dev);
+ memset(s->fsel, 0, sizeof(s->fsel));
+
s->lev0 = 0;
s->lev1 = 0;
--
2.34.1