Implement flashing over I2C and user/stock firmware support

I2C register layout changed a bit to make various operations easier to implement in FW and for the user. Flashing/debugging tools now share more code. Firmware is now more configurable (it's now possible to compile-out various features). Self-testing for column-shorts is implemented. Firmware is optimized for low power consumption.
2024-11-08 13:38:24 +01:00 · 2021-06-27 18:45:36 +02:00 · 2021-06-27 18:45:36 +02:00 · 6a5fe581b9
commit 6a5fe581b9
parent 201b84d70e
32 changed files with 3325 additions and 1527 deletions
--- a/.gitignore
+++ b/.gitignore
@ -0,0 +1 @@
+/firmware/build/
--- a/45
+++ b/45
@ -0,0 +1,45 @@
+VERSION := $(shell git describe) $(shell git log -1 --format=%cd --date=iso)
+
+OUT ?= build/
+CFLAGS ?= -O2 -g0
+CFLAGS += -DVERSION="\"$(VERSION)\"" -I. -I$(OUT)
+
+all: $(OUT)ppkb-i2c-inputd $(OUT)ppkb-usb-flasher $(OUT)ppkb-usb-debugger $(OUT)fw-stock.bin $(OUT)ppkb-i2c-debugger $(OUT)ppkb-i2c-charger-ctl $(OUT)ppkb-i2c-flasher $(OUT)ppkb-i2c-selftest
+
+$(OUT)ppkb-usb-flasher: usb-flasher.c common.c
+	@mkdir -p $(OUT)
+	$(CC) $(CFLAGS) -o $@ $<
+
+$(OUT)ppkb-usb-debugger: usb-debugger.c common.c
+	@mkdir -p $(OUT)
+	$(CC) $(CFLAGS) -o $@ $<
+
+$(OUT)kmap.h: keymaps/physical-map.txt keymaps/factory-keymap.txt
+	@mkdir -p $(OUT)
+	php keymaps/map-to-c.php $^ > $@
+
+$(OUT)ppkb-i2c-inputd: i2c-inputd.c $(OUT)kmap.h common.c
+	@mkdir -p $(OUT)
+	$(CC) $(CFLAGS) -o $@ $<
+
+$(OUT)ppkb-i2c-debugger: i2c-debugger.c common.c
+	@mkdir -p $(OUT)
+	$(CC) $(CFLAGS) -o $@ $<
+
+$(OUT)ppkb-i2c-charger-ctl: i2c-charger-ctl.c common.c
+	@mkdir -p $(OUT)
+	$(CC) $(CFLAGS) -o $@ $<
+
+$(OUT)ppkb-i2c-flasher: i2c-flasher.c common.c
+	@mkdir -p $(OUT)
+	$(CC) $(CFLAGS) -o $@ $<
+
+$(OUT)ppkb-i2c-selftest: i2c-selftest.c common.c
+	@mkdir -p $(OUT)
+	$(CC) $(CFLAGS) -o $@ $<
+
+$(OUT)fw-stock.bin $(OUT)fw-user.bin: firmware/em85f684a.h firmware/main.c firmware/build.sh firmware/bootloader.bin
+	@mkdir -p $(OUT)
+	cd firmware && ./build.sh
+	cp -f firmware/build/fw-stock.bin $(OUT)fw-stock.bin
+	cp -f firmware/build/fw-user.bin $(OUT)fw-user.bin
--- a/27
+++ b/27
@ -1,4 +1,29 @@
-This is work in progress free software firmware for pinephone keyboard.
+FOSS firmware for pinephone keyboard
+====================================
+
+Features:
+
+- Dual firmware architecture: stock firmware + optional user firmware.
+- Stock firmware implements the full functionality of the keyboard.
+  - Stock firmware is layout independent, it reports the raw status
+    of the whole keyboard matrix.
+  - Key maps and combinations can be arbitrarily changed in the keyboard
+    driver without re-flashing the firmware.
+  - Stock firmware should be sufficient for most users who will not want
+    to do HW modifications to their keyboard to add more peripherals to
+    the keyboard MCU.
+  - Power efficient implementation using power-down feature of the MCU
+    to save power as much as possible. (currently: 9 mW when idle, 20mW
+    in active scanning mode - with at least one key pressed)
+- Stock firmware is flashed in factory and allows flashing user firmware
+  from the pinephone itself over I2C interface.
+- User firmware can be used either for updates or for customizations
+  (SW support for HW modifications of the keyboard)
+- USB stack and tools for stock firmware flashing using ELAN's original
+  bootloader to ease development of the stock firmware.
+- Self-testing features to quickly test for issues with the keyboard matrix.
+- Fully based on FOSS software, with no dependencies. You only need
+  sdcc 4.1+ to build the firmware.

 See demo video https://megous.com/dl/tmp/kb.mp4 and some technical overview https://xnux.eu/log/

--- a/README.flashing
+++ b/README.flashing
@ -1,13 +1,19 @@
-Flashing tool from usb-flasher/ directory can be used to flash the
-firmware over USB.
+Flashing over USB
+=================
+
+This method allows updating the stock firmware on the keyboard.
+
+Flashing tool ppkb-usb-flasher can be used to flash the firmware over USB.
+You'll have to solder USB cable to the keyboard controller board to be able
+to user this method.

 For example to build and flash your own firmware you could run these
 commands:

   # save the original firmware in case you want to restore it

-   ./usb-flasher/ppkb-flasher info > saved-info.txt
-   ./usb-flasher/ppkb-flasher --rom-out saved-rom.bin read
+   ./ppkb-usb-flasher info > saved-info.txt
+   ./ppkb-usb-flasher --rom-out saved-rom.bin read

   # build the new firmware (you may need sdcc 4.1, older versions may
   # miscompile the firmware)
@ -16,20 +22,21 @@ commands:

   # flash the new firmware

-   ./usb-flasher/ppkb-flasher --rom-in firmware/build/fw.bin write reset
+   ./ppkb-usb-flasher --rom-in firmware/build/fw-stock.bin write reset
+


 Full help output
 ----------------

-Usage: ppkb-flasher [--rom-in <path>] [--rom-out <path>] [--verbose]
-                    [--help] [<read|write|info|reset>...]
+Usage: ppkb-usb-flasher [--rom-in <path>] [--rom-out <path>] [--verbose]
+                        [--help] [<read|write|info|reset>...]

 Options:
  -i, --rom-in <path>   Specify path to binary file you want to flash.
  -o, --rom-out <path>  Specify path where you want to store the contents
                        of code ROM read from the device.
-  -s, --rom-size <size> Specify how many bytes of code rom to flash
+  -s, --size <size>     Specify how many bytes of code rom to flash
                        starting from offset 0x2000 in the rom file.
  -v, --verbose         Show details of what's going on.
  -h, --help            This help.
@ -43,7 +50,68 @@ Commands:
 Format of the ROM files is a flat binary. Only the part of it starting
 from 0x2000 will be flashed. Use -s to specify how many bytes to write.

-Pinephone keyboard flashing tool 1.0
-Written by Ondrej Jirman <megi@xff.cz>, 2021
-Licensed under GPLv3, see https://xff.cz/git/pinephone-keyboard/ for
-more information.
+
+Flashing over I2C
+=================
+
+This method users stock stock firmware's flashing interface to allow writing
+user's own firmware to the keyboard, while keeping the stock firmware intact.
+
+Stock firmware will remain present and available for future updates of the
+users fiwmare.
+
+You can use this method without having to disassemble the keyboard, or
+solder anything.
+
+You'll need to build the user firmware in a special way, so that it's placed
+at address 0x4000 in code ROM. Interrupt vectors are forwarded with 0x4000
+offset.
+
+
+For example to build and flash your own firmware you could run these
+commands:
+
+   # build the new firmware (you may need sdcc 4.1, older versions may
+   # miscompile the firmware)
+
+   (cd firmware && ./build.sh)
+
+   # flash the new firmware
+
+   ./ppkb-i2c-flasher --rom-in firmware/build/fw-user.bin write reset
+
+Depending on the user firmware, you may need to change stock firmware entry
+method using -e option.
+
+
+Full help output
+----------------
+
+Usage: ppkb-i2c-flasher [--rom-in <path>] [--rom-out <path>] [--verbose]
+                        [--help] [<read|write|erase|info|reset>...]
+
+Options:
+  -i, --rom-in <path>   Specify path to binary file you want to flash.
+  -o, --rom-out <path>  Specify path where you want to store the contents
+                        of code ROM read from the device.
+  -s, --size <size>     Specify how many bytes of code rom to flash
+                        starting from offset 0x4000 in the rom file.
+  -e, --entry <manual|i2c|none>
+                        Specify how to enter the stock firmware:
+                        - manual: Ask the user to power-cycle the keyboard
+                        - i2c: Send I2C command to make supporting user
+                        - none: Assume stock firmware is already running
+  -v, --verbose         Show details of what's going on.
+  -h, --help            This help.
+
+Commands:
+  info      Display information about the current firmware.
+  read      Read ROM from the device to --rom-out file.
+  write     Flash ROM file to the device from --rom-in.
+  erase     Erase the user firmware.
+  reset     Perform software reset of the MCU.
+
+Format of the ROM files is a flat binary. Only the part of it starting
+from 0x4000 will be flashed. Use -s to specify how many bytes to write.
+The stock firmware between 0x2000 and 0x4000 will be preserved.
+
--- a/README.i2c-intf
+++ b/README.i2c-intf
@ -20,6 +20,13 @@ Device address is 0x15.
 Registers
 ---------

+General ranges:
+  0x00 - 0x1f - Read-only status range
+  0x20 - 0x2f - Writable keyboard control range
+  0x70 - 0xf4 - Flashing interface
+  0xff        - Debug log
+
+
 0x00: Device ID1 (0x4b)
 0x01: Device ID2 (0x42)
 0x02: Firmware revision
@ -27,8 +34,15 @@ Registers
   bit 0: USB debugger
   bit 1: Flashing mode
   bit 2: Self-test features
+   bit 3: Stock firmware flag (only stock firmware should have this set)
   
-0x04: System configuration
+0x06: bits 3-0: number of rows, bits 7-4 number of cols
+0x07: CRC8 of keyboard data from 0x08-0x13
+0x08: Keyboard data for column 1
+...
+0x13: Keyboard data for column 12 (up to number of cols, in this case 12)
+
+0x20: System configuration
   bit 0: disable KB scanning (1: scanning disabled, 0: scanning enabled)
   bit 1: poll mode
          1: don't rely on row change detection, poll the matrix periodically
@ -39,7 +53,7 @@ Registers
          1: enabled
          0: disabled

-0x06: System command
+0x21: System command
      Writing values into this register causes the firmware to perform
      certain one-shot actions:

@ -49,23 +63,34 @@ Registers
                   (results for both tests will be stored in test-result
                    registers)

-0x10: Keyboard data for column 1
-...
-0x1b: Keyboard data for column 12
-0x1c: CRC8 of keyboard data from 0x10-0x0b
+      The register is set to 0x00 or 0xff after the operation completes.
+      0xff means error.

-0x20: Writing value 0x53 ('S') to this register stops the main app from
+0x22: Writing value 0x53 ('S') to this register stops the main app from
      jumping to the user app.

-0x50: Self-test results (32 bytes)
-...
-0x6f:
-      

-0x70: Flashing mode unlock key
-      (writing 0x46 to this register unlocks the flashing mode.)
+0x70: 128B block of EEPROM data (either read from code memory or to be
+...   written)
+0xef

-0x71: Flashing control
+0xf0: target address low byte
+0xf1: target address high byte
+
+0xf2: CRC8 calculated for the 128B block of data from 0x70-0xef
+      - this must be written by the user when preparing data for write
+        operation, MCU checks the CRC8 of the data and compares it against
+        the value in this register before starting the execution of
+        0x57 command
+      - this is updated by the MCU after reading the data from flash memory
+        so the user can check the checksum against the data read from
+        0x70-0xef
+
+0xf3: Flashing mode unlock key
+      - writing 0x46 to this register unlocks the flashing mode.
+      - this register is cleared after command completion
+
+0xf4: Flashing control
      Writing various commands to this register makes the MCU execute them,
      if the MCU is not executing the previous command. Available commands:
      
@ -79,36 +104,30 @@ Registers
      This register will ignore further commands as long as the last operation
      is still in progress. This register will contain the code of the
      currently executed operation, and will be cleared after the operation
-      finishes. Completion is also signalled by pulsing the INT pin shortly.
+      finishes.
      
      If the operation fails, this register will contain value 0xff. If it
      succeeds it will contain value 0x00.
-      
-0x7d: target address low byte
-0x7e: target address high byte
-
-0x7f: CRC8 calculated for the 128B block of data from 0x80-0xff
-      - this must be written by the user when preparing data for write
-        operation, MCU checks the CRC8 of the data and compares it against
-        the value in this register before starting the execution of
-        0x57 command
-
-      - this is updated by the MCU after reading the data from flash memory
-
-0x80: 128B block of EEPROM data (either read from code memory or to be
-...   written)
-0xff

+0xff: Debug log
+      - reading from this register returns next character of the debug log
+        or 0
+      - register address is not auto-advanced for the 0xff address, so you
+        can read any number of bytes from 0xff to get the debug log text
+      - debug log stores results of self-tests

 Usage
 -----

-User can modify register 0x03 to choose how the firmware should operate.
+User can modify register 0x20 to choose how the firmware should operate.
 The settings are not persistent across resets.

 To read the keyboard matrix status, the user can perform a 13B read transaction
-from address 0x10 and calculate CRC8 on the first 12 bytes and compare it with
-the 13th byte.
+from address 0x07. Data for each column start at 0x08, 0x07 contains CRC8 checksum
+of the 12 bytes starting from 0x08.
+
+You can verify the data by calculating CRC8 on the last 12 bytes and compare it
+with the 1st byte.

 Bit 0 corresponds to row 1, bit 5 to row 6, bits 6 and 7 are always 0.

@ -122,23 +141,43 @@ The firmware is split into 3 parts:
 0x2000 - 0x4000: Stock FOSS firmware (flashable from stock bootloader)
 0x4000 - 0x8000: User app (optional, flashable over I2C from stock FOSS firmware)

-When the stock FOSS firmware runs after MCU powerup or reset, it will wait for 200ms
-and listen on I2C. If 0x53 is not written to register 0x20 during that time and
-the user's app is flashed and commited, it will redirect interrupt vectors to
-0x4000+offset and jump to 0x4000, which will start executing user's app.
+It is necessary to execute stock firmware to flash the user firmware. Stock
+firmware will normally re-direct execution to user firmware if user firmware
+is flashed.
+
+After MCU powerup or reset, stock firmware will listen for 1s on I2C interface,
+before passing control to the user firmware. If 0x53 is not written to register
+0x22 during that time, this will prevent execution of the user's firmware.
+
+If the user firmware execution is not prevented in this way, stock firmware
+will redirect interrupt vectors from 0x0000+offset to 0x4000+offset and jump
+to 0x4000, which will start execution of user's firmware. User firmware must
+not change last byte of IRAM, because it is used by the stock firmware's
+interrupt redirection mechanism.
+
+User's firmware should always implement some way to reset the MCU via I2C
+interface. This will make development and flashing easier. Failing that,
+I2C flashing tool also allows for manual entry to stock firmware which
+is done by holding the power key for > 12s to force power cycle on the MCU.

-User app should always return value 0x00 when reading register 0x20, to make it
-easy to distinguish that the "stay in stock app" command succeeded.

 Flashing steps:

-1) Unlock by writing 0x46 to register 0x70
-2) Write address to 0x7d/0x7e
-3) Write data to 0x80-0xff and CRC8 to 0x7f
-4) Write command 0x57 to 0x71
-5) Poll 0x71 for result (either 0x00 or 0xff)
-   ... repeat 2-5 for all memory locations to be flashed
-6) Write command 0x43 to reg 0x71
-7) Wait for success
+1) Write 128 B of data to 0x70-0xef and data CRC8 to 0xf2
+2) Write address to 0xf0 (low) and 0xf1 (high byte)
+   (only range from 0x4000 to 0x7fff) is writeable
+3) Unlock by writing 0x46 to register 0xf3
+4) Write command 0x57 to 0xf4
+5) Poll 0xf4 for result (either 0x00 or 0xff)
+   ... repeat 1-5 for all memory locations to be flashed
+6) Write command 0x43 to reg 0xf4
+7) Poll 0xf4 for result (either 0x00 or 0xff)
+8) Reset the MCU by writing command 0x52 to reg 0x21

-Reset the MCU.
+
+Self-tests
+----------
+
+1) Write command 'r' or 'c' to reg 0x21
+2) Read repeatedly from register 0xff to get the self-test results
+   in human readable text form
--- a/41
+++ b/41
@ -1,29 +1,34 @@
 Firmware
 --------

- mask R5:C4 and R4:C2 because these are always on on my prototype
-  and prevent development of idle functionality
-  (Z key is stuck)
+- implement I2C configuration options
+  - disable keyboard matrix scanning

-   C1  2  3  4  5  6  7  8  9  10 11 12 
-R1  .  .  .  .  .  .  .  .  .  .  .  .
-R2  .  .  .  .  .  .  .  .  .  .  .  .
-R3  .  .  .  .  .  .  .  .  .  .  .  .
-R4  .  X  .  .  .  .  .  .  .  .  .  .
-R5  .  .  .  X  .  .  .  .  .  .  .  .
-R6  .  .  .  .  .  .  .  .  .  .  .  .
+- only enable USB in stock FW upon request over I2C (idling USB stack takes
+  about 0.6mA on top of 1.8mA baseline)

- sleep when no key is pressed and wakeup using pin change interrupt
-  on P6 port
+- first I2C TX after switch to USER firmware after flasging over I2C fails,
+  subsequent ones work
+  - also power consumption indicates power consumption of the sotck firmware
+    so maybe we just sleep in stock instead of switching after flashing?

- wakeup on I2C activity only happens on device address match, so
-  we can only sleep again after the entire transfer completes (how
-  to detect that though?)
+- power measurement / optimization
+  - currently with no USB stack and in constant PD mode: 1.8mA
+  - theoretical lower limit 0.5mA (no idea how they achieve it)

-USB Flashing Tool
-----------------

- investigate reason for URB errors/empty (in command status INT transfer)
+Charger behavior
+----------------
+
+- does the charger turn off VOUT after some period of low load?
+- does it enable it again to see if there's a need for it?
+- this auto-off behavior will kill the power to the keyboard
+  controller, so we need a way to detect it and power the controller
+  from the pinephone side by enabling VBUS.
+  - tricky!
+  - or make the user keep pressing the kb power button to use the
+    keyboard for a while, like in a train conductor has to in a train :)
+    - not good!


 Userspace input device daemon
--- a/builds/fw.bin
+++ b/builds/fw.bin
--- a/charger/build.sh
+++ b/charger/build.sh
@ -1,3 +0,0 @@
-#!/bin/sh
-
-gcc -o kbpower main.c || exit 1
--- a/charger/main.c
+++ b/charger/main.c
@ -1,385 +0,0 @@
-/*
- * Pinephone keyboard power management daemon/tool.
- *
- * Copyright (C) 2021  Ondřej Jirman <megi@xff.cz>
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-// {{{ includes
-
-#include <assert.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <inttypes.h>
-#include <stdarg.h>
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/stat.h>
-#include <time.h>
-#include <sys/ioctl.h>
-#include <unistd.h>
-#include <sys/types.h>
-#include <dirent.h>
-#include <poll.h>
-
-#include <linux/i2c.h>
-#include <linux/i2c-dev.h>
-#include <linux/gpio.h>
-//#include <i2c/smbus.h>
-
-#define DEBUG 1
-
-#if DEBUG
-#define debug(args...) printf(args)
-#else
-#define debug(args...)
-#endif
-
-// }}}
-// {{{ utils
-
-static void syscall_error(int is_err, const char* fmt, ...)
-{
-	va_list ap;
-
-	if (!is_err)
-		return;
-
-	fprintf(stderr, "ERROR: ");
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, ": %s\n", strerror(errno));
-
-	exit(1);
-}
-
-static void error(const char* fmt, ...)
-{
-	va_list ap;
-
-	fprintf(stderr, "ERROR: ");
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, "\n");
-
-	exit(1);
-}
-
-bool read_file(const char* path, char* buf, size_t size)
-{
-	int fd;
-	ssize_t ret;
-
-	fd = open(path, O_RDONLY);
-	if (fd < 0)
-		return false;
-
-	ret = read(fd, buf, size);
-	close(fd);
-	if (ret < 0)
-		return false;
-
-	if (ret < size) {
-		buf[ret] = 0;
-		return true;
-	} else {
-		buf[size - 1] = 0;
-		return false;
-	}
-}
-
-#define BIT(n) (1u << (n))
-
-#define KB_ADDR 0x15
-#define POWER_ADDR 0x75
-
-static int pogo_i2c_open(void)
-{
-	int ret;
-	char path[256], buf[1024];
-	int fd = -1;
-
-	for (int i = 0; i < 8; i++) {
-		snprintf(path, sizeof path, "/sys/class/i2c-adapter/i2c-%d/uevent", i);
-		if (!read_file(path, buf, sizeof buf))
-			continue;
-		
-		if (!strstr(buf, "OF_FULLNAME=/soc/i2c@1c2b400"))
-			continue;
-		
-		snprintf(path, sizeof path, "/dev/i2c-%d", i);
-		
-		int fd = open(path, O_RDWR);
-		syscall_error(fd < 0, "open(%s) failed");
-
-		//ret = ioctl(fd, I2C_SLAVE, addr);
-		//syscall_error(ret < 0, "I2C_SLAVE failed");
-
-		return fd;		
-	}
-	
-	error("Can't find POGO I2C adapter");
-	return -1;
-}
-
-uint8_t read_power(int fd, uint8_t reg)
-{
-	int ret;
-	uint8_t val;
-	struct i2c_msg msgs[] = {
-		{ POWER_ADDR, 0, 1, &reg }, // address
-		{ POWER_ADDR, I2C_M_RD, 1, &val },
-	};
-
-	struct i2c_rdwr_ioctl_data msg = {
-		.msgs = msgs,
-		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
-	};
-
-	ret = ioctl(fd, I2C_RDWR, &msg);
-	syscall_error(ret < 0, "I2C_RDWR failed");
-	
-	return val;
-}
-
-void write_power(int fd, uint8_t reg, uint8_t val)
-{
-	int ret;
-	uint8_t buf[] = {reg, val};
-
-	struct i2c_msg msgs[] = {
-		{ POWER_ADDR, 0, 2, buf },
-	};
-
-//	printf("wr 0x%02hhx: %02hhx\n", reg, val);
-
-	struct i2c_rdwr_ioctl_data msg = {
-		.msgs = msgs,
-		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
-	};
-
-	ret = ioctl(fd, I2C_RDWR, &msg);
-	syscall_error(ret < 0, "I2C_RDWR failed");
-}
-
-void update_power(int fd, uint8_t reg, uint8_t mask, uint8_t val)
-{
-	uint8_t tmp;
-	
-	tmp = read_power(fd, reg);
-	tmp &= ~mask;
-	tmp |= val & mask;
-	write_power(fd, reg, tmp);
-}
-
-// bits 4-0 are mapped to gpio4 - gpio0
-//#define MFP_CTL0 0x51
-//#define MFP_CTL1 0x52
-//#define GPIO_INEN 0x53
-//#define GPIO_OUTEN 0x54
-//#define GPIO_DATA 0x55
-
-#define BATVADC_DAT_L 0xa2
-#define BATVADC_DAT_H 0xa3
-#define BATIADC_DAT_L 0xa4
-#define BATIADC_DAT_H 0xa5
-#define BATOCV_DAT_L 0xa8
-#define BATOCV_DAT_H 0xa9
-
-// in mV
-unsigned get_bat_voltage(int fd)
-{
-	unsigned l = read_power(fd, BATVADC_DAT_L);
-	unsigned h = read_power(fd, BATVADC_DAT_H);
-	
-	if (h & 0x20)
-		return 2600 - ((~l & 0xff) + ((~h & 0x1f) << 8) + 1) * 1000 / 3724;
-
-	return 2600 + (l + (h << 8)) * 1000 / 3724;
-}
-
-int get_bat_current(int fd)
-{
-	unsigned l = read_power(fd, BATIADC_DAT_L);
-	unsigned h = read_power(fd, BATIADC_DAT_H);
-	
-	if (h & 0x20)
-		return - (int)((~l & 0xff) + ((~h & 0x1f) << 8) + 1) * 1000 / 1341;
-
-	return (l + (h << 8)) * 1000 / 1341;
-}
-
-unsigned get_bat_oc_voltage(int fd)
-{
-	unsigned l = read_power(fd, BATOCV_DAT_L);
-	unsigned h = read_power(fd, BATOCV_DAT_H);
-	
-	if (h & 0x20)
-		return 2600 - ((~l & 0xff) + ((~h & 0x1f) << 8) + 1) * 1000 / 3724;
-
-	return 2600 + (l + (h << 8)) * 1000 / 3724;
-}
-
-enum {
-	POWER_CHARGER_ENABLED,
-	POWER_VOUT_ENABLED,
-	POWER_VOUT_AUTO,
-};
-
-#define SYS_CTL0 0x01
-#define SYS_CTL1 0x02
-#define SYS_CTL2 0x0c
-#define SYS_CTL3 0x03
-#define SYS_CTL4 0x04
-#define SYS_CTL5 0x07
-
-#define Charger_CTL1 0x22
-#define Charger_CTL2 0x24
-#define CHG_DIG_CTL4 0x25
-
-void power_setup(int fd, unsigned flags)
-{
-	update_power(fd, SYS_CTL1, 0x03, 0x00); // disable automatic control based on load detection
-	update_power(fd, SYS_CTL0, 0x1e, BIT(1) | BIT(2)); // 2=boost 1=charger enable
-	update_power(fd, SYS_CTL3, BIT(5), 0); // disable "2x key press = shutdown" function
-	update_power(fd, SYS_CTL4, BIT(5), 0); // disable "VIN pull out -> VOUT auto-enable" function
-	
-	update_power(fd, CHG_DIG_CTL4, 0x1f, 15); // set charging current (in 100mA steps)
-}
-
-#define READ0 0x71
-#define READ1 0x72
-#define READ2 0x77
-
-const char* get_chg_status_text(uint8_t s)
-{
-	switch (s) {
-	case 0: return "Idle";
-	case 1: return "Trickle charge";
-	case 2: return "Constant current phase";
-	case 3: return "Constant voltage phase";
-	case 4: return "Constant voltage stop";
-	case 5: return "Full";
-	case 6: return "Timeout";
-	default: return "Unknown";
-	}
-}
-
-void power_status(int fd)
-{
-	uint8_t r0 = read_power(fd, READ0);
-	uint8_t r1 = read_power(fd, READ1);
-	uint8_t r2 = read_power(fd, READ2);
-
-	printf("Charger: %s (%s%s%s%s%s%s%s)\n", get_chg_status_text((r0 >> 5) & 0x7),
-		r0 & BIT(4) ? " chg_op" : "",
-		r0 & BIT(3) ? " chg_end" : "",
-		r0 & BIT(2) ? " cv_timeout" : "",
-		r0 & BIT(1) ? " chg_timeout" : "",
-		r0 & BIT(0) ? " trickle_timeout" : "",
-		r1 & BIT(6) ? " VIN overvoltage" : "",
-		r1 & BIT(5) ? " <= 75mA load" : ""
-	);
-
-	printf("Button: %02hhx (%s%s%s%s)\n", r2,
-		r2 & BIT(3) ? " btn_press" : " btn_not_press",
-		r2 & BIT(2) ? " double_press" : "",
-		r2 & BIT(1) ? " long_press" : "",
-		r2 & BIT(0) ? " short_press" : ""
-	);
-
-	printf("0x70: %02hhx\n", read_power(fd, 0x70));
-
-	update_power(fd, READ2, 0x7, 0x7);
-}
-
-/*
- Independent control of
-  - Boost (5V VOUT to power Pinephone)
-  - Battery Charger
-
- Optional automatic power on when load is inserted
- Optional auto enable of VOUT when disconnecting VIN (reg 0x04)
-
- Optiobal automatic shutdown when VOUT has light load (customizable via reg
-  0x0c, min. is 100mA, shutdown lastsa 8-64s (see reg 0x04))
-  
- Charger_CTL1 0x22
-  - Control of charging current based on VOUT undervoltage (it tries
-    to keep VOUT in a certain range by reducing load on VIN by
-    decreasing charging current?)
-
- Battery type selection 4.2/4.3/4.35V
-  - + extra margin 0-42mV during constant voltage phase?
-  - External (via VSET pin) or internal setting (via reg 0x24)
-
- Charging current selection (100mA - 2.3A ?)
-
- Charging status register
-  - charging state - idle, trickle, constant voltage/current phase, full,
-    timeout
-  - LED heavey load indication
-  - VIN overvoltage indication (> 5.6V)
-
- Button press status
-  - current state: UP/DOWN
-  - long press
-  - short press
-
-GPIO:
-
- KEY input
-  - Long press button time selection 1-4s
-  - Enable/disable 2x short press shutdown function
- L3/L4 function selection:
-  - GPIO0/1
-  - normal function
- LIGHT pin function selection:
-  - GPIO2
-  - VREF
-  - WLED
- VSET
-  - VSET (normal function to select battery voltage via PIN setting)
-  - GPIO4
- RSET
-  - GPIO3
-  - battery internal resistance selection via resistor on the RSET pin
-
- separate input/output enable register for all 5 GPIOs
- GPIO data register to read/write values to pins
-
-ADC:
-
- 14 bit two register VBAT, IBAT, VBAT_OCV readings
-*/
-
-int main(int ac, char* av[])
-{
-	int fd, ret;
-
-	fd = pogo_i2c_open();
-
-	printf("V=%u mV (OCV %u mV)   I=%d mA\n", get_bat_voltage(fd), get_bat_oc_voltage(fd), get_bat_current(fd));
-
-//	uint8_t v = read_power(fd, 2);
-//	printf("%02hhx\n", v);
-	
-	return 0;
-}
--- a/common.c
+++ b/common.c
@ -0,0 +1,387 @@
+#include <assert.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <inttypes.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <poll.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <dirent.h>
+#include <getopt.h>
+
+#include <linux/usbdevice_fs.h>
+#include <linux/i2c.h>
+#include <linux/i2c-dev.h>
+#include <linux/gpio.h>
+
+#define BIT(n) (1u << (n))
+
+#define KB_ADDR 0x15
+#define POWER_ADDR 0x75
+
+static bool verbose;
+#define debug(args...) { if (verbose) printf(args); }
+
+static void syscall_error(int is_err, const char* fmt, ...)
+{
+	va_list ap;
+
+	if (!is_err)
+		return;
+
+	fprintf(stderr, "ERROR: ");
+	va_start(ap, fmt);
+	vfprintf(stderr, fmt, ap);
+	va_end(ap);
+	fprintf(stderr, ": %s\n", strerror(errno));
+
+	exit(1);
+}
+
+static void error(const char* fmt, ...)
+{
+	va_list ap;
+
+	fprintf(stderr, "ERROR: ");
+	va_start(ap, fmt);
+	vfprintf(stderr, fmt, ap);
+	va_end(ap);
+	fprintf(stderr, "\n");
+
+	exit(1);
+}
+
+static bool read_file(const char* path, char* buf, size_t size)
+{
+	int fd;
+	ssize_t ret;
+
+	fd = open(path, O_RDONLY);
+	if (fd < 0)
+		return false;
+
+	ret = read(fd, buf, size);
+	close(fd);
+	if (ret < 0)
+		return false;
+
+	if (ret < size) {
+		buf[ret] = 0;
+		return true;
+	} else {
+		buf[size - 1] = 0;
+		return false;
+	}
+}
+
+static int open_usb_dev(uint16_t vid, uint16_t pid)
+{
+	char path[256], buf[256];
+	struct dirent *e;
+	unsigned e_vid, e_pid, bus, dev;
+	int fd = -1, ret;
+	DIR* d;
+
+        d = opendir("/sys/bus/usb/devices");
+	syscall_error(d == NULL, "opendir(/sys/bus/usb/devices) failed");
+
+	while (true) {
+		errno = 0;
+		e = readdir(d);
+		syscall_error(e == NULL && errno, "readdir(/sys/bus/usb/devices) failed");
+		if (!e)
+			break;
+
+                if (!strcmp(e->d_name, ".") || !strcmp(e->d_name, ".."))
+			continue;
+
+		snprintf(path, sizeof path,
+			 "/sys/bus/usb/devices/%s/idVendor", e->d_name);
+		if (!read_file(path, buf, sizeof buf))
+			continue;
+
+		ret = sscanf(buf, "%x", &e_vid);
+		if (ret != 1)
+			error("Failed to parse %s", path);
+
+		snprintf(path, sizeof path,
+			 "/sys/bus/usb/devices/%s/idProduct", e->d_name);
+		if (!read_file(path, buf, sizeof buf))
+			continue;
+
+		ret = sscanf(buf, "%x", &e_pid);
+		if (ret != 1)
+			error("Failed to parse %s", path);
+
+		if (e_vid == vid && e_pid == pid) {
+			snprintf(path, sizeof path,
+				 "/sys/bus/usb/devices/%s/busnum", e->d_name);
+			if (!read_file(path, buf, sizeof buf))
+				error("Failed to read %s", path);
+
+			ret = sscanf(buf, "%u", &bus);
+			if (ret != 1)
+				error("Failed to parse %s", path);
+
+			snprintf(path, sizeof path,
+				 "/sys/bus/usb/devices/%s/devnum", e->d_name);
+			if (!read_file(path, buf, sizeof buf))
+				error("Failed to read %s", path);
+
+			ret = sscanf(buf, "%u", &dev);
+			if (ret != 1)
+				error("Failed to parse %s", path);
+
+			snprintf(path, sizeof path,
+				 "/dev/bus/usb/%03u/%03u", bus, dev);
+
+			debug("Found %04x:%04x at %s\n", e_vid, e_pid, path);
+
+			fd = open(path, O_RDWR);
+			syscall_error(fd < 0, "open(%s) failed", path);
+			break;
+		}
+	}
+
+	errno = ENOENT;
+	closedir(d);
+	return fd;
+}
+
+static int handle_urb(int usb_fd, struct usbdevfs_urb* urb, int timeout)
+{
+	int ret;
+	struct usbdevfs_urb* reaped_urb;
+	int retries = 0;
+
+retry:
+        ret = ioctl(usb_fd, USBDEVFS_SUBMITURB, urb);
+        if (ret < 0)
+        	return ret;
+
+	struct pollfd fd = {
+		.fd = usb_fd,
+		.events = POLLOUT,
+	};
+
+	ret = poll(&fd, 1, timeout);
+	if (ret <= 0) {
+		if (ret == 0)
+			errno = ETIMEDOUT;
+
+		int save_errno = errno;
+
+		// on timeout or other poll error, we need to discard and reap the submitted URB
+		ret = ioctl(usb_fd, USBDEVFS_DISCARDURB, urb);
+
+		// even if discard fails, URB may still be reapable, we need to try reaping anyway
+		ret = ioctl(usb_fd, USBDEVFS_REAPURBNDELAY, &reaped_urb);
+
+		// reap must immediately succeed, otherwise this is fatal
+		syscall_error(ret < 0, "USBDEVFS_REAPURBNDELAY failed");
+
+		errno = save_errno;
+		return -1;
+	}
+
+	// hopefully POLLERR means we get some error immediately on reap
+
+	ret = ioctl(usb_fd, USBDEVFS_REAPURB, &reaped_urb);
+        if (ret < 0)
+        	return ret;
+
+	// EPROTO errors are recoverable
+	if (urb->status == -71 && retries < 3) {
+		retries++;
+		goto retry;
+	}
+
+	if (urb->status != 0) {
+		errno = -urb->status;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int pogo_i2c_open(void)
+{
+	int ret;
+	char path[256], buf[1024];
+	int fd = -1;
+
+	for (int i = 0; i < 8; i++) {
+		snprintf(path, sizeof path, "/sys/class/i2c-adapter/i2c-%d/uevent", i);
+		if (!read_file(path, buf, sizeof buf))
+			continue;
+		
+		if (!strstr(buf, "OF_FULLNAME=/soc/i2c@1c2b400"))
+			continue;
+		
+		snprintf(path, sizeof path, "/dev/i2c-%d", i);
+		
+		int fd = open(path, O_RDWR);
+		syscall_error(fd < 0, "open(%s) failed");
+
+		return fd;		
+	}
+	
+	error("Can't find POGO I2C adapter");
+	return -1;
+}
+
+ssize_t xwrite(int fd, uint8_t* buf, size_t len)
+{
+	size_t off = 0;
+	
+	while (off < len) {
+		ssize_t ret = write(fd, buf + off, len - off);
+		if (ret < 0)
+			return ret;
+		
+		off += ret;
+	}
+	
+	return off;
+}
+
+static const uint8_t crc8_0x7_table[] = {
+	0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15,
+	0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
+	0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
+	0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
+	0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5,
+	0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
+	0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85,
+	0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
+	0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
+	0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
+	0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2,
+	0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
+	0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32,
+	0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
+	0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
+	0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
+	0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c,
+	0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
+	0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec,
+	0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
+	0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
+	0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
+	0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c,
+	0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
+	0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b,
+	0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
+	0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
+	0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
+	0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb,
+	0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
+	0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb,
+	0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3
+};
+
+static uint8_t crc8(const uint8_t *pdata, size_t nbytes)
+{
+	unsigned int idx;
+	uint8_t crc = 0;
+
+	while (nbytes--) {
+		idx = (crc ^ *pdata);
+		crc = (crc8_0x7_table[idx]) & 0xff;
+		pdata++;
+	}
+
+	return crc;
+}
+
+uint64_t time_abs(void)
+{
+	struct timespec tmp;
+	int ret;
+
+	ret = clock_gettime(CLOCK_MONOTONIC, &tmp);
+	if (ret < 0)
+		return 0;
+
+	return tmp.tv_sec * 1000000000ull + tmp.tv_nsec;
+}
+
+static int gpiochip_open(const char* match)
+{
+	int ret;
+	char path[256], buf[1024];
+	int fd = -1;
+
+	for (int i = 0; i < 8; i++) {
+		snprintf(path, sizeof path, "/sys/bus/gpio/devices/gpiochip%d/uevent", i);
+		if (!read_file(path, buf, sizeof buf))
+			continue;
+
+		if (!strstr(buf, match))
+			continue;
+
+		snprintf(path, sizeof path, "/dev/gpiochip%d", i);
+
+		int fd = open(path, O_RDWR);
+		syscall_error(fd < 0, "open(%s) failed");
+
+		return fd;
+	}
+
+	error("Can't find %s gpiochip", match);
+	return -1;
+}
+
+static int gpio_setup_pl12(unsigned flags)
+{
+	int ret;
+	struct gpio_v2_line_request req = {
+		.num_lines = 1,
+		.offsets[0] = 12,
+		.config.flags = flags,
+		.consumer = "ppkbd",
+	};
+
+	int fd = gpiochip_open("OF_FULLNAME=/soc/pinctrl@1f02c00");
+
+	ret = ioctl(fd, GPIO_V2_GET_LINE_IOCTL, &req);
+	syscall_error(ret < 0, "GPIO_V2_GET_LINE_IOCTL failed");
+
+	close(fd);
+
+	return req.fd;
+}
+
+static int gpio_get_value(int lfd)
+{
+	int ret;
+	struct gpio_v2_line_values vals = {
+		.mask = 1,
+	};
+
+	ret = ioctl(lfd, GPIO_V2_LINE_GET_VALUES_IOCTL, &vals);
+	syscall_error(ret < 0, "GPIO_V2_GET_LINE_IOCTL failed");
+
+	return vals.bits & 0x1;
+}
+
+static int gpio_set_value(int lfd, int val)
+{
+	int ret;
+	struct gpio_v2_line_values vals = {
+		.mask = 1,
+	};
+
+	ret = ioctl(lfd, GPIO_V2_LINE_GET_VALUES_IOCTL, &vals);
+	syscall_error(ret < 0, "GPIO_V2_GET_LINE_IOCTL failed");
+
+	return vals.bits & 0x1;
+}
--- a/firmware/build.sh
+++ b/firmware/build.sh
@ -5,6 +5,45 @@ set -e
 rm -rf build
 mkdir -p build

-sdcc -mmcs51 --iram-size 256 --xram-size 2048 --code-size 0x6000 --code-loc 0x2000 --opt-code-size -I. main.c -o build/fw.ihx
-makebin build/fw.ihx build/fw.bin
-dd if=bootloader.bin of=build/fw.bin conv=notrunc &>/dev/null
+hex2bin()
+{
+	local name="$1"
+	
+	makebin build/$name.ihx build/$name.bin
+	dd if=bootloader.bin of=build/$name.bin conv=notrunc &>/dev/null
+}
+
+# build stock FW
+
+cpp -P -nostdinc -I. -D__ASM_ONLY__ stock-ivt.asm build/stock-ivt.asm
+sdas8051 -plosgff build/stock-ivt.rel build/stock-ivt.asm
+
+echo Stock FW
+sdcc \
+	-mmcs51 --iram-size 256 --xram-size 2048 \
+	--code-size 0x2000 --code-loc 0x2130 \
+	-Wl-bIVECT=0x2000 \
+	-I. \
+	-DFW_REVISION_STR="\"$(git describe) $(git log -1 --format=%cd --date=iso)\"" \
+	-DCONFIG_STOCK_FW=1 \
+	build/stock-ivt.rel main.c \
+	-o build/fw-stock.ihx
+
+hex2bin fw-stock
+
+# build user FW
+
+echo User FW
+sdcc \
+	-mmcs51 --iram-size 255 --xram-size 2048 \
+	--code-size 0x4000 --code-loc 0x4000 \
+	-I. \
+	-DFW_REVISION_STR="\"$(git describe) $(git log -1 --format=%cd --date=iso)\"" \
+	-DCONFIG_STOCK_FW=0 \
+	-DCONFIG_USB_STACK=0 \
+	-DCONFIG_DEBUG_LOG=1 \
+	-DCONFIG_SELFTEST=0 \
+	main.c \
+	-o build/fw-user.ihx
+
+hex2bin fw-user
--- a/firmware/em85f684a.h
+++ b/firmware/em85f684a.h
@ -20,6 +20,8 @@
 #ifndef __EM85F684A_H__
 #define __EM85F684A_H__

+#ifndef __ASM_ONLY__
+
 __sfr __at(0x87) PCON;                // Power Control
 __sfr __at(0xc0) RSTSC;               // Reset Source
 __sfr __at(0xbf) P0_PRST;             // Peripheral Reset
@ -105,11 +107,9 @@ __sfr __at(0xc4) P0_I2CADB;           // I2CA Data Buffer Register
 __sfr __at(0xc5) P0_I2CADAL;          // I2CA Device Address Register L
 __sfr __at(0xc6) P0_I2CADAH;          // I2CA Device Address Register H
 __sfr __at(0xc7) P0_I2CASF;           // I2CA status flag
-
 __sfr __at(0xcd) P0_DEVPD1;           // Peripheral power down
 __sfr __at(0xce) P0_DEVPD2;           // Peripheral power down
 __sfr __at(0xcf) P0_DEVPD3;           // Peripheral power down
-
 __sfr __at(0xd1) P0_SMBTO1;           // SMbus Time Out 1 Register
 __sfr __at(0xd2) P0_SMBTR1;           // SMbus Timer reload 1 Register
 __sfr __at(0xd3) P0_SMBTO2;           // SMbus Time Out 2 Register
@ -273,19 +273,24 @@ __sbit __at(0xea) P92;
 __sbit __at(0xe9) P91;
 __sbit __at(0xe8) P90;

+#endif
+
 #define IRQ_EINT0      0  // External Interrupt 0
 #define IRQ_TIMER0     1  // Timer0 Overflow
 #define IRQ_EINT1      2  // External Interrupt 1
 #define IRQ_TIMER1     3  // Timer1 Overflow
 #define IRQ_UART0      4  // Serial Port 0
 #define IRQ_PINCHANGE  6  // PIN CHANGE Interrupt 0
+#define IRQ_LVD        7  // Low voltage detect Interrupt
 #define IRQ_SYSTEMHOLD 8  // System Hold Interrupt
 #define IRQ_INT2_3     10 // External Interrupt 2~3
 #define IRQ_SPI	       11 // SPI Interrupt
-#define IRQ_ADC	       13 // ADC Conversion Complete
-#define IRQ_TIMER2     14 // Timer2 Overflow
+#define IRQ_PWMD       12 // PWMD Interrupt
+#define IRQ_TIMER3     14 // Timer3 Overflow
 #define IRQ_PWMA       15 // PWMA Interrupt
+#define IRQ_PWME       16 // PWME Interrupt
 #define IRQ_USB	       17 // USB Interrupt
+#define IRQ_PWMF       18 // PWMF Interrupt
 #define IRQ_I2CA       20 // I2CA Interrupt
 #define IRQ_PWMB       23 // PWMB Interrupt
 #define IRQ_PWMC       24 // PWMC Interrupt
--- a/firmware/main.c
+++ b/firmware/main.c
--- a/firmware/registers.h
+++ b/firmware/registers.h
@ -0,0 +1,71 @@
+/**
+ * Pinephone Keyboard Firmware
+ *
+ * Copyright (C) 2021  Ondřej Jirman <megi@xff.cz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __PPKB_I2C_REGISTERS__
+#define __PPKB_I2C_REGISTERS__
+
+// defines register API in BCD format (currently 1.0)
+// on incompatible change this may need to be changed
+#define FW_REVISION		0x10
+
+#define REG_DEVID_K		0x00
+#define REG_DEVID_B		0x01
+#define REG_FW_REVISION		0x02
+#define REG_FW_FEATURES		0x03
+#define REG_FW_FEATURES_USB_DEBUGGER	BIT(0)
+#define REG_FW_FEATURES_FLASHING_MODE	BIT(1)
+#define REG_FW_FEATURES_SELF_TEST	BIT(2)
+#define REG_FW_FEATURES_STOCK_FW	BIT(3)
+
+#define REG_KEYMATRIX_SIZE	0x06
+#define REG_KEYMATRIX_STATE_CRC8 0x07
+#define REG_KEYMATRIX_STATE	0x08
+#define REG_KEYMATRIX_STATE_END	0x13
+
+#define REG_SYS_CONFIG		0x20
+#define REG_SYS_CONFIG_SCAN_BLOCK	BIT(0)
+#define REG_SYS_CONFIG_POLL_MODE	BIT(1)
+#define REG_SYS_CONFIG_USB_DEBUG_EN	BIT(2)
+
+#define REG_SYS_COMMAND		0x21
+#define REG_SYS_COMMAND_MCU_RESET	'r'
+#define REG_SYS_COMMAND_SELFTEST	't'
+#define REG_SYS_COMMAND_USB_IAP		'i'
+
+#define REG_SYS_USER_APP_BLOCK	0x22
+#define REG_SYS_USER_APP_BLOCK_MAGIC	0x53
+
+#define REG_FLASH_DATA_START	0x70
+#define REG_FLASH_DATA_END	0xef
+#define REG_FLASH_ADDR_L	0xf0
+#define REG_FLASH_ADDR_H	0xf1
+#define REG_FLASH_CRC8		0xf2
+
+#define REG_FLASH_UNLOCK	0xf3
+#define REG_FLASH_UNLOCK_MAGIC	0x46
+
+#define REG_FLASH_CMD		0xf4
+#define REG_FLASH_CMD_READ_ROM	0x52
+#define REG_FLASH_CMD_WRITE_ROM	0x57
+#define REG_FLASH_CMD_ERASE_ROM	0x45
+#define REG_FLASH_CMD_COMMIT	0x43
+
+#define REG_DEBUG_LOG		0xff
+
+#endif
--- a/firmware/stock-ivt.asm
+++ b/firmware/stock-ivt.asm
@ -0,0 +1,101 @@
+/**
+ * Pinephone Keyboard Firmware
+ *
+ * Copyright (C) 2021  Ondřej Jirman <megi@xff.cz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "em85f684a.h"
+
+	.area RSEG    (ABS,DATA)
+	.org 0x0000
+	ar0 = 0x00
+	psw0 = 0xd0
+
+.macro irq_user irq
+	. = (ivct_start + irq * 8 + 3)
+	ljmp (irq * 8 + 0x4003)
+	.ds 5
+.endm
+
+.macro irq_ignore irq
+	. = (ivct_start + irq * 8 + 3)
+	reti
+	.ds 7
+.endm
+
+.macro irq_stock irq, name
+	. = (ivct_start + irq * 8 + 3)
+	push    psw0
+	push    ar0
+	mov	r0, #_stock_flag
+	ajmp    name
+.endm
+
+.macro irq_stock_fwd irq, name
+name'_fwd:
+	cjne    @r0, #1, 001$
+	pop 	ar0
+	pop 	psw0
+	ljmp 	name
+001$:
+	pop 	ar0
+	pop 	psw0
+	ljmp (irq * 8 + 0x4003)
+.endm
+
+	.module ivect
+	.area IVECT (REL)
+
+	ivct_start = .
+
+	ljmp	      __sdcc_gsinit_startup
+
+	irq_ignore    5
+	irq_ignore    9
+	irq_ignore    13
+	irq_ignore    19
+	irq_ignore    21
+	irq_ignore    22
+	irq_ignore    25
+	irq_ignore    26
+	irq_ignore    27
+
+        irq_user   IRQ_EINT0
+        irq_user   IRQ_TIMER0
+        irq_user   IRQ_EINT1
+        irq_stock  IRQ_TIMER1, _timer1_interrupt_fwd
+        irq_user   IRQ_UART0
+        irq_stock  IRQ_PINCHANGE, _pinchange_interrupt_fwd
+        irq_user   IRQ_LVD
+        irq_user   IRQ_SYSTEMHOLD
+        irq_user   IRQ_INT2_3
+        irq_user   IRQ_SPI
+        irq_user   IRQ_PWMD
+        irq_user   IRQ_PWME
+        irq_user   IRQ_PWMF
+        irq_user   IRQ_TIMER3
+        irq_user   IRQ_PWMA
+        irq_user   IRQ_USB
+        irq_stock  IRQ_USB _usb_interrupt_fwd
+        irq_user   IRQ_I2CA
+        irq_user   IRQ_PWMB
+        irq_user   IRQ_PWMC
+        irq_stock  IRQ_I2CB _i2c_b_interrupt_fwd
+
+	irq_stock_fwd IRQ_TIMER1, _timer1_interrupt
+	irq_stock_fwd IRQ_PINCHANGE, _pinchange_interrupt
+	irq_stock_fwd IRQ_I2CB, _i2c_b_interrupt
+	irq_stock_fwd IRQ_USB, _usb_interrupt
--- a/i2c-charger-ctl.c
+++ b/i2c-charger-ctl.c
@ -0,0 +1,478 @@
+/*
+ * Pinephone keyboard power management daemon/tool.
+ *
+ * Copyright (C) 2021  Ondřej Jirman <megi@xff.cz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "common.c"
+
+/*
+ * - Independent control of
+ *   - Boost (5V VOUT to power Pinephone)
+ *   - Battery Charger
+ *
+ * - Optional automatic power on when load is inserted
+ * - Optional auto enable of VOUT when disconnecting VIN (reg 0x04)
+ *
+ * - Optiobal automatic shutdown when VOUT has light load (customizable via reg
+ *   0x0c, min. is 100mA, shutdown lastsa 8-64s (see reg 0x04))
+ *
+ * - Charger_CTL1 0x22
+ *   - Control of charging current based on VOUT undervoltage (it tries
+ *     to keep VOUT in a certain range by reducing load on VIN by
+ *     decreasing charging current?)
+ *
+ * - Battery type selection 4.2/4.3/4.35V
+ *   - + extra margin 0-42mV during constant voltage phase?
+ *   - External (via VSET pin) or internal setting (via reg 0x24)
+ *
+ * - Charging current selection (100mA - 2.3A ?)
+ *
+ * - Charging status register
+ *   - charging state - idle, trickle, constant voltage/current phase, full,
+ *     timeout
+ *   - LED heavey load indication
+ *   - VIN overvoltage indication (> 5.6V)
+ *
+ * - Button press status
+ *   - current state: UP/DOWN
+ *   - long press
+ *   - short press
+ *
+ * GPIO:
+ *
+ * - KEY input
+ *   - Long press button time selection 1-4s
+ *   - Enable/disable 2x short press shutdown function
+ * - L3/L4 function selection:
+ *   - GPIO0/1
+ *   - normal function
+ * - LIGHT pin function selection:
+ *   - GPIO2
+ *   - VREF
+ *   - WLED
+ * - VSET
+ *   - VSET (normal function to select battery voltage via PIN setting)
+ *   - GPIO4
+ * - RSET
+ *   - GPIO3
+ *   - battery internal resistance selection via resistor on the RSET pin
+ *
+ * - separate input/output enable register for all 5 GPIOs
+ * - GPIO data register to read/write values to pins
+ *
+ * ADC:
+ *
+ * - 14 bit two register VBAT, IBAT, VBAT_OCV readings
+*/
+
+#define SYS_CTL0 0x01
+#define SYS_CTL1 0x02
+#define SYS_CTL2 0x0c
+#define SYS_CTL3 0x03
+#define SYS_CTL4 0x04
+#define SYS_CTL5 0x07
+
+#define Charger_CTL1 0x22
+#define Charger_CTL2 0x24
+#define CHG_DIG_CTL4 0x25
+#define CHG_DIG_CTL4_2 0x26
+
+#define READ0 0x71
+#define READ1 0x72
+#define READ2 0x77
+
+// bits 4-0 are mapped to gpio4 - gpio0
+#define MFP_CTL0 0x51
+#define MFP_CTL1 0x52
+#define GPIO_INEN 0x53
+#define GPIO_OUTEN 0x54
+#define GPIO_DATA 0x55
+
+#define BATVADC_DAT_L 0xa2
+#define BATVADC_DAT_H 0xa3
+#define BATIADC_DAT_L 0xa4
+#define BATIADC_DAT_H 0xa5
+#define BATOCV_DAT_L 0xa8
+#define BATOCV_DAT_H 0xa9
+
+uint8_t read_power(int fd, uint8_t reg)
+{
+	int ret;
+	uint8_t val;
+	struct i2c_msg msgs[] = {
+		{ POWER_ADDR, 0, 1, &reg }, // address
+		{ POWER_ADDR, I2C_M_RD, 1, &val },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+
+	return val;
+}
+
+void write_power(int fd, uint8_t reg, uint8_t val)
+{
+	int ret;
+	uint8_t buf[] = {reg, val};
+
+	struct i2c_msg msgs[] = {
+		{ POWER_ADDR, 0, 2, buf },
+	};
+
+	debug("wr 0x%02hhx: %02hhx\n", reg, val);
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+}
+
+void update_power(int fd, uint8_t reg, uint8_t mask, uint8_t val)
+{
+	uint8_t tmp;
+
+	tmp = read_power(fd, reg);
+	tmp &= ~mask;
+	tmp |= val & mask;
+	write_power(fd, reg, tmp);
+}
+
+// in mV
+unsigned get_bat_voltage(int fd)
+{
+	unsigned l = read_power(fd, BATVADC_DAT_L);
+	unsigned h = read_power(fd, BATVADC_DAT_H);
+
+	if (h & 0x20)
+		return 2600 - ((~l & 0xff) + ((~h & 0x1f) << 8) + 1) * 1000 / 3724;
+
+	return 2600 + (l + (h << 8)) * 1000 / 3724;
+}
+
+int get_bat_current(int fd)
+{
+	unsigned l = read_power(fd, BATIADC_DAT_L);
+	unsigned h = read_power(fd, BATIADC_DAT_H);
+
+	if (h & 0x20)
+		return - (int)((~l & 0xff) + ((~h & 0x1f) << 8) + 1) * 1000 / 1341;
+
+	return (l + (h << 8)) * 1000 / 1341;
+}
+
+unsigned get_bat_oc_voltage(int fd)
+{
+	unsigned l = read_power(fd, BATOCV_DAT_L);
+	unsigned h = read_power(fd, BATOCV_DAT_H);
+
+	if (h & 0x20)
+		return 2600 - ((~l & 0xff) + ((~h & 0x1f) << 8) + 1) * 1000 / 3724;
+
+	return 2600 + (l + (h << 8)) * 1000 / 3724;
+}
+
+const char* get_chg_status_text(uint8_t s)
+{
+	switch (s) {
+	case 0: return "Idle";
+	case 1: return "Trickle charge";
+	case 2: return "Constant current phase";
+	case 3: return "Constant voltage phase";
+	case 4: return "Constant voltage stop";
+	case 5: return "Full";
+	case 6: return "Timeout";
+	default: return "Unknown";
+	}
+}
+
+void power_status(int fd)
+{
+	uint8_t r0 = read_power(fd, READ0);
+	uint8_t r1 = read_power(fd, READ1);
+	uint8_t r2 = read_power(fd, READ2);
+	uint8_t s0 = read_power(fd, SYS_CTL0);
+
+	printf("Charger: %s (%s%s%s%s%s%s%s)\n", get_chg_status_text((r0 >> 5) & 0x7),
+		r0 & BIT(4) ? " chg_op" : "",
+		r0 & BIT(3) ? " chg_end" : "",
+		r0 & BIT(2) ? " cv_timeout" : "",
+		r0 & BIT(1) ? " chg_timeout" : "",
+		r0 & BIT(0) ? " trickle_timeout" : "",
+		r1 & BIT(6) ? " VIN overvoltage" : "",
+		r1 & BIT(5) ? " <= 75mA load" : ""
+	);
+
+	printf("Button: %02hhx (%s%s%s%s)\n", r2,
+		r2 & BIT(3) ? " btn_press" : " btn_not_press",
+		r2 & BIT(2) ? " double_press" : "",
+		r2 & BIT(1) ? " long_press" : "",
+		r2 & BIT(0) ? " short_press" : ""
+	);
+
+	// this has some nice undocummneted status bits
+	printf("0x70: %02hhx\n", read_power(fd, 0x70));
+
+	update_power(fd, READ2, 0x7, 0x7);
+}
+
+// dump registers
+
+struct bitinfo {
+	const char* name;
+	uint8_t shift;
+	uint8_t len;
+	void (*fmt)(char* out, size_t out_size, uint8_t val);
+};
+
+struct reginfo {
+	uint8_t reg;
+	const char* name;
+	struct bitinfo* bits;
+};
+
+#define REG_START(addr, name) [addr] = { addr, name, (struct bitinfo[]){
+#define REG_END {} }},
+#define REG_BITS(name, s, l) { #name, s, l },
+#define REG(addr, name) REG_START(addr, name) { name, 0, 8 }, REG_END
+#define REG_SIMPLE(n) REG(n, #n)
+
+struct reginfo regs[256] = {
+	REG_START(SYS_CTL0, "SYS_CTL0")
+		REG_BITS(CHARGER_EN, 1, 1)
+		REG_BITS(BOOST_EN, 2, 1)
+		REG_BITS(LIGHT_EN, 3, 1)
+		REG_BITS(FLASHLIGHT_DET_EN, 4, 1)
+	REG_END
+	REG_START(SYS_CTL1, "SYS_CTL1")
+		REG_BITS(AUTO_POWERON_ON_VIN_INSERT_EN, 0, 1)
+		REG_BITS(LIGHT_LOAD_AUTO_SHUTDOWN_EN, 1, 1)
+	REG_END
+	REG_START(SYS_CTL2, "SYS_CTL2")
+		REG_BITS(LIGHT_SHUTDOWN_CURRENT, 3, 5)
+	REG_END
+	REG_START(SYS_CTL3, "SYS_CTL3")
+		REG_BITS(DOUBLE_PRESS_SHUTDOWN_EN, 5, 1)
+		REG_BITS(LONG_PRESS_TIME, 6, 2)
+	REG_END
+	REG_START(SYS_CTL4, "SYS_CTL4")
+		REG_BITS(SHUTDOWN_TIME, 6, 2)
+		REG_BITS(VIN_PULLOUT_BOOST_ON, 5, 1)
+	REG_END
+	REG_START(SYS_CTL5, "SYS_CTL5")
+		REG_BITS(NTC_EN, 6, 1)
+		REG_BITS(FLASH_LED_EN_0_LONG_PRESS_1_DOUBLE_PRESS, 1, 1)
+		REG_BITS(SHUTDOWN_1_LONG_PRESS_0_DOUBLE_PRESS, 0, 1)
+	REG_END
+	REG_START(Charger_CTL1, "Charger_CTL1")
+		REG_BITS(UV_LOOP, 2, 2)
+	REG_END
+	REG_START(Charger_CTL2, "Charger_CTL2")
+		REG_BITS(BAT_TYPE, 5, 2)
+		REG_BITS(CV_PRESSURE, 1, 2)
+	REG_END
+	REG_START(CHG_DIG_CTL4_2, "CHG_DIG_CTL4_2")
+		REG_BITS(BAT_TYPE_SEL_1_VSET_PIN_0_REGISTER, 6, 1)
+	REG_END
+	REG_START(CHG_DIG_CTL4, "CHG_DIG_CTL4")
+		REG_BITS(CHG_CURRENT, 0, 5)
+	REG_END
+	REG_SIMPLE(MFP_CTL0)
+	REG_SIMPLE(MFP_CTL1)
+	REG_SIMPLE(GPIO_INEN)
+	REG_SIMPLE(GPIO_OUTEN)
+	REG_SIMPLE(GPIO_DATA)
+	REG_SIMPLE(BATVADC_DAT_L)
+	REG_SIMPLE(BATVADC_DAT_H)
+	REG_SIMPLE(BATOCV_DAT_L)
+	REG_SIMPLE(BATOCV_DAT_H)
+	REG_SIMPLE(BATIADC_DAT_L)
+	REG_SIMPLE(BATIADC_DAT_H)
+
+	REG_START(0x70, "READ_70")
+//		REG_BITS(DISCHARGING, 2, 1)
+		REG_BITS(VOUT_BOOST, 2, 1)
+		REG_BITS(CHARGING, 3, 1)
+		REG_BITS(VIN_INSERTED, 4, 1)
+		REG_BITS(VIN_NOT_INSERTED, 5, 1)
+	REG_END
+
+	REG_SIMPLE(READ0)
+	REG_SIMPLE(READ1)
+	REG_SIMPLE(READ2)
+};
+
+static void dump_regs(int fd)
+{
+	for (int addr = 0; addr <= 0xff; addr++) {
+		struct reginfo* ri = &regs[addr];
+
+		uint8_t val = read_power(fd, addr);
+		if (val == 0 && !ri->name)
+			continue;
+
+		printf("%02x: %02hhx", addr, val);
+		if (ri->name) {
+			printf(" (%s)", ri->name);
+
+			for (int i = 0; ri->bits[i].name; i++) {
+				struct bitinfo* bi = &ri->bits[i];
+				uint8_t bval = (val >> bi->shift) & (((1u) << (bi->len)) - 1);
+
+				printf(" %s=0x%02hhx", bi->name, bval);
+			}
+		}
+		printf("\n");
+	}
+}
+
+static void usage(void)
+{
+	printf(
+	       "Usage: ppkb-charger-ctl [--verbose] [--help]\n"
+	       "                        [<info|power-on|power-off|charger-on|charger-off|auto>...]\n"
+	       "\n"
+	       "Options:\n"
+	       "  -c, --current         Change the charging current (mA).\n"
+	       "  -v, --verbose         Show details of what's going on.\n"
+	       "  -h, --help            This help.\n"
+	       "\n"
+	       "Commands:\n"
+	       "  info        Display information about the current state of the charger chip.\n"
+	       "  power-on    Power on VOUT (boost output to the phone and keyboard).\n"
+	       "  power-off   Power off VOUT.\n"
+	       "  charger-on  Start charging the battery.\n"
+	       "  charger-off Stop charging the battery.\n"
+	       "  auto        Switch to automatic control of VOUT/Charging (default configuration).\n"
+	       "  dump        Dump charger chip registers.\n"
+	       "\n"
+	       "Pinephone keyboard charger control tool " VERSION "\n"
+	       "Written by Ondrej Jirman <megi@xff.cz>, 2021\n"
+	       "Licensed under GPLv3, see https://xff.cz/git/pinephone-keyboard/ for\n"
+	       "more information.\n"
+	);
+
+	exit(2);
+}
+
+int main(int ac, char* av[])
+{
+	int fd, ret;
+	int current = -1;
+
+	while (1) {
+		int option_index = 0;
+		struct option long_options[] = {
+			{ "current", required_argument, 0, 'c' },
+			{ "verbose", no_argument,       0, 'v' },
+			{ "help",    no_argument,       0, 'h' },
+			{ 0,         0,                 0,  0  }
+		};
+
+		int c = getopt_long(ac, av, "c:vh", long_options, &option_index);
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'c':
+			errno = 0;
+			char* next = NULL;
+			current = strtol(optarg, &next, 10);
+			if (errno || next == optarg) {
+				printf("ERROR: Can't parse --current %s\n\n", optarg);
+				usage();
+			}
+			break;
+		case 'v':
+			verbose = 1;
+			break;
+		case 'h':
+		case '?':
+		default:
+			usage();
+			break;
+		}
+	}
+
+	if (optind == ac)
+		usage();
+
+	if (current > 2000) {
+		printf("ERROR: --current %d too big\n\n", current);
+		usage();
+	}
+
+	if (current != -1 && current < 100) {
+		printf("ERROR: --current %d too small\n\n", current);
+		usage();
+	}
+
+	fd = pogo_i2c_open();
+
+	if (current != -1) {
+		//update_power(fd, SYS_CTL0, BIT(2), BIT(2));
+	}
+
+	int lfd = gpio_setup_pl12(GPIO_V2_LINE_FLAG_INPUT | GPIO_V2_LINE_FLAG_BIAS_PULL_UP | /*GPIO_V2_LINE_FLAG_ACTIVE_HIGH |*/ GPIO_V2_LINE_FLAG_EDGE_FALLING);
+
+//	update_power(fd, SYS_CTL1, 0x03, 0x00); // disable automatic control based on load detection
+//	update_power(fd, SYS_CTL0, 0x1e, BIT(1) | BIT(2)); // 2=boost 1=charger enable
+//	update_power(fd, SYS_CTL3, BIT(5), 0); // disable "2x key press = shutdown" function
+//	update_power(fd, SYS_CTL4, BIT(5), 0); // disable "VIN pull out -> VOUT auto-enable" function
+//	update_power(fd, CHG_DIG_CTL4, 0x1f, 15); // set charging current (in 100mA steps)
+
+	for (int i = optind; i < ac; i++) {
+		if (!strcmp(av[i], "power-on")) {
+			update_power(fd, SYS_CTL0, BIT(2), BIT(2));
+		} else if (!strcmp(av[i], "power-off")) {
+			update_power(fd, SYS_CTL0, BIT(2), 0);
+			update_power(fd, SYS_CTL1, 0x03, 0x00); // disable automatic control based on load detection
+			update_power(fd, SYS_CTL4, BIT(5), 0); // disable "VIN pull out -> VOUT auto-enable" function
+		} else if (!strcmp(av[i], "charger-on")) {
+			update_power(fd, SYS_CTL0, BIT(1), BIT(1));
+		} else if (!strcmp(av[i], "charger-off")) {
+			update_power(fd, SYS_CTL0, BIT(1), 0);
+		} else if (!strcmp(av[i], "info")) {
+			power_status(fd);
+			printf("V=%u mV (OCV %u mV) I=%d mA\n",
+			       get_bat_voltage(fd),
+			       get_bat_oc_voltage(fd),
+			       get_bat_current(fd));
+		} else if (!strcmp(av[i], "dump")) {
+			dump_regs(fd);
+		} else if (!strcmp(av[i], "auto")) {
+			// enable automatic control based on load detection
+			update_power(fd, SYS_CTL1, 0x03, 0x03);
+			// disable "2x key press = shutdown" function
+			update_power(fd, SYS_CTL3, BIT(5), BIT(5));
+			// disable "VIN pull out -> VOUT auto-enable" function
+			update_power(fd, SYS_CTL4, BIT(5), BIT(5));
+		} else {
+			printf("ERROR: Unknown command: %s\n\n", av[i]);
+			usage();
+		}
+	}
+
+	return 0;
+}
--- a/i2c-debugger.c
+++ b/i2c-debugger.c
@ -0,0 +1,60 @@
+/*
+ * Pinephone keyboard I2C debugging tool.
+ *
+ * Copyright (C) 2021  Ondřej Jirman <megi@xff.cz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "common.c"
+#include "firmware/registers.h"
+
+void dump_log(int fd)
+{
+	int ret;
+	uint8_t addr = REG_DEBUG_LOG;
+	uint8_t buf[64];
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, 1, &addr }, // set 0xff address
+		{ KB_ADDR, I2C_M_RD, sizeof(buf), buf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+
+	int i;
+	for (i = 0; i < sizeof(buf) && buf[i]; i++);
+	
+	if (i > 0)
+		xwrite(1, buf, i);
+}
+
+int main(int ac, char* av[])
+{
+	int fd, ret;
+
+	fd = pogo_i2c_open();
+
+	while (1) {
+		dump_log(fd);
+		usleep(10000);
+	}
+
+	return 0;
+}
--- a/i2c-flasher.c
+++ b/i2c-flasher.c
@ -0,0 +1,509 @@
+/*
+ * Pinephone keyboard I2C flashing tool.
+ *
+ * Copyright (C) 2021  Ondřej Jirman <megi@xff.cz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "common.c"
+#include "firmware/registers.h"
+
+static int iic_fd = -1;
+
+static void dump_log(void)
+{
+	int ret;
+	uint8_t addr = 0xff;
+	uint8_t buf[64];
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, 1, &addr }, // set 0xff address
+		{ KB_ADDR, I2C_M_RD, sizeof(buf), buf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(iic_fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+
+	int i;
+	for (i = 0; i < sizeof(buf) && buf[i]; i++);
+
+	if (i > 0)
+		xwrite(1, buf, i);
+}
+
+static void wr_buf(uint8_t addr, uint8_t* buf, size_t size)
+{
+	int ret;
+	uint8_t mbuf[size + 1];
+
+	mbuf[0] = addr;
+	memcpy(mbuf + 1, buf, size);
+
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, size + 1, mbuf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	debug("WR[%02hhx]:", addr);
+	for (int i = 0; i < size; i++)
+		debug(" %02hhx", buf[i]);
+	debug("\n");
+
+	ret = ioctl(iic_fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+}
+
+static void rd_buf(uint8_t addr, uint8_t* buf, size_t size)
+{
+	int ret;
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, 1, &addr },
+		{ KB_ADDR, I2C_M_RD, size, buf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(iic_fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+
+	debug("RD[%02hhx]:", addr);
+	for (int i = 0; i < size; i++) {
+		if (i % 8 == 0 && i > 0)
+			debug("\n       ");
+		debug(" %02hhx", buf[i]);
+	}
+	debug("\n");
+}
+
+static int rd_buf_nofail(uint8_t addr, uint8_t* buf, size_t size)
+{
+	int ret;
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, 1, &addr },
+		{ KB_ADDR, I2C_M_RD, size, buf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(iic_fd, I2C_RDWR, &msg);
+
+	if (ret == 2) {
+		debug("RD[%02hhx]:", addr);
+		for (int i = 0; i < size; i++) {
+			if (i % 8 == 0 && i > 0)
+				debug("\n       ");
+			debug(" %02hhx", buf[i]);
+		}
+		debug("\n");
+	}
+
+	return ret == 2 ? 0 : -1;
+}
+
+static uint8_t rd_reg(uint8_t addr)
+{
+	uint8_t reg;
+	rd_buf(addr, &reg, 1);
+	return reg;
+}
+
+static int wait_flash_cmd_done(uint8_t cmd)
+{
+	int ret;
+
+	for (int i = 0; i < 10; i++) {
+		uint8_t status;
+
+		ret = rd_buf_nofail(REG_FLASH_CMD, &status, 1);
+		if (ret == 0) {
+			if (status == 0xffu)
+				error("Flashing command 0x%02hhx failed", cmd);
+			else if (status == 0x00)
+				return 0;
+		}
+
+		usleep(5000);
+	}
+	
+	error("Flashing command 0x%02hhx timed out", cmd);
+}
+
+static void read_rom_block(uint8_t* out, uint16_t addr)
+{
+	uint8_t read_rom_start[] = {
+		addr & 0xff, // Addr L
+		(addr >> 8) & 0xff, // Addr H
+		0x00, // CRC-8
+		REG_FLASH_UNLOCK_MAGIC, // Unlock
+		REG_FLASH_CMD_READ_ROM, // Read ROM
+	};
+
+	wr_buf(REG_FLASH_ADDR_L, read_rom_start, sizeof read_rom_start);
+
+	wait_flash_cmd_done(REG_FLASH_CMD_READ_ROM);
+
+	rd_buf(REG_FLASH_DATA_START, out, 128);
+
+	if (rd_reg(REG_FLASH_CRC8) != crc8(out, 128))
+		error("CRC8 failure on ROM read");
+}
+
+static void write_rom_block(uint16_t addr, uint8_t* data)
+{
+	uint8_t write_rom_start[5] = {
+		addr & 0xff, // Addr L
+		(addr >> 8) & 0xff, // Addr H
+		crc8(data, 128), // CRC-8
+		REG_FLASH_UNLOCK_MAGIC, // Unlock
+		REG_FLASH_CMD_WRITE_ROM, // Write ROM
+	};
+
+	wr_buf(REG_FLASH_DATA_START, data, 128);
+	wr_buf(REG_FLASH_ADDR_L, write_rom_start, sizeof write_rom_start);
+
+	usleep(5000);
+	wait_flash_cmd_done(REG_FLASH_CMD_WRITE_ROM);
+}
+
+static void run_flash_cmd(uint8_t cmd)
+{
+	uint8_t cmd_data[] = {
+		REG_FLASH_UNLOCK_MAGIC, // Unlock
+		cmd, // Command
+	};
+
+	wr_buf(REG_FLASH_UNLOCK, cmd_data, sizeof cmd_data);
+
+	usleep(10000);
+	wait_flash_cmd_done(cmd);
+}
+
+static bool is_block_empty(uint8_t* buf)
+{
+	for (unsigned i = 0; i < 128; i++)
+		if (buf[i] != 0xff)
+			return false;
+
+	return true;
+}
+
+static int is_kb_stock_connected(void)
+{
+	uint8_t devid[5];
+	int ret;
+
+	ret = rd_buf_nofail(REG_DEVID_K, devid, sizeof devid);
+	if (ret)
+		return 0;
+
+	if (devid[REG_DEVID_K] != 'K' || devid[REG_DEVID_B] != 'B') // keyboard firmware magic
+		return 0;
+	if (!(devid[REG_FW_FEATURES] & REG_FW_FEATURES_STOCK_FW)) // stock firmware flag
+		return 0;
+
+	if (devid[REG_FW_REVISION] != 0x10)
+		error("Unsupported stock pinephone keyboard firmware version %02hhx, expecting 0x10\n", devid[2]);
+	if (!(devid[REG_FW_FEATURES] & REG_FW_FEATURES_FLASHING_MODE))
+		error("Your stock pinephone keyboard firmware doesn't have flashing support\n");
+
+	return 1;
+}
+
+static void usage(void)
+{
+	printf(
+	       "Usage: ppkb-i2c-flasher [--rom-in <path>] [--rom-out <path>] [--verbose]\n"
+	       "                        [--help] [<read|write|erase|info|reset>...]\n"
+	       "\n"
+	       "Options:\n"
+	       "  -i, --rom-in <path>   Specify path to binary file you want to flash.\n"
+	       "  -o, --rom-out <path>  Specify path where you want to store the contents\n"
+	       "                        of code ROM read from the device.\n"
+	       "  -s, --size <size>     Specify how many bytes of code rom to flash\n"
+	       "                        starting from offset 0x4000 in the rom file.\n"
+	       "  -e, --entry <manual|i2c|none>\n"
+	       "                        Specify how to enter the stock firmware:\n"
+	       "                        - manual: Ask the user to power-cycle the keyboard\n"
+	       "                        - i2c: Send I2C command to make supporting user\n"
+	       "                        - none: Assume stock firmware is already running\n"
+	       "  -v, --verbose         Show details of what's going on.\n"
+	       "  -h, --help            This help.\n"
+	       "\n"
+	       "Commands:\n"
+	       "  info      Display information about the current firmware.\n"
+	       "  read      Read ROM from the device to --rom-out file.\n"
+	       "  write     Flash ROM file to the device from --rom-in.\n"
+	       "  erase     Erase the user firmware.\n"
+	       "  reset     Perform software reset of the MCU.\n"
+	       "  usbiap    Restart to USB IAP mode.\n"
+	       "\n"
+	       "Format of the ROM files is a flat binary. Only the part of it starting\n"
+	       "from 0x4000 will be flashed. Use -s to specify how many bytes to write.\n"
+	       "The stock firmware between 0x2000 and 0x4000 will be preserved.\n"
+	       "\n"
+	       "Pinephone keyboard I2C flashing tool " VERSION "\n"
+	       "Written by Ondrej Jirman <megi@xff.cz>, 2021\n"
+	       "Licensed under GPLv3, see https://xff.cz/git/pinephone-keyboard/ for\n"
+	       "more information.\n"
+	);
+
+	exit(2);
+}
+
+int main(int ac, char* av[])
+{
+	char* rom_in = NULL;
+	char* rom_out = NULL;
+	char* entry_type = "i2c";
+	int size = 0x1200;
+	int ret;
+
+	while (1) {
+		int option_index = 0;
+		struct option long_options[] = {
+			{ "rom-in",  required_argument, 0, 'i' },
+			{ "rom-out", required_argument, 0, 'o' },
+			{ "size",    required_argument, 0, 's' },
+			{ "entry",   required_argument, 0, 'e' },
+			{ "verbose", no_argument,       0, 'v' },
+			{ "help",    no_argument,       0, 'h' },
+			{ 0,         0,                 0,  0  }
+		};
+
+		int c = getopt_long(ac, av, "i:o:s:e:vh", long_options, &option_index);
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'o':
+			rom_out = strdup(optarg);
+			break;
+		case 'i':
+			rom_in = strdup(optarg);
+			break;
+		case 'e':
+			entry_type = strdup(optarg);
+			break;
+		case 's':
+			if (strstr(optarg, "0x") == optarg) {
+				errno = 0;
+				char* next = NULL;
+				size = strtol(optarg + 2, &next, 16);
+				if (errno || next == optarg + 2) {
+					printf("ERROR: Can't parse --size %s\n\n", optarg);
+					usage();
+				}
+			} else {
+				errno = 0;
+				char* next = NULL;
+				size = strtol(optarg, &next, 10);
+				if (errno || next == optarg) {
+					printf("ERROR: Can't parse --size %s\n\n", optarg);
+					usage();
+				}
+			}
+			break;
+		case 'v':
+			verbose = 1;
+			break;
+		case 'h':
+		case '?':
+		default:
+			usage();
+			break;
+		}
+	}
+
+	if (optind == ac)
+		usage();
+
+	if (size < 128) {
+		printf("ERROR: --size 0x%04x too small\n\n", size);
+		usage();
+	}
+
+	if (size > 0x4000) {
+		printf("ERROR: --size 0x%04x too large\n\n", size);
+		usage();
+	}
+
+	if (size % 128 != 0) {
+		printf("ERROR: --size 0x%04x is not multiple of 128\n\n", size);
+		usage();
+	}
+
+	for (int i = optind; i < ac; i++) {
+		if (!strcmp(av[i], "read")) {
+			if (!rom_out) {
+				printf("ERROR: You must specify target file to write rom contents to via --rom-out\n\n");
+				usage();
+			}
+		} else if (!strcmp(av[i], "write")) {
+			if (!rom_in) {
+				printf("ERROR: You must source file for flashing via --rom-in\n\n");
+				usage();
+			}
+		} else if (!strcmp(av[i], "info")) {
+			;
+		} else if (!strcmp(av[i], "reset")) {
+			;
+		} else if (!strcmp(av[i], "erase")) {
+			;
+		} else if (!strcmp(av[i], "usbiap")) {
+			;
+		} else {
+			printf("ERROR: Unknown command: %s\n\n", av[i]);
+			usage();
+		}
+	}
+
+	printf("Opening keyboard I2C device\n");
+	iic_fd = pogo_i2c_open();
+
+	if (!is_kb_stock_connected()) {
+		if (!strcmp(entry_type, "i2c")) {
+			// send MCU reset command
+			uint8_t cmd[] = {REG_SYS_COMMAND_MCU_RESET};
+			wr_buf(REG_SYS_COMMAND, cmd, sizeof cmd);
+
+			// stock firmware should report itself quickly
+			//
+			// tell firmware to block enrty to user app (we have 1s
+			// window to do this after reset)
+			int i;
+			for (i = 0; i < 10; i++) {
+				if (is_kb_stock_connected()) {
+					uint8_t cmd[] = {REG_SYS_USER_APP_BLOCK_MAGIC};
+					wr_buf(REG_SYS_USER_APP_BLOCK, cmd, sizeof cmd);
+					break;
+				}
+
+				usleep(250000);
+			}
+
+			if (i == 10)
+				error("Reset command issued over I2C failed, stock firmware failed to report itself within 2.5s");
+		} else if (!strcmp(entry_type, "manual")) {
+			printf("Please power off the keyboard by holding the keyboard power key for > 12s, then release the power key and press it shortly, once, to power it on again.\n");
+
+			while (true) {
+				if (is_kb_stock_connected()) {
+					uint8_t cmd[] = {REG_SYS_USER_APP_BLOCK_MAGIC};
+					wr_buf(REG_SYS_USER_APP_BLOCK, cmd, sizeof cmd);
+					break;
+				}
+
+				usleep(250000);
+			}
+		} else if (!strcmp(entry_type, "none")) {
+			error("Stock pinephone keyboard firmware not detected running on the keyboard\n");
+		} else {
+			error("Unknown entry method %s", entry_type);
+		}
+
+		// if after 1s the stock firmware is still running,
+		// everything is ok
+		usleep(1000000);
+		if (!is_kb_stock_connected())
+			error("Failed to block the user app from running");
+	}
+
+	for (int i = optind; i < ac; i++) {
+		if (!strcmp(av[i], "read")) {
+			printf("Reading code ROM\n");
+			uint8_t rom[0x8000];
+			for (unsigned i = 0; i < sizeof(rom); i += 128)
+				read_rom_block(rom + i, i);
+
+			int fd = open(rom_out, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+			if (fd >= 0) {
+				ssize_t wr = write(fd, rom, sizeof rom);
+				syscall_error(wr != sizeof(rom), "write failed");
+				close(fd);
+			}
+		} else if (!strcmp(av[i], "write")) {
+			int fd;
+
+			uint8_t rom[0x8000];
+			memset(rom, 0xff, sizeof rom);
+
+			fd = open(rom_in, O_RDONLY);
+			syscall_error(fd < 0, "open(%s) failed", rom_in);
+			ssize_t len = read(fd, rom, 0x8000);
+			syscall_error(len < 0, "read failed");
+			close(fd);
+			if (len != 0x8000)
+				error("Invalid ROM file (%s) size (%d), must be 32768 bytes", rom_in, (int)len);
+
+			printf("Flashing code ROM\n");
+			for (unsigned i = 0x4000; i < 0x4000 + size; i += 128)
+				write_rom_block(i, rom + i);
+
+			uint8_t rd_rom[0x8000];
+			for (unsigned i = 0x4000; i < 0x4000 + size; i += 128) {
+				read_rom_block(rd_rom + i, i);
+
+				if (memcmp(rd_rom + i, rom + i, 128)) {
+					printf("WARNING: Block 0x%04x write failed, retrying...\n");
+					error("Retries disabled");
+				}
+			}
+
+			printf("Finishing flashing\n");
+			run_flash_cmd(REG_FLASH_CMD_COMMIT);
+		} else if (!strcmp(av[i], "info")) {
+			uint8_t devid[5];
+
+			rd_buf(0x00, devid, sizeof devid);
+
+			printf("DEVID Register dump:\n");
+			for (int i = 0; i < sizeof(devid); i++)
+				printf("0x%02x: 0x%02hhx\n", i, devid[i]);
+		} else if (!strcmp(av[i], "reset")) {
+			printf("Restarting the MCU\n");
+
+			// send MCU reset command
+			uint8_t cmd[] = {REG_SYS_COMMAND_MCU_RESET};
+			wr_buf(REG_SYS_COMMAND, cmd, sizeof cmd);
+		} else if (!strcmp(av[i], "usbiap")) {
+			printf("Restarting to USB IAP mode, if you don't have USB interface soldered on, you'll have to power-cycle the keyboard to get out of this flashing mode.\n");
+
+			// send MCU reset command
+			uint8_t cmd[] = {REG_SYS_COMMAND_USB_IAP};
+			wr_buf(REG_SYS_COMMAND, cmd, sizeof cmd);
+		} else if (!strcmp(av[i], "erase")) {
+			run_flash_cmd(REG_FLASH_CMD_ERASE_ROM);
+		} else {
+			printf("ERROR: Unknown command: %s\n\n", av[i]);
+			usage();
+		}
+	}
+
+	return 0;
+}
--- a/inputd/main.c
+++ b/inputd/main.c
@ -19,100 +19,20 @@

 // {{{ includes

-#include <assert.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <inttypes.h>
-#include <stdarg.h>
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/stat.h>
-#include <time.h>
-#include <sys/ioctl.h>
-#include <unistd.h>
-#include <sys/types.h>
-#include <dirent.h>
-#include <poll.h>
+#include "common.c"
+#include "firmware/registers.h"

-#include <linux/i2c.h>
-#include <linux/i2c-dev.h>
-#include <linux/gpio.h>
 #include <linux/input.h>
 #include <linux/uinput.h>

-#define DEBUG 0
-#define MEGI_PROTO_BUG 0
-
-#if DEBUG
-#define debug(args...) printf(args)
-#else
-#define debug(args...)
-#endif
-
-// }}}
-// {{{ utils
-
-static void syscall_error(int is_err, const char* fmt, ...)
-{
-	va_list ap;
-
-	if (!is_err)
-		return;
-
-	fprintf(stderr, "ERROR: ");
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, ": %s\n", strerror(errno));
-
-	exit(1);
-}
-
-static void error(const char* fmt, ...)
-{
-	va_list ap;
-
-	fprintf(stderr, "ERROR: ");
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, "\n");
-
-	exit(1);
-}
-
-bool read_file(const char* path, char* buf, size_t size)
-{
-	int fd;
-	ssize_t ret;
-
-	fd = open(path, O_RDONLY);
-	if (fd < 0)
-		return false;
-
-	ret = read(fd, buf, size);
-	close(fd);
-	if (ret < 0)
-		return false;
-
-	if (ret < size) {
-		buf[ret] = 0;
-		return true;
-	} else {
-		buf[size - 1] = 0;
-		return false;
-	}
-}
-
-#define KB_ADDR 0x15
+#define MEGI_PROTO_BUG 1

 int read_kb(int fd, uint8_t data[16])
 {
 	int ret;
+	uint8_t b = 5;
 	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, 1, &b },
 		{ KB_ADDR, I2C_M_RD, 16, data },
 	};

@ -122,17 +42,16 @@ int read_kb(int fd, uint8_t data[16])
 	};

 	ret = ioctl(fd, I2C_RDWR, &msg);
-	//syscall_error(ret < 0, "I2C_RDWR failed");
+	syscall_error(ret < 0, "I2C_RDWR failed");

 	return ret == 1 ? 0 : -1;
 }

-#if 0
-int write_kb(int fd, uint8_t data[16])
+int write_kb(int fd, uint8_t* data)
 {
 	int ret;
 	struct i2c_msg msgs[] = {
-		{ KB_ADDR, 0, 16, data },
+		{ KB_ADDR, 0, 4, data },
 	};

 	struct i2c_rdwr_ioctl_data msg = {
@ -141,102 +60,10 @@ int write_kb(int fd, uint8_t data[16])
 	};

 	ret = ioctl(fd, I2C_RDWR, &msg);
-	//syscall_error(ret < 0, "I2C_RDWR failed");
+	syscall_error(ret < 0, "I2C_RDWR failed");

 	return ret == 1 ? 0 : -1;
 }
-#endif
-
-static int gpiochip_open(void)
-{
-	int ret;
-	char path[256], buf[1024];
-	int fd = -1;
-
-	for (int i = 0; i < 8; i++) {
-		snprintf(path, sizeof path, "/sys/bus/gpio/devices/gpiochip%d/uevent", i);
-		if (!read_file(path, buf, sizeof buf))
-			continue;
-
-		if (!strstr(buf, "OF_FULLNAME=/soc/pinctrl@1f02c00"))
-			continue;
-
-		snprintf(path, sizeof path, "/dev/gpiochip%d", i);
-
-		int fd = open(path, O_RDWR);
-		syscall_error(fd < 0, "open(%s) failed");
-
-		//ret = ioctl(fd, I2C_SLAVE, addr);
-		//syscall_error(ret < 0, "I2C_SLAVE failed");
-
-		return fd;
-	}
-
-	error("Can't find POGO I2C adapter");
-	return -1;
-}
-
-static int setup_gpio(void)
-{
-	int ret;
-	struct gpio_v2_line_request req = {
-		.num_lines = 1,
-		.offsets[0] = 12,
-		.config.flags = GPIO_V2_LINE_FLAG_INPUT | GPIO_V2_LINE_FLAG_BIAS_PULL_UP | /*GPIO_V2_LINE_FLAG_ACTIVE_HIGH |*/ GPIO_V2_LINE_FLAG_EDGE_FALLING,
-		.consumer = "ppkbd",
-	};
-
-	int fd = gpiochip_open();
-
-	ret = ioctl(fd, GPIO_V2_GET_LINE_IOCTL, &req);
-	syscall_error(ret < 0, "GPIO_V2_GET_LINE_IOCTL failed");
-
-	close(fd);
-
-	return req.fd;
-}
-
-static int get_int_value(int lfd)
-{
-	int ret;
-	struct gpio_v2_line_values vals = {
-		.mask = 1,
-	};
-
-	ret = ioctl(lfd, GPIO_V2_LINE_GET_VALUES_IOCTL, &vals);
-	syscall_error(ret < 0, "GPIO_V2_GET_LINE_IOCTL failed");
-
-	return vals.bits & 0x1;
-}
-
-static int pogo_i2c_open(void)
-{
-	int ret;
-	char path[256], buf[1024];
-	int fd = -1;
-
-	for (int i = 0; i < 8; i++) {
-		snprintf(path, sizeof path, "/sys/class/i2c-adapter/i2c-%d/uevent", i);
-		if (!read_file(path, buf, sizeof buf))
-			continue;
-
-		if (!strstr(buf, "OF_FULLNAME=/soc/i2c@1c2b400"))
-			continue;
-
-		snprintf(path, sizeof path, "/dev/i2c-%d", i);
-
-		int fd = open(path, O_RDWR);
-		syscall_error(fd < 0, "open(%s) failed");
-
-		//ret = ioctl(fd, I2C_SLAVE, addr);
-		//syscall_error(ret < 0, "I2C_SLAVE failed");
-
-		return fd;
-	}
-
-	error("Can't find POGO I2C adapter");
-	return -1;
-}

 #include "kmap.h"

@ -360,7 +187,7 @@ void on_press(uint8_t phys_idx)
 			fn_mode = 1;
 			return;
 		}
-	
+
 		keys = keymap_fn[phys_idx];
 	} else if (pine_idx >= 0 || pine_mode) {
 		if (key == KEY_ESC) {
@ -454,7 +281,6 @@ void update_keys(uint8_t* map)
 		}
 	}

-
 	// which new keys are pressed?
 	for (int i = 0; i < n_keys; i++) {
 		int key = keys[i];
@ -468,18 +294,6 @@ void update_keys(uint8_t* map)
 	}
 }

-uint64_t time_abs(void)
-{
-	struct timespec tmp;
-	int ret;
-
-	ret = clock_gettime(CLOCK_MONOTONIC, &tmp);
-	if (ret < 0)
-		return 0;
-
-	return tmp.tv_sec * 1000000000ull + tmp.tv_nsec;
-}
-
 int main(int ac, char* av[])
 {
 	int fd, ret;
@ -487,7 +301,7 @@ int main(int ac, char* av[])
 	fd = pogo_i2c_open();
 	uinput_fd = open_uinput_dev();

-	int lfd = setup_gpio();
+	int lfd = gpio_setup_pl12(GPIO_V2_LINE_FLAG_INPUT | GPIO_V2_LINE_FLAG_BIAS_PULL_UP | /*GPIO_V2_LINE_FLAG_ACTIVE_HIGH |*/ GPIO_V2_LINE_FLAG_EDGE_FALLING);

 	struct pollfd fds[2] = {
 		{ .fd = lfd, .events = POLLIN, },
@ -495,6 +309,9 @@ int main(int ac, char* av[])

 	debug("\033[2J");

+	uint8_t buf[4] = {1, 2, 3, 4};
+	ret = write_kb(fd, buf);
+
 	while (1) {
 		ret = poll(fds, 1, 10000);
 		syscall_error(ret < 0, "poll failed");
@ -510,9 +327,7 @@ int main(int ac, char* av[])
 			if (ret)
 				continue;

-#if DEBUG
-			print_bitmap(buf + 4);
-#endif
+//			print_bitmap(buf + 4);
 			update_keys(buf + 4);
 		}
 	}
--- a/i2c-selftest.c
+++ b/i2c-selftest.c
@ -0,0 +1,114 @@
+/*
+ * Pinephone keyboard I2C debugging tool.
+ *
+ * Copyright (C) 2021  Ondřej Jirman <megi@xff.cz>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "common.c"
+#include "firmware/registers.h"
+
+static int iic_fd = -1;
+
+static void dump_log(void)
+{
+	int ret;
+	uint8_t addr = REG_DEBUG_LOG;
+	uint8_t buf[64];
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, 1, &addr }, // set 0xff address
+		{ KB_ADDR, I2C_M_RD, sizeof(buf), buf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(iic_fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+
+	int i;
+	for (i = 0; i < sizeof(buf) && buf[i]; i++);
+	
+	if (i > 0)
+		xwrite(1, buf, i);
+}
+
+static void wr_buf(uint8_t addr, uint8_t* buf, size_t size)
+{
+	int ret;
+	uint8_t mbuf[size + 1];
+
+	mbuf[0] = addr;
+	memcpy(mbuf + 1, buf, size);
+
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, size + 1, mbuf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	debug("WR[%02hhx]:", addr);
+	for (int i = 0; i < size; i++)
+		debug(" %02hhx", buf[i]);
+	debug("\n");
+
+	ret = ioctl(iic_fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+}
+
+static void rd_buf(uint8_t addr, uint8_t* buf, size_t size)
+{
+	int ret;
+	struct i2c_msg msgs[] = {
+		{ KB_ADDR, 0, 1, &addr },
+		{ KB_ADDR, I2C_M_RD, size, buf },
+	};
+
+	struct i2c_rdwr_ioctl_data msg = {
+		.msgs = msgs,
+		.nmsgs = sizeof(msgs) / sizeof(msgs[0])
+	};
+
+	ret = ioctl(iic_fd, I2C_RDWR, &msg);
+	syscall_error(ret < 0, "I2C_RDWR failed");
+
+	debug("RD[%02hhx]:", addr);
+	for (int i = 0; i < size; i++) {
+		if (i % 8 == 0 && i > 0)
+			debug("\n       ");
+		debug(" %02hhx", buf[i]);
+	}
+	debug("\n");
+}
+
+int main(int ac, char* av[])
+{
+	iic_fd = pogo_i2c_open();
+
+	uint8_t cmd[] = {REG_SYS_COMMAND_SELFTEST};
+	wr_buf(REG_SYS_COMMAND, cmd, sizeof cmd);
+
+	while (1) {
+		dump_log();
+		usleep(10000);
+	}
+
+	return 0;
+}
--- a/inputd/build.sh
+++ b/inputd/build.sh
@ -1,4 +0,0 @@
-#!/bin/sh
-
-php map-to-c.php factory-keymap.txt > kmap.h
-gcc -o ppkbd main.c || exit 1
--- a/inputd/kmap.h
+++ b/inputd/kmap.h
@ -1,284 +0,0 @@
-static const uint8_t el_phys_map[256] = {
-	0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 
-	0x19, 0x1a, 0x1b, 0x1c, 0xff, 0xff, 0xff, 0xff, 
-	0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27, 0x28, 
-	0x29, 0x2a, 0x2b, 0x2c, 0xff, 0xff, 0xff, 0xff, 
-	0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 
-	0x39, 0x3a, 0x3b, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 
-	0x49, 0x4a, 0x4b, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0x51, 0xff, 0xff, 0x54, 0xff, 0x56, 0xff, 
-	0x58, 0x57, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0x52, 0x53, 0xff, 0x55, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
-};
-
-static const int used_keys[] = {
-	KEY_ESC,
-	KEY_1,
-	KEY_LEFTSHIFT,
-	KEY_BACKSLASH,
-	KEY_F1,
-	KEY_2,
-	KEY_F2,
-	KEY_3,
-	KEY_DOLLAR,
-	KEY_F3,
-	KEY_4,
-	KEY_EURO,
-	KEY_F4,
-	KEY_5,
-	KEY_GRAVE,
-	KEY_F5,
-	KEY_6,
-	KEY_F6,
-	KEY_7,
-	KEY_MINUS,
-	KEY_F7,
-	KEY_8,
-	KEY_EQUAL,
-	KEY_F8,
-	KEY_9,
-	KEY_F9,
-	KEY_BACKSPACE,
-	KEY_DELETE,
-	KEY_TAB,
-	KEY_Q,
-	KEY_W,
-	KEY_E,
-	KEY_R,
-	KEY_T,
-	KEY_Y,
-	KEY_U,
-	KEY_I,
-	KEY_O,
-	KEY_P,
-	KEY_ENTER,
-	KEY_LEFTMETA,
-	KEY_SYSRQ,
-	KEY_A,
-	KEY_S,
-	KEY_D,
-	KEY_F,
-	KEY_G,
-	KEY_H,
-	KEY_J,
-	KEY_K,
-	KEY_L,
-	KEY_SEMICOLON,
-	KEY_INSERT,
-	KEY_Z,
-	KEY_X,
-	KEY_C,
-	KEY_V,
-	KEY_B,
-	KEY_N,
-	KEY_M,
-	KEY_COMMA,
-	KEY_HOME,
-	KEY_DOT,
-	KEY_UP,
-	KEY_SLASH,
-	KEY_END,
-	KEY_LEFTCTRL,
-	KEY_FN,
-	KEY_LEFTALT,
-	KEY_SPACE,
-	KEY_RIGHTALT,
-	KEY_APOSTROPHE,
-	KEY_LEFT,
-	KEY_LEFTBRACE,
-	KEY_DOWN,
-	KEY_RIGHTBRACE,
-	KEY_RIGHT,
-};
-
-static const char* key_names[] = {
-	[KEY_ESC] = "ESC",
-	[KEY_1] = "1",
-	[KEY_LEFTSHIFT] = "LEFTSHIFT",
-	[KEY_BACKSLASH] = "BACKSLASH",
-	[KEY_F1] = "F1",
-	[KEY_2] = "2",
-	[KEY_F2] = "F2",
-	[KEY_3] = "3",
-	[KEY_DOLLAR] = "DOLLAR",
-	[KEY_F3] = "F3",
-	[KEY_4] = "4",
-	[KEY_EURO] = "EURO",
-	[KEY_F4] = "F4",
-	[KEY_5] = "5",
-	[KEY_GRAVE] = "GRAVE",
-	[KEY_F5] = "F5",
-	[KEY_6] = "6",
-	[KEY_F6] = "F6",
-	[KEY_7] = "7",
-	[KEY_MINUS] = "MINUS",
-	[KEY_F7] = "F7",
-	[KEY_8] = "8",
-	[KEY_EQUAL] = "EQUAL",
-	[KEY_F8] = "F8",
-	[KEY_9] = "9",
-	[KEY_F9] = "F9",
-	[KEY_BACKSPACE] = "BACKSPACE",
-	[KEY_DELETE] = "DELETE",
-	[KEY_TAB] = "TAB",
-	[KEY_Q] = "Q",
-	[KEY_W] = "W",
-	[KEY_E] = "E",
-	[KEY_R] = "R",
-	[KEY_T] = "T",
-	[KEY_Y] = "Y",
-	[KEY_U] = "U",
-	[KEY_I] = "I",
-	[KEY_O] = "O",
-	[KEY_P] = "P",
-	[KEY_ENTER] = "ENTER",
-	[KEY_LEFTMETA] = "LEFTMETA",
-	[KEY_SYSRQ] = "SYSRQ",
-	[KEY_A] = "A",
-	[KEY_S] = "S",
-	[KEY_D] = "D",
-	[KEY_F] = "F",
-	[KEY_G] = "G",
-	[KEY_H] = "H",
-	[KEY_J] = "J",
-	[KEY_K] = "K",
-	[KEY_L] = "L",
-	[KEY_SEMICOLON] = "SEMICOLON",
-	[KEY_INSERT] = "INSERT",
-	[KEY_Z] = "Z",
-	[KEY_X] = "X",
-	[KEY_C] = "C",
-	[KEY_V] = "V",
-	[KEY_B] = "B",
-	[KEY_N] = "N",
-	[KEY_M] = "M",
-	[KEY_COMMA] = "COMMA",
-	[KEY_HOME] = "HOME",
-	[KEY_DOT] = "DOT",
-	[KEY_UP] = "UP",
-	[KEY_SLASH] = "SLASH",
-	[KEY_END] = "END",
-	[KEY_LEFTCTRL] = "LEFTCTRL",
-	[KEY_FN] = "FN",
-	[KEY_LEFTALT] = "LEFTALT",
-	[KEY_SPACE] = "SPACE",
-	[KEY_RIGHTALT] = "RIGHTALT",
-	[KEY_APOSTROPHE] = "APOSTROPHE",
-	[KEY_LEFT] = "LEFT",
-	[KEY_LEFTBRACE] = "LEFTBRACE",
-	[KEY_DOWN] = "DOWN",
-	[KEY_RIGHTBRACE] = "RIGHTBRACE",
-	[KEY_RIGHT] = "RIGHT",
-};
-
-static const int keymap_base[256][2] = {
-	[0x11] = { KEY_ESC },
-	[0x12] = { KEY_1 },
-	[0x13] = { KEY_2 },
-	[0x14] = { KEY_3 },
-	[0x15] = { KEY_4 },
-	[0x16] = { KEY_5 },
-	[0x17] = { KEY_6 },
-	[0x18] = { KEY_7 },
-	[0x19] = { KEY_8 },
-	[0x1a] = { KEY_9 },
-	[0x1c] = { KEY_BACKSPACE },
-	[0x21] = { KEY_TAB },
-	[0x22] = { KEY_Q },
-	[0x23] = { KEY_W },
-	[0x24] = { KEY_E },
-	[0x25] = { KEY_R },
-	[0x26] = { KEY_T },
-	[0x27] = { KEY_Y },
-	[0x28] = { KEY_U },
-	[0x29] = { KEY_I },
-	[0x2a] = { KEY_O },
-	[0x2b] = { KEY_P },
-	[0x2c] = { KEY_ENTER },
-	[0x31] = { KEY_LEFTMETA },
-	[0x32] = { KEY_A },
-	[0x33] = { KEY_S },
-	[0x34] = { KEY_D },
-	[0x35] = { KEY_F },
-	[0x36] = { KEY_G },
-	[0x37] = { KEY_H },
-	[0x38] = { KEY_J },
-	[0x39] = { KEY_K },
-	[0x3a] = { KEY_L },
-	[0x3b] = { KEY_SEMICOLON },
-	[0x41] = { KEY_LEFTSHIFT },
-	[0x42] = { KEY_Z },
-	[0x43] = { KEY_X },
-	[0x44] = { KEY_C },
-	[0x45] = { KEY_V },
-	[0x46] = { KEY_B },
-	[0x47] = { KEY_N },
-	[0x48] = { KEY_M },
-	[0x49] = { KEY_COMMA },
-	[0x4a] = { KEY_DOT },
-	[0x4b] = { KEY_SLASH },
-	[0x51] = { KEY_LEFTCTRL },
-	[0x52] = { KEY_FN },
-	[0x53] = { KEY_LEFTALT },
-	[0x54] = { KEY_SPACE },
-	[0x55] = { KEY_RIGHTALT },
-	[0x56] = { KEY_APOSTROPHE },
-	[0x57] = { KEY_LEFTBRACE },
-	[0x58] = { KEY_RIGHTBRACE },
-};
-
-static const int keymap_fn[256][2] = {
-	[0x12] = { KEY_LEFTSHIFT, KEY_BACKSLASH },
-	[0x13] = { KEY_BACKSLASH },
-	[0x14] = { KEY_DOLLAR },
-	[0x15] = { KEY_EURO },
-	[0x16] = { KEY_LEFTSHIFT, KEY_GRAVE },
-	[0x17] = { KEY_GRAVE },
-	[0x18] = { KEY_MINUS },
-	[0x19] = { KEY_EQUAL },
-	[0x1a] = { KEY_LEFTSHIFT, KEY_MINUS },
-	[0x1c] = { KEY_DELETE },
-	[0x31] = { KEY_LEFTSHIFT, KEY_SYSRQ },
-	[0x3b] = { KEY_INSERT },
-	[0x49] = { KEY_HOME },
-	[0x4a] = { KEY_UP },
-	[0x4b] = { KEY_END },
-	[0x56] = { KEY_LEFT },
-	[0x57] = { KEY_DOWN },
-	[0x58] = { KEY_RIGHT },
-};
-
-static const int keymap_pine[256][2] = {
-	[0x12] = { KEY_F1 },
-	[0x13] = { KEY_F2 },
-	[0x14] = { KEY_F3 },
-	[0x15] = { KEY_F4 },
-	[0x16] = { KEY_F5 },
-	[0x17] = { KEY_F6 },
-	[0x18] = { KEY_F7 },
-	[0x19] = { KEY_F8 },
-	[0x1a] = { KEY_F9 },
-};
-
--- a/keymaps/factory-keymap-megi.txt
+++ b/keymaps/factory-keymap-megi.txt
@ -0,0 +1,63 @@
+# physical layout Row:Col -> KEY COMBO_KEY1 COMBO_KEY2
+# combo1 is using FN key, combo2 is using PINE key
+# see factory-keymap.jpg
+# https://elixir.bootlin.com/linux/latest/source/include/uapi/linux/input-event-codes.h#L76
+
+1:1  ESC
+1:2  1 LEFTSHIFT+BACKSLASH F1
+1:3  2 BACKSLASH F2
+1:4  3 DOLLAR F3    # pound, really, but who uses that? also it doesn't have a keycode in Linux
+1:5  4 EURO F4
+1:6  5 LEFTSHIFT+GRAVE F5
+1:7  6 GRAVE F6
+1:8  7 MINUS F7
+1:9  8 EQUAL F8
+1:10 9 LEFTSHIFT+MINUS F9
+1:11 0 PLUS F10
+1:12 BACKSPACE DELETE
+
+2:1  TAB
+2:2  Q
+2:3  W
+2:4  E
+2:5  R
+2:6  T
+2:7  Y Z
+2:8  U
+2:9  I
+2:10 O
+2:11 P
+2:12 ENTER
+
+3:1  LEFTMETA LEFTSHIFT+SYSRQ    # LEFTMETA = PINE key
+3:2  A
+3:3  S
+3:4  D
+3:5  F
+3:6  G
+3:7  H
+3:8  J
+3:9  K
+3:10 L
+3:11 SEMICOLON INSERT
+
+4:1  LEFTSHIFT
+4:2  Z
+4:3  X
+4:4  C LEFTCTRL
+4:5  V
+4:6  B
+4:7  N
+4:8  M
+4:9  COMMA HOME
+4:10 DOT UP
+4:11 SLASH END
+
+5:1  LEFTCTRL
+5:2  FN
+5:3  LEFTALT
+5:4  SPACE
+5:5  RIGHTALT
+5:6  APOSTROPHE LEFT
+5:7  LEFTBRACE DOWN
+5:8  RIGHTBRACE RIGHT
--- a/keymaps/factory-keymap.jpg
+++ b/keymaps/factory-keymap.jpg
--- a/keymaps/factory-keymap.txt
+++ b/keymaps/factory-keymap.txt
--- a/keymaps/map-to-c.php
+++ b/keymaps/map-to-c.php
@ -20,8 +20,8 @@
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

-$pmap = file_get_contents("physical-map.txt");
-$kmap = file_get_contents($argv[1]);
+$pmap = file_get_contents($argv[1]);
+$kmap = file_get_contents($argv[2]);

 // high nibble = row, low nibble col
 $el_phys_map = [];
--- a/keymaps/physical-map.jpg
+++ b/keymaps/physical-map.jpg
--- a/keymaps/physical-map.txt
+++ b/keymaps/physical-map.txt
--- a/usb-flasher/debugger.c
+++ b/usb-flasher/debugger.c
@ -17,7 +17,6 @@
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

-#define DEBUG 1
 #include "common.c"

 int kb_open(void)
@ -93,21 +92,6 @@ int response(uint8_t res[8])
 	return 0;
 }

-ssize_t xwrite(int fd, uint8_t* buf, size_t len)
-{
-	size_t off = 0;
-	
-	while (off < len) {
-		ssize_t ret = write(fd, buf + off, len - off);
-		if (ret < 0)
-			return ret;
-		
-		off += ret;
-	}
-	
-	return off;
-}
-
 int read_stdout(void)
 {
 	int ret;
@ -125,6 +109,11 @@ int read_stdout(void)
 	if (ret)
 		return ret;

+	debug("STD%d:", urb.actual_length);
+	for (int i = 0; i < urb.actual_length; i++)
+		debug(" %02hhx", buf[i]);
+	debug("\n");
+
 	if (urb.actual_length > 0) {
 		ssize_t rv = xwrite(1, buf, urb.actual_length);
 		if (rv < 0)
@ -180,15 +169,17 @@ int main(int ac, char* av[])
 	if (usb_fd < 0)
 		error("Failed to open the keyboard");
 	
-	int i = 0;
 	while (1) {
 		ret = read_stdout();
+		if (ret < 0 && errno != 110)
+			syscall_error(true, "read_stdout failed");

 		ret = read_keys(keys);
+		if (ret < 0 && errno != 110)
+			syscall_error(true, "read_keys failed");
+
 		if (ret == 0)
 			print_bitmap(keys);
-		
-		i++;
 	}

 	return 0;
--- a/usb-flasher/flasher.c
+++ b/usb-flasher/flasher.c
@ -17,7 +17,6 @@
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

-#define VERSION "1.0"
 #include "common.c"

 // }}}
@ -558,14 +557,14 @@ const char* boot_cond_text(uint8_t status)
 static void usage(void)
 {
 	printf(
-	       "Usage: ppkb-flasher [--rom-in <path>] [--rom-out <path>] [--verbose]\n"
-	       "                    [--help] [<read|write|info|reset>...]\n"
+	       "Usage: ppkb-usb-flasher [--rom-in <path>] [--rom-out <path>] [--verbose]\n"
+	       "                        [--help] [<read|write|info|reset>...]\n"
 	       "\n"
 	       "Options:\n"
 	       "  -i, --rom-in <path>   Specify path to binary file you want to flash.\n"
 	       "  -o, --rom-out <path>  Specify path where you want to store the contents\n"
 	       "                        of code ROM read from the device.\n"
-	       "  -s, --rom-size <size> Specify how many bytes of code rom to flash\n"
+	       "  -s, --size <size>     Specify how many bytes of code rom to flash\n"
 	       "                        starting from offset 0x2000 in the rom file.\n"
 	       "  -v, --verbose         Show details of what's going on.\n"
 	       "  -h, --help            This help.\n"
@ -579,7 +578,7 @@ static void usage(void)
 	       "Format of the ROM files is a flat binary. Only the part of it starting\n"
 	       "from 0x2000 will be flashed. Use -s to specify how many bytes to write.\n"
 	       "\n"
-	       "Pinephone keyboard flashing tool " VERSION "\n"
+	       "Pinephone keyboard USB flashing tool " VERSION "\n"
 	       "Written by Ondrej Jirman <megi@xff.cz>, 2021\n"
 	       "Licensed under GPLv3, see https://xff.cz/git/pinephone-keyboard/ for\n"
 	       "more information.\n"
@ -592,7 +591,7 @@ int main(int ac, char* av[])
 {
 	char* rom_in = NULL;
 	char* rom_out = NULL;
-	int size = 0x1000;
+	int size = 0x1200;
 	int ret;

 	while (1) {
@ -618,7 +617,7 @@ int main(int ac, char* av[])
 			rom_in = strdup(optarg);
 			break;
 		case 's':
-			if (strstr("0x", optarg) == optarg) {
+			if (strstr(optarg, "0x") == optarg) {
 				errno = 0;
 				char* next = NULL;
 				size = strtol(optarg + 2, &next, 16);
--- a/usb-flasher/build.sh
+++ b/usb-flasher/build.sh
@ -1,6 +0,0 @@
-#!/bin/bash
-
-set -e
-
-gcc -o ppkb-flasher flasher.c
-gcc -o ppkb-debugger debugger.c
--- a/usb-flasher/common.c
+++ b/usb-flasher/common.c
@ -1,205 +0,0 @@
-#include <assert.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <inttypes.h>
-#include <stdarg.h>
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <sys/stat.h>
-#include <time.h>
-#include <sys/poll.h>
-#include <sys/ioctl.h>
-#include <unistd.h>
-#include <sys/types.h>
-#include <dirent.h>
-#include <getopt.h>
-
-#include <linux/usbdevice_fs.h>
-
-static bool verbose;
-#define debug(args...) { if (verbose) printf(args); }
-
-static void syscall_error(int is_err, const char* fmt, ...)
-{
-	va_list ap;
-
-	if (!is_err)
-		return;
-
-	fprintf(stderr, "ERROR: ");
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, ": %s\n", strerror(errno));
-
-	exit(1);
-}
-
-static void error(const char* fmt, ...)
-{
-	va_list ap;
-
-	fprintf(stderr, "ERROR: ");
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, "\n");
-
-	exit(1);
-}
-
-static bool read_file(const char* path, char* buf, size_t size)
-{
-	int fd;
-	ssize_t ret;
-
-	fd = open(path, O_RDONLY);
-	if (fd < 0)
-		return false;
-
-	ret = read(fd, buf, size);
-	close(fd);
-	if (ret < 0)
-		return false;
-
-	if (ret < size) {
-		buf[ret] = 0;
-		return true;
-	} else {
-		buf[size - 1] = 0;
-		return false;
-	}
-}
-
-static int open_usb_dev(uint16_t vid, uint16_t pid)
-{
-	char path[256], buf[256];
-	struct dirent *e;
-	unsigned e_vid, e_pid, bus, dev;
-	int fd = -1, ret;
-	DIR* d;
-
-        d = opendir("/sys/bus/usb/devices");
-	syscall_error(d == NULL, "opendir(/sys/bus/usb/devices) failed");
-
-	while (true) {
-		errno = 0;
-		e = readdir(d);
-		syscall_error(e == NULL && errno, "readdir(/sys/bus/usb/devices) failed");
-		if (!e)
-			break;
-
-                if (!strcmp(e->d_name, ".") || !strcmp(e->d_name, ".."))
-			continue;
-
-		snprintf(path, sizeof path,
-			 "/sys/bus/usb/devices/%s/idVendor", e->d_name);
-		if (!read_file(path, buf, sizeof buf))
-			continue;
-
-		ret = sscanf(buf, "%x", &e_vid);
-		if (ret != 1)
-			error("Failed to parse %s", path);
-
-		snprintf(path, sizeof path,
-			 "/sys/bus/usb/devices/%s/idProduct", e->d_name);
-		if (!read_file(path, buf, sizeof buf))
-			continue;
-
-		ret = sscanf(buf, "%x", &e_pid);
-		if (ret != 1)
-			error("Failed to parse %s", path);
-
-		if (e_vid == vid && e_pid == pid) {
-			snprintf(path, sizeof path,
-				 "/sys/bus/usb/devices/%s/busnum", e->d_name);
-			if (!read_file(path, buf, sizeof buf))
-				error("Failed to read %s", path);
-
-			ret = sscanf(buf, "%u", &bus);
-			if (ret != 1)
-				error("Failed to parse %s", path);
-
-			snprintf(path, sizeof path,
-				 "/sys/bus/usb/devices/%s/devnum", e->d_name);
-			if (!read_file(path, buf, sizeof buf))
-				error("Failed to read %s", path);
-
-			ret = sscanf(buf, "%u", &dev);
-			if (ret != 1)
-				error("Failed to parse %s", path);
-
-			snprintf(path, sizeof path,
-				 "/dev/bus/usb/%03u/%03u", bus, dev);
-
-			debug("Found %04x:%04x at %s\n", e_vid, e_pid, path);
-
-			fd = open(path, O_RDWR);
-			syscall_error(fd < 0, "open(%s) failed", path);
-			break;
-		}
-	}
-
-	errno = ENOENT;
-	closedir(d);
-	return fd;
-}
-
-static int handle_urb(int usb_fd, struct usbdevfs_urb* urb, int timeout)
-{
-	int ret;
-	struct usbdevfs_urb* reaped_urb;
-	int retries = 0;
-
-retry:
-        ret = ioctl(usb_fd, USBDEVFS_SUBMITURB, urb);
-        if (ret < 0)
-        	return ret;
-
-	struct pollfd fd = {
-		.fd = usb_fd,
-		.events = POLLOUT,
-	};
-
-	ret = poll(&fd, 1, timeout);
-	if (ret <= 0) {
-		if (ret == 0)
-			errno = ETIMEDOUT;
-
-		int save_errno = errno;
-
-		// on timeout or other poll error, we need to discard and reap the submitted URB
-		ret = ioctl(usb_fd, USBDEVFS_DISCARDURB, urb);
-
-		// even if discard fails, URB may still be reapable, we need to try reaping anyway
-		ret = ioctl(usb_fd, USBDEVFS_REAPURBNDELAY, &reaped_urb);
-
-		// reap must immediately succeed, otherwise this is fatal
-		syscall_error(ret < 0, "USBDEVFS_REAPURBNDELAY failed");
-
-		errno = save_errno;
-		return -1;
-	}
-
-	// hopefully POLLERR means we get some error immediately on reap
-
-	ret = ioctl(usb_fd, USBDEVFS_REAPURB, &reaped_urb);
-        if (ret < 0)
-        	return ret;
-
-	// EPROTO errors are recoverable
-	if (urb->status == -71 && retries < 3) {
-		retries++;
-		goto retry;
-	}
-
-	if (urb->status != 0) {
-		errno = -urb->status;
-		return -1;
-	}
-
-	return 0;
-}