forked from haraldwolff/avr-fw-modules
85 lines
2.1 KiB
C
85 lines
2.1 KiB
C
#include <rb2/regbus.h>
|
|
|
|
#include <sys/atomic.h>
|
|
#include <sys/spinlock.h>
|
|
|
|
#include <sys/assert.h>
|
|
#include <sys/errno.h>
|
|
|
|
#include <hwo/utils.h>
|
|
|
|
int rb2_build_request(RB2_REQUEST *request,RB2_ADDR receiver,uint16_t registerno,uint8_t type,void *data){
|
|
request->waiter = current_thread();
|
|
request->telegram.magic = RB2_MAGIC;
|
|
request->telegram.sender.ax = __rb2_instance.ax;
|
|
request->telegram.sender.node = rb2_first_id();
|
|
request->telegram.receiver.addr = receiver.addr;
|
|
request->telegram.regno = registerno;
|
|
request->telegram.flags.byte = 0;
|
|
request->telegram.flags.type = type & 0x0F;
|
|
request->telegram.flags.read = (type & RDT_WRITE) ? 0 : 1;
|
|
|
|
if (type & RDT_WRITE){
|
|
request->telegram.i32 = *(int32_t*)data;
|
|
} else {
|
|
request->telegram.i32 = 0x8899;
|
|
};
|
|
|
|
list_init( &(request->list) );
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int rb2_enqueue_pending(RB2_REQUEST *request){
|
|
cpustate_t state;
|
|
|
|
spinlock_lock_irq( &(&__rb2_instance)->queues.sl_pending, &state );
|
|
list_append( &(request->list), (list_t*)&((&__rb2_instance)->queues.pending) );
|
|
spinlock_release_irq( &(&__rb2_instance)->queues.sl_pending, &state );
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int rb2_unqueue_pending(RB2_REQUEST *request){
|
|
cpustate_t state;
|
|
|
|
spinlock_lock_irq( &(&__rb2_instance)->queues.sl_pending, &state );
|
|
list_remove( &(request->list) );
|
|
spinlock_release_irq( &(&__rb2_instance)->queues.sl_pending, &state );
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
|
|
int rb2_request (RB2_ADDR receiver,uint16_t registerno,uint8_t type,void *data)
|
|
{
|
|
MALLOC_CLEANUP( RB2_REQUEST, request, sizeof(RB2_REQUEST) );
|
|
uint8_t n;
|
|
|
|
for (n=0;n<3;n++) {
|
|
assert( rb2_build_request( request, receiver, registerno, type, data) );
|
|
|
|
{
|
|
assert(rb2_enqueue_pending( request ));
|
|
|
|
rb2_route( &(request->telegram) );
|
|
|
|
wait_ms( RB2_REQUEST_TIMEOUT );
|
|
|
|
assert( rb2_unqueue_pending( request ) );
|
|
};
|
|
|
|
if (
|
|
((type & RDT_WRITE) == 0) &&
|
|
(request->telegram.flags.response)
|
|
) {
|
|
*(int32_t*)data = 0;
|
|
rb2_convert( request->telegram.flags.type, &request->telegram.i32, type, data );
|
|
};
|
|
|
|
if (request->telegram.flags.response && !request->telegram.flags.fail)
|
|
return ESUCCESS;
|
|
};
|
|
return -ETIMEOUT;
|
|
};
|