forked from haraldwolff/avr-fw-modules
251 lines
6.0 KiB
C
251 lines
6.0 KiB
C
/**
|
|
* 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 );
|
|
};
|
|
};
|
|
};
|
|
|
|
};
|
|
};
|