Added implementation of the time-based-one-time-password algorithm and on-device encryptionHEADmaster

6 files changed, 730 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..c4ab901
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,4 @@
+zig-out/*
+zig-pkg/*
+.zig-cache/*
+docs/*
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..924ff55
--- /dev/null
+++ b/README.md
@@ -0,0 +1,113 @@
+# QMK Time-based one-time password
+This repository implements a binary and a static library linkable to QMK that allow the user to solve TOTP based challenges on any reasonably powerfull microcontroller which includes most comporary keyboards that run QMK.
+
+The keyboard itself stores an encrypted version of the TOTP secrets. The shipped binary must be invoked to decrypt the secrets when the keyboard is plugged in.
+
+## Usage
+Extract the secret from the URL that is given by the authentification service that wishes to establish 2FA (either use an QR reader or directly get the secret if provided). The secret must be base32 (RFC 4648) encoded which is the current standard.
+
+Before compiling anything adapt the `build.zig`, l.14-19 set the correct configuration for your microcontroller in the keyboard. FOR RP2040 the configuration is correct already, if that is not your controller look up the correct model and architecture.
+
+```bash
+# zig version 0.16.0 is needed to compile this code. You can get it at https://ziglang.org/learn/getting-started/
+zig build --release=fast
+zig-out/bin/totp add [SECRET]
+# example output: {KEY,6,pD4DeoiAzCQKkHPjd6O8keDU3Q}
+# Put the output into the keymaps.c as shown in the later example (adapt key)
+cp zig-out/lib/libtotp.a /your_qmk_firmware_path/lib
+```
+
+Put something like this in your keymap.c QMK firmware configuration:
+```c
+// Hook up the external static library 
+extern bool decode_secret_and_generate_2fa(const char* hashed_pw, const char* secret_encoded, uint64_t counter, uint8_t length, char *output);
+extern void sha256(const char *input, char *output);
+
+char hashed_pw[32];
+uint64_t time_since_epoch = 0;
+
+struct TwoFAEntry {
+    char trigger;
+    unsigned int length;
+    const char *encoding;
+};
+// Expand this by all entries needed
+#define NUM_ENTRIES 1
+struct TwoFAEntry entries[NUM_ENTRIES] = {
+    // For example Codeberg
+    {KEY_C,6,pD4DeoiAzCQKkHPjd6O8keDU3Q},
+};
+
+enum custom_keycodes {
+  CKC_TRIGGER_2FA = SAFE_RANGE
+};
+
+bool g_catch_next_key = false;
+
+bool process_record_user(uint16_t keycode, keyrecord_t *record) {
+    // Check when the prefix key was pressed which entry the user wants to access
+    if(g_catch_next_key && record->event.pressed) {
+        g_catch_next_key = false;
+        char output[32];
+        for (int i = 0; i < NUM_ENTRIES; ++i) {
+            if (entries[i].trigger == keycode) {
+                if(decode_secret_and_generate_2fa(hashed_pw, entries[i].encoding,time_since_epoch+(uint64_t)(timer_read32()/1000), entries[i].length, output)) {
+                    SEND_STRING(output);
+                }
+                else {
+                    // Could not decode, the user entered wrong password
+                    SEND_STRING("errx");
+                }
+                // No need to handle the key anymore
+                return false;
+            }
+        }
+        // Unkown key, tell the user (could also be implemented by flashing RBG LEDs, however this is the most compatible)
+        SEND_STRING("k-un");
+        return false;
+    }
+    switch (keycode) {
+        case CKC_TRIGGER_2FA:
+            if (record->event.pressed) {
+                if (time_since_epoch != 0) {
+                    g_catch_next_key = true;
+                }
+                else {
+                    // Tell the user that no password was received to decrypt anything yet
+                    SEND_STRING("pw-m");
+                }
+                return false;
+            }
+        break;
+    }
+
+    return true;
+}
+
+void raw_hid_receive(uint8_t *data, uint8_t length) {
+    // Read time since epoch and the password (store only the hashed version)
+    time_since_epoch = (*((uint64_t*)data)) - (uint64_t)(timer_read32()/1000);
+    sha256((const char*)&data[8], &hashed_pw[0]);
+}
+```
+Do not forget to set `RAW_ENABLE = yes` in `rules.mk` since we need the HID feature.
+
+Compilation/Flashing in QMK can be achieved by prefixing the command with:
+```bash
+EXTRALDFLAGS="-L/absolute_qmk_firmware_path/lib -ltotp" qmk compile ...
+```
+
+After flashing pressing the key `CKC_TRIGGER_2FA` outputs `pw-m`, password missing, to remedy that we need to "unlock" the keyboard by providing the password that was used to encrypt the secrets. Run:
+```bash
+lsusb
+```
+Find the vendor identifier and device identifier on my system the output looks like this:
+```
+Bus 003 Device 013: ID beeb:0002 beekeeb piantor_pro
+```
+The vendor id is `beeb` and the device id `0002`. Now run
+
+```bash
+zig-out/bin/totp hid beeb 0002
+```
+to unlock the keyboard. This must be repeated after every loss of power for the keyboard.
diff --git a/build.zig b/build.zig
new file mode 100644
index 0000000..d97f119
--- /dev/null
+++ b/build.zig
@@ -0,0 +1,240 @@
+const std = @import("std");
+
+// Although this function looks imperative, it does not perform the build
+// directly and instead it mutates the build graph (`b`) that will be then
+// executed by an external runner. The functions in `std.Build` implement a DSL
+// for defining build steps and express dependencies between them, allowing the
+// build runner to parallelize the build automatically (and the cache system to
+// know when a step doesn't need to be re-run).
+pub fn build(b: *std.Build) void {
+    // Standard target options allow the person running `zig build` to choose
+    // what target to build for. Here we do not override the defaults, which
+    // means any target is allowed, and the default is native. Other options
+    // for restricting supported target set are available.
+    const deftarget = b.standardTargetOptions(.{});
+
+    // Standard optimization options allow the person running `zig build` to select
+    // between Debug, ReleaseSafe, ReleaseFast, and ReleaseSmall. Here we do not
+    // set a preferred release mode, allowing the user to decide how to optimize.
+    const optimize = b.standardOptimizeOption(.{});
+    // It's also possible to define more custom flags to toggle optional features
+    // of this build script using `b.option()`. All defined flags (including
+    // target and optimize options) will be listed when running `zig build --help`
+    // in this directory.
+    const targets = &[_]std.Target.Query {
+        .{
+            .cpu_model = .{.explicit = &std.Target.arm.cpu.cortex_m0plus },
+            .cpu_arch = .arm,
+            .abi = .eabi,
+            .os_tag = .freestanding
+        },
+//        .{
+//            .cpu_model = .{.explicit = &std.Target.avr.cpu.atmega32u4 },
+//            .cpu_arch = .avr,
+//            .abi = .none,
+//            .os_tag = .freestanding
+//        }
+    };
+
+    for (targets,&[_][]const u8{"totp"}) |target_query,name| {
+        const target = b.resolveTargetQuery(target_query);
+        const modarm = b.addModule(name, .{
+            // The root source file is the "entry point" of this module. Users of
+            // this module will only be able to access public declarations contained
+            // in this file, which means that if you have declarations that you
+            // intend to expose to consumers that were defined in other files part
+            // of this module, you will have to make sure to re-export them from
+            // the root file.
+            .root_source_file = b.path("src/root.zig"),
+            // Later on we'll use this module as the root module of a test executable
+            // which requires us to specify a target.
+        });
+        const lib = b.addLibrary(.{
+            .name = name,
+            .linkage = .static,
+            .root_module = b.createModule(.{
+                // b.createModule defines a new module just like b.addModule but,
+                // unlike b.addModule, it does not expose the module to consumers of
+                // this package, which is why in this case we don't have to give it a name.
+                .root_source_file = b.path("src/root.zig"),
+                // Target and optimization levels must be explicitly wired in when
+                // defining an executable or library (in the root module), and you
+                // can also hardcode a specific target for an executable or library
+                // definition if desireable (e.g. firmware for embedded devices).
+                .target = target,
+                .optimize = optimize,
+                // List of modules available for import in source files part of the
+                // root module.
+                .imports = &.{
+                    // Here "totp" is the name you will use in your source code to
+                    // import this module (e.g. `@import("totp")`). The name is
+                    // repeated because you are allowed to rename your imports, which
+                    // can be extremely useful in case of collisions (which can happen
+                    // importing modules from different packages).
+                    .{ .name = "totp", .module = modarm },
+                },
+            }),
+        });
+        b.installArtifact(lib);
+    }
+
+    const hidapi_dep = b.dependency("hidapi",.{}); 
+    const hidapi_lib = b.addLibrary(.{
+        .name = "hidapi",
+        .linkage = .static,
+        .root_module = b.createModule(.{
+            .target = deftarget,
+            .optimize = optimize,
+            .link_libc = true,
+        })
+    });
+    hidapi_lib.root_module.addIncludePath(hidapi_dep.path("hidapi"));
+    hidapi_lib.root_module.addCSourceFile(.{
+        .file = hidapi_dep.path("linux/hid.c")
+    });
+    const hid = b.addTranslateC(.{ 
+        .root_source_file = hidapi_dep.path("hidapi/hidapi.h"),
+        .target = deftarget,
+        .optimize = optimize 
+    });
+    b.installArtifact(hidapi_lib);
+    // This creates a module, which represents a collection of source files alongside
+    // some compilation options, such as optimization mode and linked system libraries.
+    // Zig modules are the preferred way of making Zig code available to consumers.
+    // addModule defines a module that we intend to make available for importing
+    // to our consumers. We must give it a name because a Zig package can expose
+    // multiple modules and consumers will need to be able to specify which
+    // module they want to access.
+
+    const mod = b.addModule("totp", .{
+        // The root source file is the "entry point" of this module. Users of
+        // this module will only be able to access public declarations contained
+        // in this file, which means that if you have declarations that you
+        // intend to expose to consumers that were defined in other files part
+        // of this module, you will have to make sure to re-export them from
+        // the root file.
+        .root_source_file = b.path("src/root.zig"),
+        // Later on we'll use this module as the root module of a test executable
+        // which requires us to specify a target.
+        .target = deftarget,
+    });
+
+    // Here we define an executable. An executable needs to have a root module
+    // which needs to expose a `main` function. While we could add a main function
+    // to the module defined above, it's sometimes preferable to split business
+    // logic and the CLI into two separate modules.
+    //
+    // If your goal is to create a Zig library for others to use, consider if
+    // it might benefit from also exposing a CLI tool. A parser library for a
+    // data serialization format could also bundle a CLI syntax checker, for example.
+    //
+    // If instead your goal is to create an executable, consider if users might
+    // be interested in also being able to embed the core functionality of your
+    // program in their own executable in order to avoid the overhead involved in
+    // subprocessing your CLI tool.
+    //
+    // If neither case applies to you, feel free to delete the declaration you
+    // don't need and to put everything under a single module.
+    const exe = b.addExecutable(.{
+        .name = "totp",
+        .root_module = b.createModule(.{
+            // b.createModule defines a new module just like b.addModule but,
+            // unlike b.addModule, it does not expose the module to consumers of
+            // this package, which is why in this case we don't have to give it a name.
+            .root_source_file = b.path("src/main.zig"),
+            // Target and optimization levels must be explicitly wired in when
+            // defining an executable or library (in the root module), and you
+            // can also hardcode a specific target for an executable or library
+            // definition if desireable (e.g. firmware for embedded devices).
+            .target = deftarget,
+            .optimize = optimize,
+            // List of modules available for import in source files part of the
+            // root module.
+            .imports = &.{
+                // Here "totp" is the name you will use in your source code to
+                // import this module (e.g. `@import("totp")`). The name is
+                // repeated because you are allowed to rename your imports, which
+                // can be extremely useful in case of collisions (which can happen
+                // importing modules from different packages).
+                .{ .name = "totp", .module = mod },
+                .{ .name = "hidapi", .module = hid.createModule()}
+            },
+        }),
+    });
+
+    exe.root_module.linkLibrary(hidapi_lib);
+    exe.root_module.linkSystemLibrary("libudev", .{});
+
+
+
+    // This declares intent for the executable to be installed into the
+    // install prefix when running `zig build` (i.e. when executing the default
+    // step). By default the install prefix is `zig-out/` but can be overridden
+    // by passing `--prefix` or `-p`.
+    b.installArtifact(exe);
+
+    // This creates a top level step. Top level steps have a name and can be
+    // invoked by name when running `zig build` (e.g. `zig build run`).
+    // This will evaluate the `run` step rather than the default step.
+    // For a top level step to actually do something, it must depend on other
+    // steps (e.g. a Run step, as we will see in a moment).
+    const run_step = b.step("run", "Run the app");
+
+    // This creates a RunArtifact step in the build graph. A RunArtifact step
+    // invokes an executable compiled by Zig. Steps will only be executed by the
+    // runner if invoked directly by the user (in the case of top level steps)
+    // or if another step depends on it, so it's up to you to define when and
+    // how this Run step will be executed. In our case we want to run it when
+    // the user runs `zig build run`, so we create a dependency link.
+    const run_cmd = b.addRunArtifact(exe);
+    run_step.dependOn(&run_cmd.step);
+
+    // By making the run step depend on the default step, it will be run from the
+    // installation directory rather than directly from within the cache directory.
+    run_cmd.step.dependOn(b.getInstallStep());
+
+    // This allows the user to pass arguments to the application in the build
+    // command itself, like this: `zig build run -- arg1 arg2 etc`
+    if (b.args) |args| {
+        run_cmd.addArgs(args);
+    }
+
+    // Creates an executable that will run `test` blocks from the provided module.
+    // Here `mod` needs to define a target, which is why earlier we made sure to
+    // set the releative field.
+    const mod_tests = b.addTest(.{
+        .root_module = mod,
+    });
+
+    // A run step that will run the test executable.
+    const run_mod_tests = b.addRunArtifact(mod_tests);
+
+    // Creates an executable that will run `test` blocks from the executable's
+    // root module. Note that test executables only test one module at a time,
+    // hence why we have to create two separate ones.
+    const exe_tests = b.addTest(.{
+        .root_module = exe.root_module,
+    });
+
+    // A run step that will run the second test executable.
+    const run_exe_tests = b.addRunArtifact(exe_tests);
+
+    // A top level step for running all tests. dependOn can be called multiple
+    // times and since the two run steps do not depend on one another, this will
+    // make the two of them run in parallel.
+    const test_step = b.step("test", "Run tests");
+    test_step.dependOn(&run_mod_tests.step);
+    test_step.dependOn(&run_exe_tests.step);
+
+    // Just like flags, top level steps are also listed in the `--help` menu.
+    //
+    // The Zig build system is entirely implemented in userland, which means
+    // that it cannot hook into private compiler APIs. All compilation work
+    // orchestrated by the build system will result in other Zig compiler
+    // subcommands being invoked with the right flags defined. You can observe
+    // these invocations when one fails (or you pass a flag to increase
+    // verbosity) to validate assumptions and diagnose problems.
+    //
+    // Lastly, the Zig build system is relatively simple and self-contained,
+    // and reading its source code will allow you to master it.
+}
diff --git a/build.zig.zon b/build.zig.zon
new file mode 100644
index 0000000..bf86b3d
--- /dev/null
+++ b/build.zig.zon
@@ -0,0 +1,48 @@
+.{
+    // This is the default name used by packages depending on this one. For
+    // example, when a user runs `zig fetch --save <url>`, this field is used
+    // as the key in the `dependencies` table. Although the user can choose a
+    // different name, most users will stick with this provided value.
+    //
+    // It is redundant to include "zig" in this name because it is already
+    // within the Zig package namespace.
+    .name = .totp,
+    // This is a [Semantic Version](https://semver.org/).
+    // In a future version of Zig it will be used for package deduplication.
+    .version = "0.0.0",
+    // Together with name, this represents a globally unique package
+    // identifier. This field is generated by the Zig toolchain when the
+    // package is first created, and then *never changes*. This allows
+    // unambiguous detection of one package being an updated version of
+    // another.
+    //
+    // When forking a Zig project, this id should be regenerated (delete the
+    // field and run `zig build`) if the upstream project is still maintained.
+    // Otherwise, the fork is *hostile*, attempting to take control over the
+    // original project's identity. Thus it is recommended to leave the comment
+    // on the following line intact, so that it shows up in code reviews that
+    // modify the field.
+    .fingerprint = 0x9dc4a90460f7bfd5, // Changing this has security and trust implications.
+    // Tracks the earliest Zig version that the package considers to be a
+    // supported use case.
+    .minimum_zig_version = "0.16.0",
+    // This field is optional.
+    // Each dependency must either provide a `url` and `hash`, or a `path`.
+    // `zig build --fetch` can be used to fetch all dependencies of a package, recursively.
+    // Once all dependencies are fetched, `zig build` no longer requires
+    // internet connectivity.
+    .dependencies = .{
+        .hidapi = .{
+            .url = "git+https://github.com/libusb/hidapi.git#ee022f543807ebfa29e4759076cf1ea2ac7ec7ae",
+            .hash = "N-V-__8AALweHgDuiQeXchZ42doW1DBFoRQQShq7lX6tLYtI",
+        },
+    },
+    .paths = .{
+        "build.zig",
+        "build.zig.zon",
+        "src",
+        // For example...
+        //"LICENSE",
+        //"README.md",
+    },
+}
diff --git a/src/main.zig b/src/main.zig
new file mode 100644
index 0000000..4a70d26
--- /dev/null
+++ b/src/main.zig
@@ -0,0 +1,187 @@
+const std = @import("std");
+const builtin = @import("builtin");
+const Io = std.Io;
+
+const totp = @import("totp");
+const hidapi = @import("hidapi");
+
+const CLIError = error {
+    MissingParameterDeviceId,
+    MissingParameterVendorId,
+    MissingKeyParameter,
+    InvalidBase32,
+    PasswordTooLong,
+    UnknowDevice
+};
+
+/// Receives the base32 encoded code decodes it and stores the output in output. Returns the size of output.
+pub fn base32decode(code: []const u8, output: []u8) !usize {
+    var position:usize = 0;
+    var value:u64 = 0;
+    var bits:usize = 0;
+    if (std.mem.findScalar(u8, code, ' ')) |_| {
+        std.log.warn("Key contains whitespace which is not valid base32, ignoring",.{});
+    }
+    for(code,0..) |b,pos| {
+        if (b==' ') {
+            continue;
+        }
+        else if (b>'Z' or b < '2' or (b>'9' and b < 'A')) {
+            std.log.err("The character at position {}: {c} is invalid base32",.{pos,b});
+            return CLIError.InvalidBase32;
+        }
+        const val = if (b >= 'A') b-'A' else (b-'2')+26;
+
+        value = (value<<5) | val;
+        bits += 5;
+        if (bits >= 8) {
+            bits -= 8;
+            output[position] = @as(u8,@intCast((value>>@intCast(bits))&0xff));
+            position+=1;
+        }
+    }
+    return position;
+}
+
+/// Prints the given prompt before asking for user input. reader is assumed to be a reader for stdin.
+/// if with_delim is set to true returns the user input in addition to the final carriage return
+pub fn read_password(prompt: []const u8, reader: *std.Io.Reader, with_delim: bool) ![]u8 {
+    std.debug.print("{s}",.{prompt});
+    const stdin = std.Io.File.stdin();
+    const original_termios = try std.posix.tcgetattr(stdin.handle);
+    var new_termios = original_termios;
+    new_termios.lflag.ECHO = false;
+    try std.posix.tcsetattr(stdin.handle, .FLUSH, new_termios);
+    defer std.posix.tcsetattr(stdin.handle, .FLUSH, original_termios) catch {};
+
+    const password = if (with_delim) try reader.takeDelimiterInclusive('\n') else (try reader.takeDelimiter('\n')).?;
+    std.debug.print("\n",.{});
+    return password;
+}
+
+pub fn main(init: std.process.Init) !void {
+    var iter = init.minimal.args.iterate();
+    _ = iter.next();
+
+    var buffer: [256]u8 = undefined;
+    if(iter.next()) |arg| {
+        const stdin = std.Io.File.stdin();
+        var reader = stdin.reader(init.io, &buffer);
+        if (std.mem.eql(u8,arg, "add")) {
+            var output_buffer: [128]u8 = undefined;
+            const key_enc = if (iter.next()) |key| key else return CLIError.MissingKeyParameter;
+            const key_len = try base32decode(key_enc, &output_buffer);
+            const key = try init.gpa.dupe(u8,output_buffer[0..key_len]);
+            defer init.gpa.free(key);
+            const password = try read_password("Password: ", &reader.interface, false);
+
+            var output: [32]u8 = undefined;
+            std.crypto.hash.sha2.Sha256.hash(password, &output, .{});
+            std.crypto.aead.chacha_poly.XChaCha20Poly1305.encrypt(output_buffer[0..key.len], output_buffer[key.len..][0..16], key, "", totp.nonce, output);
+            const slice = std.base64.standard_no_pad.Encoder.encode(&buffer, output_buffer[0..key.len+16]);
+            std.debug.print("Add this struct entry to the configuration {{KEY,6,{s}}}\n",.{slice});
+        }
+        else if (std.mem.eql(u8,arg, "decode")) {
+            const key_enc = if (iter.next()) |key| key else return CLIError.MissingKeyParameter;
+            const password = try read_password("Password: ", &reader.interface, true);
+            password[password.len-1] = 0;
+            var output: [32]u8 = undefined;
+            _ = totp.decode_secret_and_generate_2fa(password.ptr, key_enc,@as(u64,@intCast(std.Io.Timestamp.now(init.io,.real).toSeconds())), 6, output[0..]);
+            std.debug.print("2FA-Key: {s}\n", .{output[0..6]});
+        }
+        else if (std.mem.eql(u8,arg, "reencode")) {
+            var output_buffer: [128]u8 = undefined;
+            const key_enc = if (iter.next()) |key| key else return CLIError.MissingKeyParameter;
+            const password = try read_password("Old Password: ", &reader.interface, true);
+            password[password.len-1] = 0;
+            totp.sha256(password.ptr, output_buffer[0..]);
+            var secret_2fa: [128]u8 = undefined;
+            const result = totp.decode_2fa_key(key_enc, output_buffer[0..], secret_2fa[0..]);
+            if (result < 0) {
+                std.debug.print("Error decoding\n",.{});
+                return;
+            }
+            const length:usize = @intCast(result);
+            const newpassword = try read_password("New Password: ", &reader.interface, false);
+            var output: [32]u8 = undefined;
+            std.crypto.hash.sha2.Sha256.hash(newpassword, &output, .{});
+            std.crypto.aead.chacha_poly.XChaCha20Poly1305.encrypt(output_buffer[0..length], output_buffer[length..][0..16], secret_2fa[0..length], "", totp.nonce, output);
+            const slice = std.base64.standard_no_pad.Encoder.encode(&buffer, output_buffer[0..length+16]);
+            std.debug.print("Add this struct entry to the configuration {{KEY,6,{s}}}\n",.{slice});
+        }
+        else if(std.mem.eql(u8,arg,"nonce")) {
+            var op = try std.Io.Dir.openFileAbsolute(init.io,"/dev/urandom",.{});
+            var reader_nonce = op.reader(init.io, &buffer);
+            var bytes:[24]u8 = undefined;
+            _ = try reader_nonce.interface.readSliceShort(&bytes);
+            std.debug.print("Nonce {any}\n",.{bytes});
+        }
+        else if(std.mem.eql(u8,arg,"hid")) {
+            const vendorid = if (iter.next()) |vendorid| try std.fmt.parseInt(u16, vendorid, 16) else return CLIError.MissingParameterVendorId;
+            const deviceid = if (iter.next()) |devid| try std.fmt.parseInt(u16, devid, 16) else return CLIError.MissingParameterDeviceId;
+            _ = hidapi.hid_init();
+            defer _ = hidapi.hid_exit();
+            var enumerate = hidapi.hid_enumerate(vendorid, deviceid);
+            while(enumerate) |interface| {
+                if (interface.*.usage == 0x61 and interface.*.usage_page == 0xFF60) {
+                    break;
+                }
+                enumerate = enumerate.*.next;
+            }
+            if(enumerate == null) {
+                std.debug.print("Could not find device.\n", .{});
+                return CLIError.UnknowDevice;
+            }
+            const dev: ?*hidapi.hid_device = hidapi.hid_open_path(enumerate.*.path);
+            std.debug.print("Found device at {s}\n", .{enumerate.*.path});
+            if (dev) |device_handle| {
+                defer hidapi.hid_close(device_handle);
+                var write_buffer: [33]u8 = undefined;
+                // Report ID
+                write_buffer[0] = 0;
+
+                const now:u64 = @intCast(std.Io.Timestamp.now(init.io, .real).toSeconds());
+
+                // Payload
+                inline for(0..8) |x| {
+                    write_buffer[1+x] = @intCast((now>>(x*8))&0xFF);
+                }
+                const password = try read_password("Password: ", &reader.interface, false);
+                // Password length is bounded since HIDAPI report length is bounded
+                if (password.len > 23) {
+                    std.debug.print("Only passwords lengths of at most 23 bytes supported.\n", .{});
+                    return CLIError.PasswordTooLong;
+                }
+
+                @memcpy(write_buffer[9..9+password.len], password);
+                write_buffer[9+password.len] = 0;
+                const res2 = hidapi.hid_write(device_handle, &write_buffer, 33);
+                if(res2 == 33) {
+                        std.debug.print("Set up keyboard.\n", .{});
+                }
+            }
+            else {
+                std.debug.print("Could not open device with vendor id: {x}, device id: {x}\n", .{vendorid, deviceid});
+            }
+        }
+    }
+    else {
+        std.debug.print("Usage: totp [command]\nCommunicates and sets up time base one time passwords for programmable keyboards.\nCommands:\n\tadd key\n\t\tAdds a new base32 encoded key.\n\n\tdecode key\n\t\tDecodes the key and generates one time password based on it.\n\n\tnonce\n\t\tGenerates a new nonce for the symmetric cipher.\n\n\thid vendorid deviceid\n\t\tSets up HID device. IDS are interpreted as base 16 numbers.\n",.{});
+    }
+}
+
+
+test "base32 decode" {
+    var output: [20]u8 = undefined;
+    const encoded = "AABBDDEEFFHH";
+    const size = try base32decode(encoded, &output);
+
+    const encoded_err = "AABBDDEEFFHH--";
+    try std.testing.expectError(CLIError.InvalidBase32,base32decode(encoded_err,&output));
+
+    var output_2: [20]u8 = undefined;
+    const encoded_spaces = "AABBD DEEFF HH";
+    const size2 = try base32decode(encoded_spaces, &output_2);
+    try std.testing.expectEqual(size, size2);
+    try std.testing.expect(std.mem.eql(u8, output[0..size], output_2[0..size2]));
+}
diff --git a/src/root.zig b/src/root.zig
new file mode 100644
index 0000000..e4ff7a8
--- /dev/null
+++ b/src/root.zig
@@ -0,0 +1,138 @@
+const std = @import("std");
+
+// TODO: Must be replaced by each user by his/her own nonce (for example from /dev/urandom).
+pub const nonce = [_]u8{143, 54, 214, 156, 114, 22, 191, 156, 175, 116, 138, 187, 248, 246, 227, 91, 108, 217, 79, 141, 154, 107, 51, 122};
+
+/// input is a zero terminated byte string to be hashed.
+/// output is a buffer of at least 32 bytes
+pub export fn sha256(input: [*c]const u8, output: [*c]u8) void {
+    std.crypto.hash.sha2.Sha256.hash(std.mem.span(input), output[0..32], .{});
+}
+
+
+/// This code never aborts (asserts or throws an exception)
+/// password_hashed is the sha256 hashed password with exactly 32 bytes length
+/// encoded_secret is the base64 encoded encrypted secret for the TOTP
+/// timestamp is the seconds since the unix epoch
+/// length refers to the supposed length of the TOTP key, range 4-10
+/// output must be a buffer of at least 32 bytes, since it is reused throughout this function as buffer
+pub export fn decode_secret_and_generate_2fa(password_hashed: [*c]const u8, encoded_secret: [*c]const u8, timestamp: u64, length: u8, output: [*c]u8) bool {
+    var buffer: [128]u8 = undefined;
+    const result = decode_2fa_key(std.mem.span(encoded_secret),password_hashed[0..32],buffer[0..]);
+    if(result < 0) return false;
+    hotp(buffer[0..@intCast(result)], timestamp/30, length, output[0..32]);
+    return true;
+}
+
+/// This code never aborts (asserts or throws an exception)
+/// encoded_secret is the base64 encoded encrypted secret for TOTP
+/// password is the hashed password of exactly 32 bytes length
+/// output_buffer holds the decrypted TOTP secret
+/// The return value holds the number of bytes written to the output buffer
+pub fn decode_2fa_key(encoded_secret: []const u8, password: []const u8, output_buffer: []u8) i16 {
+    if (password.len != 32) return -1;
+    var buffer: [128]u8 = undefined;
+    const size = std.base64.standard_no_pad.Decoder.calcSizeForSlice(encoded_secret) catch return -2;
+    if (size>buffer.len or size-16 > output_buffer.len) return -5;
+    std.base64.standard_no_pad.Decoder.decode(&buffer, encoded_secret) catch return -3;
+    const decodedb = buffer[0..size];
+
+    std.crypto.aead.chacha_poly.XChaCha20Poly1305.decrypt(output_buffer[0..size-16], decodedb[0..size-16], decodedb[size-16..][0..16].*, "", nonce, password[0..32].*) catch return -4;
+    return @as(i16,@intCast(size))-16;
+}
+
+/// Generates a HOTP key given
+/// key is the plain bytes of the TOTP secret
+/// counter is the counter parameter for HOTP (seconds since epoch/30) for TOTP
+/// length refers to the outputted key length
+/// output_buffer contains the zero terminated code in ASCII format (must have a size of at least 20 bytes)
+pub fn hotp(key: []const u8, counter: u64, length: u8, output_buffer: []u8) void {
+    std.crypto.auth.hmac.HmacSha1.create(output_buffer[0..20], &@as([8]u8,@bitCast(std.mem.nativeToBig(u64,counter))), key);
+    const offset = output_buffer[19] & 0xf;
+    var truncated: [4]u8 = [4]u8{output_buffer[offset],output_buffer[offset+1],output_buffer[offset+2],output_buffer[offset+3]};
+    truncated[0] &= 0x7f;
+        
+    var value = std.mem.nativeToBig(u32,@as(u32,@bitCast(truncated))) % (std.math.pow(u32,10,@intCast(length)));
+    for(1..length+1) |i| {
+        const modulo = value % 10;
+        value = value/10;
+        output_buffer[length-i] = @as(u8,@intCast(modulo))+'0';
+    }
+    output_buffer[length] = 0;
+}
+
+
+
+test "Check decode" {
+    var buffer: [128]u8 = undefined;
+    const payload = [_]u8{143, 54, 214, 156, 114, 22};
+    var output_buffer: [128]u8 = undefined;
+    const testpassword = "Test123";
+    var output: [32]u8 = undefined;
+    std.crypto.hash.sha2.Sha256.hash(testpassword, &output, .{});
+    std.crypto.aead.chacha_poly.XChaCha20Poly1305.encrypt(output_buffer[0..payload.len], output_buffer[payload.len..][0..16], payload[0..], "", nonce, output);
+    const slice = std.base64.standard_no_pad.Encoder.encode(&buffer, output_buffer[0..payload.len+16]);
+    const dupe = try std.testing.allocator.dupeZ(u8, slice);
+    defer std.testing.allocator.free(dupe);
+
+
+    try std.testing.expect(decode_2fa_key(dupe, output[0..], output_buffer[0..])>0);
+    try std.testing.expectEqualStrings(output_buffer[0..payload.len], &payload);
+}
+
+
+test "Check hotp output" {
+    var output_buffer: [128]u8 = undefined;
+    const hello_world:[:0]const u8 = "Hello, world!";
+    hotp(hello_world, 42, 6, output_buffer[0..]); 
+
+    try std.testing.expectEqualStrings(output_buffer[0..6], "439256");
+}
+
+test "Check output size of hotp" {
+    var output_buffer: [128]u8 = undefined;
+    const hello_world:[:0]const u8 = "Hello, world!";
+    for(4..10) |length| {
+        hotp(hello_world, 42, @intCast(length), output_buffer[0..]); 
+
+        try std.testing.expectEqual(std.mem.span(@as([*c]u8, @ptrCast(output_buffer[0..]))).len, length);
+    }
+}
+
+test "Check full pipeline" {
+    var buffer: [128]u8 = undefined;
+    const hello_world:[:0]const u8 = "Hello, world!";
+    var output_buffer: [128]u8 = undefined;
+    const testpassword = "Test123";
+    var output: [32]u8 = undefined;
+    std.crypto.hash.sha2.Sha256.hash(testpassword, &output, .{});
+    std.crypto.aead.chacha_poly.XChaCha20Poly1305.encrypt(output_buffer[0..hello_world.len], output_buffer[hello_world.len..][0..16], hello_world, "", nonce, output);
+    const slice = std.base64.standard_no_pad.Encoder.encode(&buffer, output_buffer[0..hello_world.len+16]);
+    const dupe = try std.testing.allocator.dupeZ(u8, slice);
+    defer std.testing.allocator.free(dupe);
+
+    try std.testing.expect(decode_secret_and_generate_2fa(output[0..], dupe, 42*30, 6, output_buffer[0..]));
+
+    try std.testing.expectEqualStrings(output_buffer[0..6], "439256");
+
+    try std.testing.expect(decode_secret_and_generate_2fa(output[0..], dupe, 43*30, 6, output_buffer[0..]));
+    try std.testing.expectEqualStrings(output_buffer[0..6], "208805");
+}
+
+
+test "Check fail on wrong password" {
+    var buffer: [128]u8 = undefined;
+    const hello_world:[:0]const u8 = "Hello, world!";
+    var output_buffer: [128]u8 = undefined;
+    const testpassword = "Test123";
+    var output: [32]u8 = undefined;
+    std.crypto.hash.sha2.Sha256.hash(testpassword, &output, .{});
+    std.crypto.aead.chacha_poly.XChaCha20Poly1305.encrypt(output_buffer[0..hello_world.len], output_buffer[hello_world.len..][0..16], hello_world, "", nonce, output);
+    const slice = std.base64.standard_no_pad.Encoder.encode(&buffer, output_buffer[0..hello_world.len+16]);
+    const dupe = try std.testing.allocator.dupeZ(u8, slice);
+    defer std.testing.allocator.free(dupe);
+
+    output[10] = ~output[10];
+
+    try std.testing.expect(!decode_secret_and_generate_2fa(output[0..], dupe, 42*30, 6, output_buffer[0..]));
+}