#!/usr/bin/env perl 
use strict;

use IO::Socket::INET;
use Data::Dumper;

my $prompt = '> ';

sub my_readline_worker($) {
    my $sock = shift;
    my $ret  = "";
    my $d    = "";

    while (1) {
        return $ret if $sock->read( $d, 1 ) != 1;

        next if $d eq "\n";

        $ret .= $d;

        return $ret if $d eq "\r";
        return $ret if $ret =~ /> $/;

    }

}

sub my_readline($) {
    my $sock = shift;
    my $ret  = my_readline_worker($sock);

    #print $ret."\n";
    return $ret;
}

sub wait_for_prompt($) {
    my $ocd = shift;

    1 while ( my_readline($ocd) ne $prompt );
}

sub open_ocd($) {
    my $addr = shift;
    my $sock = IO::Socket::INET->new($addr);
    wait_for_prompt($sock);
    $sock->printf( "reset halt\n");
    wait_for_prompt($sock);
    return $sock;
}

sub write_reg($$$) {
    my ( $ocd, $r, $v ) = @_;

    $ocd->printf( "mww 0x%08x 0x%08x\n", $r, $v );
    wait_for_prompt($ocd);
}

sub read_reg($$) {
    my ( $ocd, $r ) = @_;
    my $ret;

    $ocd->printf( "mdw 0x%08x\n", $r );

    $ret = my_readline($ocd);
    $ret = my_readline($ocd);

    wait_for_prompt($ocd);

    $ret =~ s/[\r\n\s]//g;

    if ( $ret =~ /0x[0-9A-Fa-f]+:([0-9a-fA-F]+)/ ) {
        return hex($1);
    }

    return undef;
}

sub dir_set($$) {
    my ( $ocd, $v ) = @_;
    write_reg( $ocd, 0x50000518, $v );
}

sub dir_clr($$) {
    my ( $ocd, $v ) = @_;
    write_reg( $ocd, 0x5000051c, $v );
}
sub io_set($$) {
    my ( $ocd, $v ) = @_;
    write_reg( $ocd, 0x50000508, $v );
}

sub io_clr($$) {
    my ( $ocd, $v ) = @_;
    write_reg( $ocd, 0x5000050c, $v );
}

sub io_get($) {
    my $ocd = shift;
    return read_reg( $ocd, 0x50000510 );
}

my $SDABIT = 1 << 12;
my $SCLBIT = 1 << 13;

sub i2c_set($$$) {
    my ( $ocd, $scl, $sda ) = @_;
    my $clr = 0;
    my $set = 0;

    if ($scl) {
        $clr |= $SCLBIT;
    }
    else {
        $set |= $SCLBIT;
    }

    if ($sda) {
        $clr |= $SDABIT;
    }
    else {
        $set |= $SDABIT;
    }

    dir_set( $ocd, $set );
    dir_clr( $ocd, $clr );
}

sub i2c_get($) {
    my $ocd = shift;

    return ( io_get($ocd) & $SDABIT ) ? 1 : 0;
}

sub i2c_start($) {
    my $ocd = shift;

    i2c_set( $ocd, 1, 1 );
    i2c_set( $ocd, 1, 0 );
    i2c_set( $ocd, 0, 0 );

    #print "S";
}

sub i2c_stop($) {
    my $ocd = shift;

    i2c_set( $ocd, 0, 0 );
    i2c_set( $ocd, 1, 0 );
    i2c_set( $ocd, 1, 1 );

    #print "T\n";
}

sub i2c_sendbyte($$) {
    my ( $ocd, $byte ) = @_;

    for ( my $c = 0x80 ; $c ; $c >>= 1 ) {
        my $v = ( $c & $byte ) ? 1 : 0;

        #print $v;

        i2c_set( $ocd, 0, $v );
        i2c_set( $ocd, 1, $v );
        i2c_set( $ocd, 0, $v );
    }

}

sub i2c_recvbyte($) {
    my ($ocd) = @_;

    my $byte = 0;

    for ( my $c = 0x80 ; $c ; $c >>= 1 ) {
        my $v;

        i2c_set( $ocd, 0, 1 );
        i2c_set( $ocd, 1, 1 );
        $v = i2c_get($ocd);

        #print $v;

        $byte |= $c if $v;

        i2c_set( $ocd, 0, 1 );
    }

    return $byte;

}

sub i2c_startaddr($$) {
    my ( $ocd, $addr ) = @_;
    my $ret;

    i2c_start($ocd);
    i2c_sendbyte( $ocd, $addr );

    #print " ";

    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );
    $ret = i2c_get($ocd);
    i2c_set( $ocd, 0, 1 );

    #print $ret;

    return $ret;

}

sub i2c_ping($$) {
    my ( $ocd, $addr ) = @_;
    my $ret;
    $ret = i2c_startaddr( $ocd, $addr );
    i2c_stop($ocd);
    return $ret;
}

my $LIS_ADDR = 0x32;

sub lis_write($$$) {
    my ( $ocd, $reg, $v ) = @_;

    my $ret;

    i2c_startaddr( $ocd, $LIS_ADDR );

    i2c_sendbyte( $ocd, $reg );

    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );
    $ret = i2c_get($ocd);
    i2c_set( $ocd, 0, 1 );

    i2c_sendbyte( $ocd, $v );

    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );
    i2c_set( $ocd, 0, 1 );

    i2c_stop($ocd);
}

sub lis_read($$) {
    my ( $ocd, $reg ) = @_;

    my $ret;

    i2c_startaddr( $ocd, $LIS_ADDR );

    i2c_sendbyte( $ocd, $reg );

    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );
    $ret = i2c_get($ocd);
    i2c_set( $ocd, 0, 1 );

    # mad repeated start
    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );

    i2c_startaddr( $ocd, $LIS_ADDR | 1 );

    $ret = i2c_recvbyte($ocd);

    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );
    i2c_set( $ocd, 0, 1 );

    i2c_stop($ocd);

    return $ret;
}

sub lis_read16($$) {
    my ( $ocd, $reg ) = @_;

    my $ret;

    i2c_startaddr( $ocd, $LIS_ADDR );

    i2c_sendbyte( $ocd, $reg | 0x80 );

    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );
    $ret = i2c_get($ocd);
    i2c_set( $ocd, 0, 1 );

    # mad repeated start
    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );

    i2c_startaddr( $ocd, $LIS_ADDR | 1 );

    $a = i2c_recvbyte($ocd);

    i2c_set( $ocd, 0, 0 );
    i2c_set( $ocd, 1, 0 );
    i2c_set( $ocd, 0, 0 );

    $b = i2c_recvbyte($ocd);

    i2c_set( $ocd, 0, 1 );
    i2c_set( $ocd, 1, 1 );
    i2c_set( $ocd, 0, 1 );

    i2c_stop($ocd);

    $ret = $a | ( $b << 8 );

    return $ret;
}

my $ocd = open_ocd('127.0.0.1:4444');

io_clr( $ocd, $SCLBIT | $SDABIT );

i2c_start($ocd);
i2c_stop($ocd);


lis_write($ocd,0x20,0x27);
lis_write($ocd,0x22,0x0);
printf "%02x\n",lis_read($ocd,0x22);
my $a=io_get($ocd);
lis_write($ocd,0x22,0xfe);
printf "%02x\n",lis_read($ocd,0x22);
my $b=io_get($ocd);

my $v=$a;
printf "%04b %04b  %04b %04b  %04b %04b  %04b %04b\n",
	($v >>28) &0xf,
	($v >>24) &0xf,

	($v >>20) &0xf,
	($v >>16) &0xf,

	($v >>12) &0xf,
	($v >>8) &0xf,

	($v >>4) &0xf,
	($v) &0xf;
$v=$b;


printf "%04b %04b  %04b %04b  %04b %04b  %04b %04b\n",
	($v >>28) &0xf,
	($v >>24) &0xf,

	($v >>20) &0xf,
	($v >>16) &0xf,

	($v >>12) &0xf,
	($v >>8) &0xf,

	($v >>4) &0xf,
	($v) &0xf;


lis_write($ocd,0x22,0x0);


while (1) {
    printf "%04x %04x %04x\n", lis_read16( $ocd, 0x28 ),
      lis_read16( $ocd, 0x2a ), lis_read16( $ocd, 0x2c );

}

die;