349 lines
5.8 KiB
C
349 lines
5.8 KiB
C
#include <sys/assert.h>
|
|
#include <sys/atomic.h>
|
|
#include <sys/errno.h>
|
|
|
|
#include <can/can.h>
|
|
#include <can/jetcan.h>
|
|
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
|
|
#include <avr/eeprom.h>
|
|
|
|
#include <sys/runtime.h>
|
|
|
|
int32_t _dbg_jetcan_requests,
|
|
_dbg_jetcan_misses,
|
|
_dbg_jetcan_timeouts,
|
|
_dbg_jetcan_rtt,
|
|
_dbg_jetcan_rtt_min = 0x7FFFFFFFl,
|
|
_dbg_jetcan_rtt_max;
|
|
|
|
|
|
struct _jetcan jetcan;
|
|
|
|
static canfilter_t
|
|
cf_jetcan_reply =
|
|
{
|
|
id: {
|
|
addr: 0x100
|
|
},
|
|
mask: {
|
|
addr: 0x7FF,
|
|
ide: 1
|
|
}
|
|
};
|
|
|
|
#define IS_1MBIT ((can_get_device_config()&CAN_SPEED_MASK)==CAN_1MBIT)
|
|
#define IS_125KBIT ((can_get_device_config()&CAN_SPEED_MASK)==CAN_125KBIT)
|
|
|
|
|
|
int jcan_lock(void){
|
|
mutex_lock( &jetcan.lock );
|
|
return ESUCCESS;
|
|
};
|
|
int jcan_unlock(void){
|
|
mutex_release( &jetcan.lock );
|
|
return ESUCCESS;
|
|
};
|
|
|
|
JSlave* jcan_slave(uint8_t num)
|
|
{
|
|
if (num < jetcan.nSlaves){
|
|
return &jetcan.slaves[num];
|
|
};
|
|
return NULL;
|
|
};
|
|
|
|
int jcan_num_slaves(void)
|
|
{
|
|
return jetcan.nSlaves;
|
|
};
|
|
|
|
|
|
int jcan_setup_bus(jSetCModeCallback cmodecallback)
|
|
{
|
|
canfilter_t rxfilter = {
|
|
id: {
|
|
addr: 0x160
|
|
},
|
|
mask: {
|
|
addr: 0x7FF,
|
|
ide: 1
|
|
}
|
|
};
|
|
|
|
canframe_t *mid_frame,
|
|
*stp_frame;
|
|
|
|
mid_frame = malloc( sizeof(canframe_t) );
|
|
if (!mid_frame){
|
|
return -ENOMEM;
|
|
};
|
|
stp_frame = malloc( sizeof(canframe_t) );
|
|
if (!stp_frame){
|
|
free(mid_frame);
|
|
return -ENOMEM;
|
|
};
|
|
|
|
jcan_lock();
|
|
|
|
jetcan.nSlaves = 0;
|
|
|
|
cmodecallback(0x00);
|
|
wait_ms(250);
|
|
|
|
if (IS_1MBIT){
|
|
cmodecallback(0x01);
|
|
} else if (IS_125KBIT){
|
|
cmodecallback(0x02);
|
|
};
|
|
|
|
do
|
|
{
|
|
if (!can_recv_timeout(&rxfilter, stp_frame, 100)){
|
|
|
|
memset( &jetcan.slaves[jetcan.nSlaves],
|
|
0x00,
|
|
sizeof(JSlave));
|
|
|
|
jetcan.slaves[ jetcan.nSlaves ].refID = (0x0161 + jetcan.nSlaves);
|
|
jetcan.slaves[ jetcan.nSlaves ].modcode = stp_frame->payload.bytes[0];
|
|
|
|
*mid_frame = (canframe_t){
|
|
flags: {
|
|
tx: 1,
|
|
len: 2
|
|
},
|
|
id: {
|
|
addr: 0x0161,
|
|
},
|
|
payload: {
|
|
words: {
|
|
jetcan.slaves[ jetcan.nSlaves ].refID,
|
|
0,
|
|
0,
|
|
0
|
|
}
|
|
}
|
|
};
|
|
|
|
can_send( mid_frame );
|
|
|
|
jetcan.nSlaves++;
|
|
|
|
continue;
|
|
};
|
|
|
|
} while (0);
|
|
|
|
cmodecallback(0x03);
|
|
|
|
jcan_unlock();
|
|
|
|
free( mid_frame );
|
|
free( stp_frame );
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int jReadRegister32Ex(uint8_t slave,uint16_t parm,void *pValue,uint8_t *type)
|
|
{
|
|
JSlave *sl;
|
|
uint32_t *pui32 = (uint32_t*)pValue;
|
|
int retries = 3;
|
|
canframe_t req,reply;
|
|
systick_t st_start;
|
|
|
|
MUTEXED( &(jetcan.lock) );
|
|
|
|
if (!parm){
|
|
return -EPARAM;
|
|
};
|
|
|
|
_dbg_jetcan_requests++;
|
|
st_start = systick_ticks();
|
|
|
|
*pui32 = 0;
|
|
|
|
while (!can_recv(&cf_jetcan_reply, &reply));
|
|
|
|
while (retries--){
|
|
sl = jcan_slave(slave);
|
|
if (sl){
|
|
|
|
req = (canframe_t){
|
|
flags: { tx: 1, len: 3 },
|
|
id: { addr: sl->refID },
|
|
payload: {
|
|
words: {
|
|
parm,
|
|
*type
|
|
}
|
|
}
|
|
};
|
|
|
|
can_send( &req );
|
|
|
|
if (can_recv_timeout( &cf_jetcan_reply, &reply, 25 ) == 0){
|
|
*pui32 = reply.payload.dwords[0];
|
|
*type = reply.payload.bytes[4];
|
|
|
|
st_start = systick_ticks() - st_start;
|
|
_dbg_jetcan_rtt += st_start;
|
|
_dbg_jetcan_rtt -= _dbg_jetcan_rtt >> 4;
|
|
|
|
if (_dbg_jetcan_rtt_min > st_start)
|
|
_dbg_jetcan_rtt_min = st_start;
|
|
|
|
if (_dbg_jetcan_rtt_max < st_start)
|
|
_dbg_jetcan_rtt_max = st_start;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
_dbg_jetcan_misses++;
|
|
};
|
|
};
|
|
|
|
_dbg_jetcan_timeouts++;
|
|
|
|
return -EBUSY;
|
|
};
|
|
|
|
int jReadRegister32Exl(uint8_t slave,uint16_t parm,int32_t *value)
|
|
{
|
|
uint8_t type = J_LONG;
|
|
int r;
|
|
|
|
r = jReadRegister32Ex(slave,parm,value,&type);
|
|
if (r)
|
|
return r;
|
|
|
|
if (type != J_LONG)
|
|
*value = (int32_t)(*(float*)value);
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int jReadRegister32Exd(uint8_t slave,uint16_t parm,float *value)
|
|
{
|
|
uint8_t type = J_FLOAT;
|
|
int r;
|
|
|
|
r = jReadRegister32Ex(slave,parm,value,&type);
|
|
if (r)
|
|
return r;
|
|
|
|
if (type != J_FLOAT)
|
|
*value = (float)*(int32_t*)value;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
|
|
int jSetRegister32Ex(uint8_t slave,uint16_t parm,void *pValue,uint8_t type){
|
|
JSlave *sl;
|
|
uint16_t *pw16 = (uint16_t*)pValue;
|
|
int retries = 3;
|
|
canframe_t req,reply;
|
|
systick_t st_start;
|
|
MUTEXED( &(jetcan.lock) );
|
|
|
|
if (!parm){
|
|
return -EPARAM;
|
|
};
|
|
|
|
_dbg_jetcan_requests++;
|
|
st_start = systick_ticks();
|
|
|
|
while (!can_recv(&cf_jetcan_reply, &reply));
|
|
|
|
while (retries--){
|
|
sl = jcan_slave(slave);
|
|
if (sl){
|
|
|
|
req = (canframe_t){
|
|
flags: { tx: 1, len: 7 },
|
|
id: { addr: sl->refID },
|
|
payload: {
|
|
words: {
|
|
parm,
|
|
pw16[0],
|
|
pw16[1],
|
|
type
|
|
}
|
|
}
|
|
};
|
|
|
|
can_send( &req );
|
|
|
|
if (can_recv_timeout( &cf_jetcan_reply, &reply, 10 ) == 0){
|
|
|
|
st_start = systick_ticks() - st_start;
|
|
_dbg_jetcan_rtt += st_start;
|
|
_dbg_jetcan_rtt -= _dbg_jetcan_rtt >> 4;
|
|
|
|
if (_dbg_jetcan_rtt_min > st_start)
|
|
_dbg_jetcan_rtt_min = st_start;
|
|
|
|
if (_dbg_jetcan_rtt_max < (int32_t)st_start)
|
|
_dbg_jetcan_rtt_max = st_start;
|
|
|
|
return ESUCCESS;
|
|
};
|
|
_dbg_jetcan_misses++;
|
|
};
|
|
};
|
|
|
|
_dbg_jetcan_timeouts++;
|
|
|
|
return -EBUSY;
|
|
};
|
|
|
|
int jSetRegister32Exl(uint8_t slave,uint16_t parm,int32_t value)
|
|
{
|
|
return jSetRegister32Ex(slave,parm,&value,J_LONG);
|
|
};
|
|
int jSetRegister32Exd(uint8_t slave,uint16_t parm,float value)
|
|
{
|
|
return jSetRegister32Ex(slave,parm,&value,J_FLOAT);
|
|
};
|
|
|
|
/* Bit Manipulationen */
|
|
void jSetRegisterBit(uint8_t slave,uint16_t parm,uint8_t bit)
|
|
{
|
|
int32_t v;
|
|
|
|
if (jReadRegister32Exl(slave, parm, &v))
|
|
{
|
|
v |= (1<<bit);
|
|
jSetRegister32Exl(slave,parm,v);
|
|
};
|
|
};
|
|
|
|
void jClearRegisterBit(uint8_t slave,uint16_t parm,uint8_t bit)
|
|
{
|
|
int32_t v;
|
|
|
|
if (jReadRegister32Exl(slave, parm, &v))
|
|
{
|
|
v &= ~(1<<bit);
|
|
jSetRegister32Exl(slave,parm,v);
|
|
};
|
|
};
|
|
|
|
int jReadRegisterBit(uint8_t slave,uint16_t parm,uint8_t bit)
|
|
{
|
|
int32_t v;
|
|
|
|
if (jReadRegister32Exl(slave, parm, &v))
|
|
{
|
|
if (v & (1<<bit))
|
|
return 1;
|
|
else
|
|
return 0;
|
|
};
|
|
return -1;
|
|
};
|
|
|
|
|