/**
 *	servicelink_v2.c	Servicelink Version 2 Implementierung
 * 
 **/

#include <hwo/chksum.h>
#include <hwo/bits.h>

#include <rb2/regbus.h>

#include <sys/errno.h>
#include <sys/assert.h>

#include <usb/usb.h>

#include <usb/servicelink.h>

/**

servicelink nutzt CDC-ACM Profil
Endpunkt 3 für RX (HOST->DEV)
Endpunkt 4 für TX (DEV->HOST)

**/

/* __SL2_ASYNC defines asynchronous servicelink worker */
#define __SL2_ASYNC__

#define SL2_POOL_SIZE	24

#define EP_RX   4
#define EP_TX   3

#define SL2_TIMEOUT		25000L



static sl_v2_telegram_list_t
			*current_rx,
			*current_tx;

static LIST(queue_pool);	// sl_telegram_list_t	Unbenutzt
static LIST(queue_tx);		// sl_telegram_list_t	Wartet auf Senden...
static LIST(queue_pending);	// sl_telegram_list_t	Wartet auf RegBUS Antwort...

int sl2_dbg_free_telegrams(void){
	return list_count( &queue_pool );
};


int sl2_2_rb2(sl_v2_telegram_list_t *tele){
	
#ifdef __SL2_ASYNC__
	uint8_t type;
	
	//usb_device.state.Debugged = usb_device.state.Debugged ? 0 : 1;
	
	
	type =	(tele->sl_telegram.is_float		? RDT_FLOAT : RDT_INT32) |
			(tele->sl_telegram.has_value	? RDT_WRITE : 0);
			
	if (rb2_build_request( &(tele->rb2_request), (RB2_ADDR){ ax: tele->sl_telegram.ax, node:tele->sl_telegram.node}, tele->sl_telegram.register_no, type, &tele->sl_telegram.value.i32) < 0){
		tele->sl_telegram.failed = 1;
		list_append( &(tele->list), &queue_tx );
	} else {
		
		if (rb2_enqueue_pending( &(tele->rb2_request) ) < 0){
			tele->sl_telegram.failed = 1;
			list_append( &(tele->list), &queue_tx );
		} else {
			rb2_route( &(tele->rb2_request.telegram) );
			
			tele->timeout = systick_ticks() + SL2_TIMEOUT;
			list_append( &(tele->list), &queue_pending );
		};
	};
	

#else
	if (tele->sl_telegram.is_float){
		if (tele->sl_telegram.has_value){
			if (rb2_write_float( (RB2_ADDR){ ax: tele->sl_telegram.ax, node:tele->sl_telegram.node}, tele->sl_telegram.register_no, &tele->sl_telegram.value.f32 ) < 0){
				tele->sl_telegram.failed = 1;
			} else {
				tele->sl_telegram.has_value = 0;
			};
		} else {
			if (rb2_read_float( (RB2_ADDR){ ax: tele->sl_telegram.ax, node:tele->sl_telegram.node}, tele->sl_telegram.register_no, &tele->sl_telegram.value.f32 ) < 0){
				tele->sl_telegram.failed = 1;
			} else {
				tele->sl_telegram.has_value = 1;
			};			
		};
		
	} else {
		if (tele->sl_telegram.has_value){
			if (rb2_write_int32( (RB2_ADDR){ ax: tele->sl_telegram.ax, node:tele->sl_telegram.node}, tele->sl_telegram.register_no, &tele->sl_telegram.value.i32 ) < 0){
				tele->sl_telegram.failed = 1;
			} else {
				tele->sl_telegram.has_value = 0;
			};
		} else {
			if (rb2_read_int32( (RB2_ADDR){ ax: tele->sl_telegram.ax, node:tele->sl_telegram.node}, tele->sl_telegram.register_no, &tele->sl_telegram.value.i32 ) < 0){
				tele->sl_telegram.failed = 1;
			} else {
				tele->sl_telegram.has_value = 1;
			};			
		};
	};
	
	list_append( &(tele->list), &queue_tx );
#endif
		
	return ESUCCESS;
};

#ifdef __SL2_ASYNC__

void sl2_async_check(void){
	list_t	*l,*t;
	sl_v2_telegram_list_t
			*tele;

	for_each_list_entry_save(l, t, &queue_pending){
		tele = list_entry( l, sl_v2_telegram_list_t, list );
		
		if (tele->rb2_request.telegram.flags.fail){
			
			rb2_unqueue_pending( &(tele->rb2_request) );
			list_remove( &(tele->list) );
			
			tele->sl_telegram.failed = 1;
			tele->sl_telegram.value.i32 = tele->rb2_request.telegram.i32;
			
			list_append( &(tele->list), &queue_tx );
			
		} else if (tele->rb2_request.telegram.flags.response) {
			
			rb2_unqueue_pending( &(tele->rb2_request) );
			list_remove( &(tele->list) );
			
			tele->sl_telegram.has_value = !tele->sl_telegram.has_value;
			if (tele->sl_telegram.has_value){
				tele->sl_telegram.value.i32 = tele->rb2_request.telegram.i32;
			};			
			
			list_append( &(tele->list), &queue_tx );
			
		} else if (tele->timeout < systick_ticks()) {
			
			rb2_unqueue_pending( &(tele->rb2_request) );
			list_remove( &(tele->list) );
			
			tele->sl_telegram.failed = 1;
			tele->sl_telegram.timeout = 1;
			
			list_append( &(tele->list), &queue_tx );
			
		};
	};
};
 
#endif


void servicelink2(void *_arg) {
	sl_v2_telegram_list_t
				*tele;
	list_t		*l;
				
	int i;

	thread_set_priority( NULL, TP_LOW );
				
	for (i=0;i < SL2_POOL_SIZE;i++){
		tele = malloc( sizeof( sl_v2_telegram_list_t ) );
		if (tele){
			memset( tele, 0x00, sizeof( sl_v2_telegram_list_t) );
			list_append( &(tele->list), &queue_pool );
		};
	};
				
	while (1){
		
		while (usb_device_has_configuration(0)){
			
			if (current_rx){
				list_remove( &(current_rx->list) );
				list_append( &(current_rx->list), &queue_pool );
				current_rx = NULL;
			};
			if (current_tx){
				list_remove( &(current_tx->list) );
				list_append( &(current_tx->list), &queue_pool );
				current_tx = NULL;
			};
			
			wait_ms(100);
		};
		
		wait_ms(1);
		
		if (usb_endpoint_check_buffer( EP_RX ) <= 0) {
			usb_endpoint_unset_buffer( EP_RX );
			
			if (current_rx){
				sl2_2_rb2( current_rx );
				current_rx = NULL;
			};
			if (!current_rx){
				l = list_fetch_first( &queue_pool );
				if (l){
					current_rx = list_entry( l, sl_v2_telegram_list_t, list );
					usb_endpoint_set_buffer( EP_RX, &(current_rx->sl_telegram), sizeof( sl_v2_telegram_t ) );
					current_rx->timeout = systick_ticks() + SL2_TIMEOUT;
				} else {
				};
			};
		} else {
			if (current_rx && (current_rx->timeout < systick_ticks())){	// Reset Endpoint Buffer for resync...
				usb_endpoint_set_buffer( EP_RX, &(current_rx->sl_telegram), sizeof( sl_v2_telegram_t ) );
				current_rx->timeout = systick_ticks() + SL2_TIMEOUT;
			};
		};
		
#ifdef __SL2_ASYNC__

		sl2_async_check();

#endif


		if (usb_endpoint_check_buffer( EP_TX ) <= 0) {
			if (current_tx){
				list_append( &(current_tx->list), &queue_pool );
				current_tx = NULL;
			};
			if (!current_tx){
				l = list_fetch_first( &queue_tx );
				if (l){
					current_tx = list_entry( l, sl_v2_telegram_list_t, list );
					usb_endpoint_set_buffer( EP_TX, &(current_tx->sl_telegram), sizeof( sl_v2_telegram_t ) );
				} else {
					usb_endpoint_unset_buffer( EP_TX );
				};
			};
		};
		
	};
};