180 lines
4.0 KiB
C
180 lines
4.0 KiB
C
#include <hwo/utils.h>
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include <rb2/regbus.h>
|
|
#include <sys/errno.h>
|
|
|
|
int32_t dbg_rb2_router_ident;
|
|
|
|
|
|
void rb2_route(RB2_TELEGRAM *request) {
|
|
|
|
rb2_telegram_buffer_t *txtele = rb2_pool_pop();
|
|
|
|
if (txtele) {
|
|
list_init( &txtele->list );
|
|
txtele->telegram = *request;
|
|
rb2_route_listed( txtele );
|
|
};
|
|
};
|
|
|
|
void rb2_route_listed(rb2_telegram_buffer_t *txtele) {
|
|
cpustate_t state;
|
|
|
|
spinlock_lock_irq( &(&__rb2_instance)->queues.sl_router, &state );
|
|
list_append( &(txtele->list), (list_t*) &((&__rb2_instance)->queues.router) );
|
|
spinlock_release_irq( &(&__rb2_instance)->queues.sl_router, &state );
|
|
|
|
thread_wake( (&__rb2_instance)->threads.router );
|
|
};
|
|
|
|
|
|
uint8_t rb2_router_handle_ident(rb2_telegram_buffer_t *telegram){
|
|
|
|
if (!telegram->link){
|
|
rb2_pool_push(telegram);
|
|
return -EFAIL;
|
|
};
|
|
|
|
dbg_rb2_router_ident--;
|
|
|
|
telegram->link->identrx = 3;
|
|
|
|
if (telegram->link->ring) {
|
|
if ((&__rb2_instance)->ringmaster) {
|
|
(&__rb2_instance)->axes = telegram->telegram.ax;
|
|
rb2_pool_push(telegram);
|
|
return ESUCCESS;
|
|
} else {
|
|
(&__rb2_instance)->ax = (++telegram->telegram.ax);
|
|
telegram->link->send( telegram->link, telegram );
|
|
return ESUCCESS;
|
|
};
|
|
} else {
|
|
if (telegram->telegram.sender.ax){
|
|
(&__rb2_instance)->ax = telegram->telegram.sender.ax;
|
|
};
|
|
|
|
telegram->link->idmask = telegram->telegram.endpoints;
|
|
rb2_pool_push(telegram);
|
|
return ESUCCESS;
|
|
};
|
|
};
|
|
|
|
uint8_t rb2_router_handle_foreign(rb2_telegram_buffer_t *telegram){
|
|
list_t *i;
|
|
uint8_t temp;
|
|
|
|
for_each_list_entry(i,&((&__rb2_instance)->links.list)){
|
|
RB2_LINK *link = list_entry(i,RB2_LINK,list);
|
|
|
|
if (link->send == NULL){
|
|
continue;
|
|
};
|
|
|
|
// Eigenes Achsensystem ist Ziel...
|
|
if (
|
|
(telegram->telegram.receiver.ax == RB_AX_LOCAL) ||
|
|
(telegram->telegram.receiver.ax == (&__rb2_instance)->ax)
|
|
) {
|
|
|
|
if (link->idmask & (1<<telegram->telegram.receiver.node)) {
|
|
link->send( link, telegram );
|
|
return 0;
|
|
};
|
|
} else { // Fremde Achse
|
|
if (link->ring) {
|
|
if ((&__rb2_instance)->ringmaster && ((&__rb2_instance)->axes < telegram->telegram.receiver.ax) ) {
|
|
break;
|
|
} else {
|
|
link->send( link, telegram );
|
|
return 0;
|
|
};
|
|
} else if (link->idmask & (0x01)) {
|
|
link->send( link, telegram );
|
|
return 0;
|
|
};
|
|
};
|
|
};
|
|
|
|
/* Keine gültige Schnittstelle -> FAIL senden */
|
|
if (telegram->telegram.flags.response) { // FAIL für Response ist unmöglich
|
|
rb2_pool_push(telegram);
|
|
return 0;
|
|
} else {
|
|
temp = telegram->telegram.sender.addr;
|
|
telegram->telegram.sender.addr = telegram->telegram.receiver.addr;
|
|
telegram->telegram.receiver.addr = temp;
|
|
telegram->telegram.flags.response = 1;
|
|
telegram->telegram.flags.fail = 1;
|
|
telegram->telegram.i32 = 0x99880000L | ((telegram->telegram.sender.addr<<8) & 0xFF00) | ((__rb2_instance.ax << 4) & 0xF0) | (rb2_first_id() & 0x0F);
|
|
|
|
rb2_route_listed( telegram );
|
|
return 0;
|
|
};
|
|
};
|
|
|
|
/**
|
|
* @brief Der Router Thread des RegBUS Systems.
|
|
*
|
|
* Wartet auf Telegramme zum weiterleiten und verteilen an die entsprechenden Empfänger (z.B. lokale API, Schnittstellen)
|
|
* @param arg
|
|
* \callgraph
|
|
* \callergraph
|
|
*/
|
|
void rb2_router(void *arg) {
|
|
list_t *i,*tmp;
|
|
cpustate_t state;
|
|
int n;
|
|
|
|
while (1)
|
|
{
|
|
yield();
|
|
|
|
rb2_loop_ident();
|
|
|
|
yield();
|
|
|
|
rb2_loop_rx();
|
|
|
|
spinlock_lock_irq( &(&__rb2_instance)->queues.sl_router, &state );
|
|
|
|
n = 0;
|
|
|
|
for_each_list_entry_save(i,tmp,&((&__rb2_instance)->queues.router)){
|
|
rb2_telegram_buffer_t *toroute = list_entry(i,rb2_telegram_buffer_t,list);
|
|
list_remove(i);
|
|
|
|
spinlock_release_irq( &(&__rb2_instance)->queues.sl_router, &state );
|
|
|
|
if (toroute->telegram.flags.ident){
|
|
rb2_router_handle_ident(toroute);
|
|
} else if (rb2_request_is_local( &(toroute->telegram) )){
|
|
rb2_api_rx( toroute );
|
|
} else {
|
|
rb2_router_handle_foreign(toroute);
|
|
};
|
|
|
|
spinlock_lock_irq( &(&__rb2_instance)->queues.sl_router, &state );
|
|
|
|
n++;
|
|
if (n>5){
|
|
break;
|
|
};
|
|
};
|
|
|
|
spinlock_release_irq( &(&__rb2_instance)->queues.sl_router, &state );
|
|
|
|
};
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|