#include <stdio.h>
#include <fcntl.h>

const unsigned int b_sda_out = 1<<1;
const unsigned int b_sda_in = 1<<4;
const unsigned int b_sclk = 1<<2;
const unsigned int b_cs = 1<<3;

int gpioouten, gpioout, gpioin;

void init_directions() {
    unsigned int gpio;
    read(gpioouten, &gpio, sizeof(gpio));
    gpio |= b_sda_out | b_sclk | b_cs;
    gpio &= ~b_sda_in;
    write(gpioouten, &gpio, sizeof(gpio));
}

void i2c_setbits(int sda, int sclk) {
    unsigned int gpio;
    read(gpioout, &gpio, sizeof(gpio));
    
    if (sda) // Inverse
        gpio &= ~b_sda_out;
    else
        gpio |= b_sda_out;

    if (sclk)
        gpio |= b_sclk;
    else
        gpio &= ~b_sclk;

    write(gpioout, &gpio, sizeof(gpio));
}

void init_i2c() {
    unsigned int gpio;
    i2c_setbits(1,1);
    
    read(gpioout, &gpio, sizeof(gpio));
    gpio |= b_cs;
    write(gpioout, &gpio, sizeof(gpio));
}

int i2c_getbit() {
    unsigned int gpio;
    read(gpioin, &gpio, sizeof(gpio));
    
    if (gpio & b_sda_in)
        return 1;
    else
        return 0;
}

void i2c_wait() {
    // Slow enough
}

void i2c_writestart() {
    i2c_setbits(1, 1);
    i2c_setbits(0, 1);
    i2c_wait();
    i2c_setbits(0, 0);
    i2c_wait();
}

void i2c_writebit(int bit) {
    i2c_setbits(bit, 0);
    i2c_setbits(bit, 1);
    i2c_wait();
    i2c_setbits(bit, 0);
}

int i2c_readbit() {
    int result;
    
    i2c_setbits(1, 1);
    i2c_wait();
    result = i2c_getbit();
    i2c_setbits(1, 0);
    
    return result;
}

void i2c_writebyte(unsigned char byte) {
    int i;
    for (i=7; i>=0; i--) {
        i2c_writebit((byte >> i) & 0x01);
        i2c_wait();
    }
}

int i2c_ping(int address) {
    i2c_writestart();
    i2c_writebyte((address & 0x7F) << 1);
    return i2c_readbit() ? 0 : 1;
}

int main(int argc, char **argv) {
    gpioouten = open("/dev/gpio/outen",O_RDWR);
    gpioout = open("/dev/gpio/out",O_RDWR);
    gpioin = open("/dev/gpio/in",O_RDWR);

    init_directions();
    init_i2c();
    
    printf("I2C test utility\n");
    
    int address;
    for (address = 0; address <= 0x7F; address++) {
        printf("%02x%c ", address, i2c_ping(address) ? '+' : '-');
        
        if ((address & 0x0F) == 0x0F)
            putchar('\n');
    }

    return 0;
}