138 lines
2.7 KiB
C
Executable File
138 lines
2.7 KiB
C
Executable File
#include <can/cannode.h>
|
|
#include <can/can.h>
|
|
|
|
#include <sys/errno.h>
|
|
#include <sys/assert.h>
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
cannode_t* can_node_create(int identity,register_node_proc node_proc){
|
|
cannode_t* cn = malloc(sizeof(cannode_t));
|
|
if (cn){
|
|
memset(cn,0x00,sizeof(cannode_t));
|
|
|
|
cn->identity = identity & 0x0F;
|
|
cn->node_proc = node_proc;
|
|
cn->rxfilter = (canfilter_t){
|
|
id: {
|
|
addr: ((int32_t)cn->identity) << 20,
|
|
ide: 1
|
|
},
|
|
mask: {
|
|
addr: 0x1FFF0000l,
|
|
ide: 1
|
|
}
|
|
};
|
|
};
|
|
return cn;
|
|
};
|
|
|
|
int can_node_received(cannode_t *cn,canframe_t *frame){
|
|
unsigned int regno = frame->id.addr & 0xffff;
|
|
|
|
if (frame->id.rtr){
|
|
|
|
if (!cn->node_proc( RNPOP_READ, regno, &(frame->payload.bytes[4]), &(frame->payload.dwords[0]) )){
|
|
|
|
frame->id.rtr = 0;
|
|
frame->flags.len = 5;
|
|
|
|
assert( can_send( frame ) );
|
|
|
|
};
|
|
} else if (frame->flags.len == 2){
|
|
int i;
|
|
|
|
for (i=regno;i < (regno+frame->payload.words[0]); i++){
|
|
|
|
if (!cn->node_proc( RNPOP_READ, regno, &(frame->payload.bytes[4]), &(frame->payload.dwords[0]) )){
|
|
|
|
frame->id.rtr = 0;
|
|
frame->id.addr = (((int32_t)cn->identity) << 20)|(regno);
|
|
frame->flags.len = 5;
|
|
|
|
assert( can_send( frame ) );
|
|
};
|
|
};
|
|
} else if (frame->flags.len == 6){
|
|
cn->node_proc( RNPOP_WRITE, regno, &(frame->payload.bytes[4]), &(frame->payload.dwords[0]) );
|
|
|
|
frame->id.rtr = 0;
|
|
frame->flags.len = 5;
|
|
|
|
assert( can_send( frame ) );
|
|
};
|
|
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int can_node_handler(cannode_t *cn){
|
|
canframe_t frame;
|
|
|
|
if (cn == NULL){
|
|
return -ENULLPTR;
|
|
};
|
|
|
|
while (!can_recv(&(cn->rxfilter),&frame)){
|
|
assert( can_node_received( cn, &frame ) );
|
|
};
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
|
|
int can_node_read (int node,unsigned int reg,uint8_t *type,void *buffer){
|
|
canframe_t frame;
|
|
canfilter_t f;
|
|
|
|
node &= 0x0f;
|
|
|
|
memset( &frame, 0x00, sizeof(frame) );
|
|
frame.id.addr = (((uint32_t)node)<<20) | (reg);
|
|
frame.id.ide = 1;
|
|
frame.id.rtr = 1;
|
|
frame.flags.len = 1;
|
|
frame.flags.tx = 1;
|
|
|
|
assert( can_send( &frame ) );
|
|
|
|
f = (canfilter_t){
|
|
id: {
|
|
addr: (((int32_t)node) << 20) | (reg),
|
|
ide: 1
|
|
},
|
|
mask: {
|
|
addr: 0x1FFFFFFFl,
|
|
ide: 1,
|
|
rtr: 1
|
|
}
|
|
};
|
|
|
|
*(int32_t*)buffer = 0;
|
|
|
|
assert( can_recv_timeout( &f, &frame, 10 ));
|
|
|
|
*(int32_t*)buffer = frame.payload.dwords[0];
|
|
*type = frame.payload.bytes[4];
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int can_node_write (int node,unsigned int reg,uint8_t type,void *buffer){
|
|
canframe_t frame;
|
|
|
|
memset( &frame, 0x00, sizeof(frame) );
|
|
frame.id.addr = (((int32_t)node)<<20) | (reg);
|
|
frame.id.ide = 1;
|
|
frame.id.rtr = 0;
|
|
frame.flags.len = 6;
|
|
frame.payload.dwords[0] = *(uint32_t*)buffer;
|
|
frame.payload.bytes[4] = type;
|
|
|
|
can_send( &frame );
|
|
|
|
return ESUCCESS;
|
|
};
|