forked from haraldwolff/avr-fw-modules
114 lines
2.9 KiB
C
114 lines
2.9 KiB
C
#include <usb/servicelink.h>
|
|
#include <hwo/chksum.h>
|
|
#include <hwo/bits.h>
|
|
#include <rb2/regbus.h>
|
|
#include <avr/eeprom.h>
|
|
|
|
#include <sys/errno.h>
|
|
|
|
#include <usb/usb.h>
|
|
|
|
void servicelink(FILE *stream)
|
|
{
|
|
struct servicelink_telegram telegram;
|
|
avrChksum chk;
|
|
int l;
|
|
|
|
thread_set_priority( NULL, TP_LOW );
|
|
|
|
while (1)
|
|
{
|
|
int ch;
|
|
|
|
ch = fgetc( stream );
|
|
if (ch == SL_MAGIC)
|
|
{
|
|
read(stream,(char*)&telegram.header,sizeof(struct sl_header));
|
|
|
|
if (telegram.header.request & SL_REQ_WRITE)
|
|
read(stream,(char*)&telegram.value, 4);
|
|
else
|
|
telegram.value.i32 = 0;
|
|
|
|
read(stream,(char*)&telegram.chksum,sizeof(telegram.chksum));
|
|
|
|
chksum( &chk, &telegram, sizeof(telegram) - sizeof(avrChksum) );
|
|
|
|
if (chk.combined == telegram.chksum.combined)
|
|
{
|
|
if (telegram.header.request & SL_REQ_WRITE)
|
|
{
|
|
if (telegram.header.request & SL_REQ_INT)
|
|
{
|
|
if (
|
|
rb2_write_int32(
|
|
(RB2_ADDR){ ax: telegram.header.achse, node: telegram.header.knoten },
|
|
telegram.header.register_no,
|
|
&telegram.value.i32 ) == ESUCCESS
|
|
)
|
|
telegram.header.request |= SL_REQ_ACK;
|
|
else
|
|
telegram.header.request |= SL_REQ_FAIL;
|
|
|
|
} else if (telegram.header.request & SL_REQ_FLOAT)
|
|
{
|
|
if (
|
|
rb2_write_float(
|
|
(RB2_ADDR){ ax: telegram.header.achse, node: telegram.header.knoten },
|
|
telegram.header.register_no,
|
|
&telegram.value.f32 ) == ESUCCESS
|
|
)
|
|
telegram.header.request |= SL_REQ_ACK;
|
|
else
|
|
telegram.header.request |= SL_REQ_FAIL;
|
|
};
|
|
};
|
|
|
|
telegram.value.i32 = 0;
|
|
|
|
if (telegram.header.request & SL_REQ_READ)
|
|
{
|
|
if (telegram.header.request & SL_REQ_INT)
|
|
{
|
|
if (
|
|
rb2_read_int32(
|
|
(RB2_ADDR){ ax: telegram.header.achse, node: telegram.header.knoten },
|
|
telegram.header.register_no,
|
|
&telegram.value.i32 ) == ESUCCESS
|
|
)
|
|
telegram.header.request |= SL_REQ_ACK;
|
|
else
|
|
telegram.header.request |= SL_REQ_FAIL;
|
|
|
|
} else if (telegram.header.request & SL_REQ_FLOAT)
|
|
{
|
|
if (
|
|
rb2_read_float(
|
|
(RB2_ADDR){ ax: telegram.header.achse, node: telegram.header.knoten },
|
|
telegram.header.register_no,
|
|
&telegram.value.f32 ) == ESUCCESS
|
|
)
|
|
telegram.header.request |= SL_REQ_ACK;
|
|
else
|
|
telegram.header.request |= SL_REQ_FAIL;
|
|
};
|
|
};
|
|
|
|
l = ((telegram.header.request & (SL_REQ_ACK | SL_REQ_READ)) == (SL_REQ_ACK | SL_REQ_READ) ) ? sizeof(telegram) : sizeof(telegram) - 4;
|
|
chksum( &telegram.chksum, &telegram, l - sizeof(avrChksum) );
|
|
|
|
stream->put( SL_MAGIC, stream );
|
|
write( stream, (char*)&telegram.header, sizeof( telegram.header ) );
|
|
|
|
if ((telegram.header.request & (SL_REQ_ACK | SL_REQ_READ)) == (SL_REQ_ACK | SL_REQ_READ) )
|
|
write( stream, (char*)&telegram.value, 4 );
|
|
|
|
write( stream, (char*)&telegram.chksum, sizeof(avrChksum) );
|
|
};
|
|
|
|
};
|
|
yield();
|
|
};
|
|
};
|
|
|