177 lines
3.8 KiB
C
Executable File
177 lines
3.8 KiB
C
Executable File
#include <rb2/regbus.h>
|
|
#include <hwo/threads.h>
|
|
|
|
#include <can/can.h>
|
|
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
|
|
#if 0
|
|
|
|
int32_t dbg_rb2_can_tx;
|
|
int32_t dbg_rb2_can_rx;
|
|
int32_t dbg_rb2_can_rx_err;
|
|
int16_t dbg_rb2_can_rx_ptr[2];
|
|
|
|
RB2_CAN_LINK *rb2_can_link;
|
|
|
|
void rb2_can_prepare_rx(CANREQUEST *can);
|
|
void rb2_can_irq_tx(CANREQUEST *request);
|
|
|
|
rb2_telegram_buffer_t* rb2_can_receive (RB2_LINK *link);
|
|
uint8_t rb2_can_send (RB2_LINK *link,rb2_telegram_buffer_t *telegram);
|
|
|
|
void rb2_can_irq_rx(CANREQUEST *request);
|
|
|
|
RB2_LINK* rb2_create_can_link(void) {
|
|
if (rb2_can_link)
|
|
return &(rb2_can_link->link);
|
|
|
|
rb2_can_link = malloc( sizeof( RB2_CAN_LINK ) );
|
|
|
|
if (rb2_can_link) {
|
|
uint8_t n;
|
|
|
|
memset( rb2_can_link, 0x00, sizeof( RB2_CAN_LINK ) );
|
|
|
|
rb2_can_link->link.receive = NULL;
|
|
rb2_can_link->link.send = rb2_can_send;
|
|
|
|
for (n=0;n<2;n++) {
|
|
rb2_can_link->canrx[n] = malloc( sizeof(CANREQUEST) );
|
|
if (rb2_can_link->canrx[n]) {
|
|
rb2_can_prepare_rx( rb2_can_link->canrx[n] );
|
|
can_enqueue( rb2_can_link->canrx[n] );
|
|
};
|
|
};
|
|
|
|
list_append( &(rb2_can_link->link.list), &((&__rb2_instance)->links.list) );
|
|
|
|
return &(rb2_can_link->link);
|
|
};
|
|
return NULL;
|
|
};
|
|
|
|
void rb2_can_prepare_rx(CANREQUEST *can) {
|
|
if (can) {
|
|
memset( can, 0x00, sizeof( CANREQUEST ) );
|
|
|
|
can->ide = 1;
|
|
can->idmask.id = 0x1FFF0000L;
|
|
can->id.id = 0x00080000L;
|
|
can->irq_notify = rb2_can_irq_rx;
|
|
};
|
|
};
|
|
|
|
uint8_t rb2_can_send (RB2_LINK *link,rb2_telegram_buffer_t *telegram) {
|
|
CANREQUEST *can = malloc( sizeof(CANREQUEST) );
|
|
uint8_t n;
|
|
|
|
if (can) {
|
|
memset( can, 0x00, sizeof( CANREQUEST ) );
|
|
|
|
can->ide = 1;
|
|
can->tx = 1;
|
|
can->id.id = 0x00080000L |
|
|
( ((uint32_t)(&__rb2_instance)->ax) << 12) |
|
|
( ((uint32_t)rb2_first_id()) << 8) |
|
|
telegram->telegram.flags.byte;
|
|
|
|
can->len = 8;
|
|
can->data.bytes[0] = telegram->telegram.receiver.addr;
|
|
can->data.bytes[1] = telegram->telegram.sender.addr;
|
|
can->data.words[1] = telegram->telegram.regno;
|
|
can->data.dwords[1] = telegram->telegram.ui32;
|
|
|
|
if (telegram->telegram.flags.ident) {
|
|
for (n=0;n<(RB2_CANLINK_IDDEPTH-1);n++){
|
|
RB2_CAN_LINK(link)->idmasks[ n ] = RB2_CAN_LINK(link)->idmasks[ n + 1 ];
|
|
};
|
|
RB2_CAN_LINK(link)->idmasks[n] = 0;
|
|
};
|
|
|
|
can->irq_notify = rb2_can_irq_tx;
|
|
|
|
if (can_enqueue( can )){
|
|
free( can );
|
|
return -1;
|
|
} else {
|
|
if (_dbg_trace_rb2_can_tx)
|
|
_dbg_trace_rb2_can_tx( telegram );
|
|
};
|
|
};
|
|
free(telegram);
|
|
return 0;
|
|
};
|
|
|
|
void rb2_can_irq_tx(CANREQUEST *request){
|
|
dbg_rb2_can_tx++;
|
|
|
|
free(request);
|
|
};
|
|
|
|
void rb2_can_irq_rx(CANREQUEST *request){
|
|
RB2_CAN_LINK *clink = rb2_can_link;
|
|
rb2_telegram_buffer_t *t = NULL;
|
|
uint8_t n;
|
|
|
|
dbg_rb2_can_rx_ptr[ 1 ] = dbg_rb2_can_rx_ptr[ 0 ];
|
|
dbg_rb2_can_rx_ptr[ 0 ] = (int16_t)request;
|
|
|
|
if (!request->err) {
|
|
|
|
t = malloc( sizeof(rb2_telegram_buffer_t) );
|
|
|
|
if (t) {
|
|
memset( t, 0x00, sizeof(rb2_telegram_buffer_t) );
|
|
|
|
t->telegram.flags.byte = request->id.id & 0x000000FF;
|
|
t->telegram.receiver.addr = request->data.bytes[0];
|
|
t->telegram.sender.addr = request->data.bytes[1];
|
|
t->telegram.regno = request->data.words[1];
|
|
t->telegram.ui32 = request->data.dwords[1];
|
|
|
|
if (t->telegram.flags.ident) {
|
|
for (n=0;n<RB2_CANLINK_IDDEPTH;n++){
|
|
clink->idmasks[n] |= t->telegram.endpoints;
|
|
};
|
|
t->telegram.endpoints = clink->idmasks[0];
|
|
} else {
|
|
if (
|
|
((&__rb2_instance)->ax == t->telegram.receiver.ax) &&
|
|
(clink->link.idmask & (1<<t->telegram.receiver.node))
|
|
){
|
|
free( t );
|
|
t = NULL;
|
|
};
|
|
};
|
|
|
|
if (t){
|
|
dbg_rb2_can_rx++;
|
|
|
|
t->link = &(rb2_can_link->link);
|
|
|
|
if (_dbg_trace_rb2_can_rx)
|
|
_dbg_trace_rb2_can_rx( t );
|
|
|
|
rb2_route_listed( t );
|
|
};
|
|
};
|
|
} else {
|
|
dbg_rb2_can_rx_err++;
|
|
};
|
|
|
|
rb2_can_prepare_rx( request );
|
|
|
|
if (!can_enqueue( request )){
|
|
|
|
};
|
|
};
|
|
|
|
|
|
#endif
|
|
|
|
|