#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();
	};
};