static int pio_set(char* buf, mmap_gpio* pio) { uint32_t port = pio->port - 'A'; if (port > PIO_NR_PORTS) { return -1; } uint32_t *addr, val; uint32_t port_num_func, port_num_pull; uint32_t offset_func, offset_pull; port_num_func = pio->pin >> 3; offset_func = ((pio->pin & 0x07) << 2); port_num_pull = pio->pin >> 4; offset_pull = ((pio->pin & 0x0f) << 1); /* func */ if (pio->cfg >= 0) { addr = (uint32_t*)PIO_REG_CFG(buf, port, port_num_func); val = le32toh(*addr); val &= ~(0x07 << offset_func); val |= (pio->cfg & 0x07) << offset_func; *addr = htole32(val); } /* pull */ if (pio->pull >= 0) { addr = (uint32_t*)PIO_REG_PULL(buf, port, port_num_pull); val = le32toh(*addr); val &= ~(0x03 << offset_pull); val |= (pio->pull & 0x03) << offset_pull; *addr = htole32(val); } /* dlevel */ if (pio->drv_level >= 0) { addr = (uint32_t*)PIO_REG_DLEVEL(buf, port, port_num_pull); val = le32toh(*addr); val &= ~(0x03 << offset_pull); val |= (pio->drv_level & 0x03) << offset_pull; *addr = htole32(val); } /* data */ if (pio->data >= 0) { addr = (uint32_t*)PIO_REG_DATA(buf, port); val = le32toh(*addr); if (pio->data) val |= (0x01 << pio->pin); else val &= ~(0x01 << pio->pin); *addr = htole32(val); } return PIO_SUCCESS; }
static int pio_get(const char* buf, mmap_gpio* pio) { uint32_t port = pio->port - 'A'; if (port > PIO_NR_PORTS) { return -1; } uint32_t val; uint32_t port_num_func, port_num_pull; uint32_t offset_func, offset_pull; port_num_func = pio->pin >> 3; offset_func = ((pio->pin & 0x07) << 2); port_num_pull = pio->pin >> 4; offset_pull = ((pio->pin & 0x0f) << 1); /* func */ pio->cfg_address = PIO_REG_CFG(buf, port, port_num_func); val = le32toh(*(pio->cfg_address)); pio->cfg = (val>>offset_func) & 0x07; /* pull */ pio->pull_address = PIO_REG_PULL(buf, port, port_num_pull); val = le32toh(*(pio->pull_address)); pio->pull = (val>>offset_pull) & 0x03; /* dlevel */ pio->dlevel_address = PIO_REG_DLEVEL(buf, port, port_num_pull); val = le32toh(*(pio->dlevel_address)); pio->drv_level = (val>>offset_pull) & 0x03; /* i/o data */ if (pio->cfg > 1) pio->data = -1; else { pio->data_address = PIO_REG_DATA(buf, port); val = le32toh(*(pio->data_address)); pio->data = (val >> pio->pin) & 0x01; } return PIO_SUCCESS; }