xMega und TWI / I2C

zur Info: zwischenzeitlich konnte ich den Lesezugriff über folgende Funktion realisieren.
Kritik ist Willkommen!

var twitimeout : SysTimer8;

{-------------------------------------------------------------------------------
TWICombinedRead
input parameters:
DEVADR : 7-bit I2C Device Address
subaddress : subaddress
pdata : location for the read data
bytesToRead : bytes to read
-------------------------------------------------------------------------------}
function twiCombinedRead (
DEVADR : byte;
subaddress : Word;
pData : pointer;
bytesToRead : byte
) : boolean;
begin


SetSysTimer(twitimeout,2); //

// Reset TWIE
TWIEMASTERCTRLA := $00;
TWIEMASTERCTRLA := $08;
TWIEMASTERSTATUS := $FD; // clear all flags, force bus state = idle;

// write I2C address Byte +RW = 0
TWIEMASTERADDR := DEVADR shl 1;
repeat
until Bit(TWIEMASTERSTATUS, 6) or isSysTimerZero(twitimeout); // wif?
if TWIEMASTERSTATUS <> $62 then // return, if incorrect STATUS
Return (false);
endif;

// write subaddress
TWIEMASTERDATA := HI(subaddress); // write highbyte subaddress
repeat
until Bit(TWIEMASTERSTATUS, 6) or isSysTimerZero(twitimeout); // wif?
if TWIEMASTERSTATUS <> $62 then // return, if incorrect STATUS
Return (false);
endif;
TWIEMASTERDATA := LO(subaddress); // write lowbyte subaddress
repeat
until Bit(TWIEMASTERSTATUS, 6) or isSysTimerZero(twitimeout); // wif
if TWIEMASTERSTATUS <> $62 then // return, if incorrect STATUS
Return (false);
endif;

// bytesToRead?
if bytesToRead = 0 then
TWIEMASTERCTRLC := $07; // stop
Return(true);
endif;

// read first byte (read access with repeated start condition)
TWIEMASTERADDR := (DEVADR shl 1) or $01; // write I2C address + R/W = 1
repeat
until Bit(TWIEMASTERSTATUS, 7) or isSysTimerZero(twitimeout); // rif?
if TWIEMASTERSTATUS <> $A2 then // return, if incorrect STATUS
Return (false);
endif;
pData^++ := TWIEMASTERDATA;
dec(bytesToRead);

// read next bytes
while bytesToRead > 0 do
TWIEMASTERCTRLC := $02; // ack + read more byte
repeat
until Bit(TWIEMASTERSTATUS, 7) or isSysTimerZero(twitimeout); // rif?
if TWIEMASTERSTATUS <> $A2 then // return, if incorrect STATUS
Return (false);
endif;
pData^++ := TWIEMASTERDATA;
dec(bytesToRead);
endwhile;

// send stop condition
TWIEMASTERCTRLC := $07;
Return(true);
end;