/* Callback function (from the CAN ISR()).
Parse the Msg based on the msg id.
#define ID_MARK_MOTOR_STOP 0x0040 1 or 2 in data[0]
Set Stop 1 (Mark the current motor position as Stop 1 - angle given)
#define ID_MOVE_TO_ANGLE 0x0042 Instance goes with intended Receiver
#define ID_MOVE_SPEED 0x0043
#define ID_SET_MAX_MOTOR_ACCEL 0x0044 */
void can_file_message( sCAN* mMsg )
{
if ( id_match( mMsg->id, create_CAN_eid(ID_MARK_MOTOR_STOP, MyInstance)) )
{
can_proc_set_stop_msg( mMsg );
// Save EEPROM
save_cal( ); // defined in pot.c
}
else if ( id_match(mMsg->id, create_CAN_eid(ID_MOVE_TO_ANGLE, MyInstance)) )
{
// includes a speed
can_proc_move_to_angle_msg( mMsg );
// Initiate Motor
}
/* else if ( id_match(mMsg->id, create_CAN_eid(ID_MOVE_SPEED, MyInstance)) )
{
can_proc_move_speed_msg( mMsg );
} */
else if ( id_match(mMsg->id, create_CAN_eid(ID_SET_MAX_MOTOR_ACCEL, MyInstance)) )
{
can_proc_max_acceleration_msg( mMsg );
}
else if ( (isConfigured(MODE_TILT_RESPONDER)) &&
(id_match(mMsg->id, create_CAN_eid(ID_ACCEL_XYZ, 0)) ) )
{
can_proc_tilt_msg( mMsg );
}
}
/************************************************************
INCOMING CAN MESSAGE Callback :
Call back function from the CAN ISR().
Since each board has a unique 8 bit identifier (Instance number)
The last 8 bits of the LED message identifiers below will
specify the board which board the LED is intended for. Since this
filtering will likely be in hardware, we don't need to check this
in the CAN software callback function.
************************************************************/
BOOL can_board_msg_responder( struct sCAN* mMsg )
{
if ( id_match( mMsg->id, create_CAN_eid (ID_SYSTEM_LED_REQUEST, 0)) )
{
can_parse_led_request( mMsg ); // in leds.c
return TRUE;
}
else if ( id_match( mMsg->id, create_CAN_eid (ID_BOARD_PRESENCE_REQUEST, 0)) )
{
can_parse_board_presence_request( mMsg );
return TRUE;
}
else if ( match( mMsg->id, create_CAN_eid (ID_SYSTEM_CONFIGURE, MyInstance)) )
{
can_parse_configure_request( mMsg ); // in configuration.c
return TRUE;
}
else if ( id_match( mMsg->id, create_CAN_eid (ID_SYSTEM_SHUT_DOWN, 0)) )
{
ShutDownState = SHUTDOWN_PENDING;
return TRUE;
}
else if ( id_match( mMsg->id, create_CAN_eid (ID_INSTANCE_CLAIM, 0)) )
{
// Abort any ID_INSTANCE_CLAIM, TX pending.
// unless it already was sucessfully transmitted.
if (Confirmed == CLAIM_PENDING)
{
// This code is part of the CAN ISR(). So the CANPAGE is already
// directed at the Receive buffer. We want to terminate the Transmit
// which is on a different MOB.
// ABORT TRANSMIT MOB FOR ID_INSTANCE_CLAIM :
int8_t savecanpage;
//savecanpage = CANPAGE; // Save current MOB
//CANPAGE = (INSTANCE_TX_MOB<<4); // Selects MOB with highest priority interrupt
// Can_mob_abort(); //
//CANPAGE = savecanpage; // Restore original MOB
Confirmed = NOT_CLAIMED;
can_process_instance_request( mMsg ); // bumps the MyInstance
// MyInstance updated in process function below.
} else if (Confirmed == NOT_CLAIMED )
can_process_instance_request( mMsg ); // bumps the MyInstance
return TRUE;
}
return FALSE;
}
bool connectionDeleteInstance(conn_type type, conn_id id)
{
conn_instance* curr = conn_list_head;
conn_instance* prev = curr;
while(curr != NULL)
{
if(id_match(curr, type, id))
{
/* Check for empty connection list */
if (!conn_list_head->next)
{
/* Connection list is empty set the ptr to null */
conn_list_head = NULL;
}
else
{
/* If we're removing the first item in the list update the list ptr */
if (curr == conn_list_head)
conn_list_head = curr->next;
else
prev->next = curr->next;
}
free(curr);
return TRUE;
}
prev = curr;
curr = curr->next;
}
return FALSE;
}
char FindMailBox( tID mID )
{
int i;
for (i=0; i<MAX_CAN_MSG_MEMORY_SIZE; i++)
if ( id_match(mID, Received[i].id) )
return i;
return -1;
}
conn_instance* connectionGetInstance(conn_type type, conn_id id)
{
conn_instance* curr = conn_list_head;
while(curr != NULL)
{
if(id_match(curr, type, id))
{
break;
}
curr = curr->next;
}
return curr;
}
static int
match(const struct sk_buff *skb,
const struct net_device *in,
const struct net_device *out,
const struct xt_match *match,
const void *matchinfo,
int offset,
unsigned int protoff,
int *hotdrop)
{
struct frag_hdr _frag, *fh;
const struct ip6t_frag *fraginfo = matchinfo;
unsigned int ptr;
if (ipv6_find_hdr(skb, &ptr, NEXTHDR_FRAGMENT, NULL) < 0)
return 0;
fh = skb_header_pointer(skb, ptr, sizeof(_frag), &_frag);
if (fh == NULL) {
*hotdrop = 1;
return 0;
}
DEBUGP("INFO %04X ", fh->frag_off);
DEBUGP("OFFSET %04X ", ntohs(fh->frag_off) & ~0x7);
DEBUGP("RES %02X %04X", fh->reserved, ntohs(fh->frag_off) & 0x6);
DEBUGP("MF %04X ", fh->frag_off & htons(IP6_MF));
DEBUGP("ID %u %08X\n", ntohl(fh->identification),
ntohl(fh->identification));
DEBUGP("IPv6 FRAG id %02X ",
(id_match(fraginfo->ids[0], fraginfo->ids[1],
ntohl(fh->identification),
!!(fraginfo->invflags & IP6T_FRAG_INV_IDS))));
DEBUGP("res %02X %02X%04X %02X ",
(fraginfo->flags & IP6T_FRAG_RES), fh->reserved,
ntohs(fh->frag_off) & 0x6,
!((fraginfo->flags & IP6T_FRAG_RES)
&& (fh->reserved || (ntohs(fh->frag_off) & 0x06))));
DEBUGP("first %02X %02X %02X ",
(fraginfo->flags & IP6T_FRAG_FST),
ntohs(fh->frag_off) & ~0x7,
!((fraginfo->flags & IP6T_FRAG_FST)
&& (ntohs(fh->frag_off) & ~0x7)));
DEBUGP("mf %02X %02X %02X ",
(fraginfo->flags & IP6T_FRAG_MF),
ntohs(fh->frag_off) & IP6_MF,
!((fraginfo->flags & IP6T_FRAG_MF)
&& !((ntohs(fh->frag_off) & IP6_MF))));
DEBUGP("last %02X %02X %02X\n",
(fraginfo->flags & IP6T_FRAG_NMF),
ntohs(fh->frag_off) & IP6_MF,
!((fraginfo->flags & IP6T_FRAG_NMF)
&& (ntohs(fh->frag_off) & IP6_MF)));
return (fh != NULL)
&&
(id_match(fraginfo->ids[0], fraginfo->ids[1],
ntohl(fh->identification),
!!(fraginfo->invflags & IP6T_FRAG_INV_IDS)))
&&
!((fraginfo->flags & IP6T_FRAG_RES)
&& (fh->reserved || (ntohs(fh->frag_off) & 0x6)))
&&
!((fraginfo->flags & IP6T_FRAG_FST)
&& (ntohs(fh->frag_off) & ~0x7))
&&
!((fraginfo->flags & IP6T_FRAG_MF)
&& !(ntohs(fh->frag_off) & IP6_MF))
&&
!((fraginfo->flags & IP6T_FRAG_NMF)
&& (ntohs(fh->frag_off) & IP6_MF));
}
static int
match(const struct sk_buff *skb,
const struct net_device *in,
const struct net_device *out,
const void *matchinfo,
int offset,
const void *protohdr,
u_int16_t datalen,
int *hotdrop)
{
struct frag_hdr *frag = NULL;
const struct ip6t_frag *fraginfo = matchinfo;
unsigned int temp;
int len;
u8 nexthdr;
unsigned int ptr;
unsigned int hdrlen = 0;
/* type of the 1st exthdr */
nexthdr = skb->nh.ipv6h->nexthdr;
/* pointer to the 1st exthdr */
ptr = sizeof(struct ipv6hdr);
/* available length */
len = skb->len - ptr;
temp = 0;
while (ipv6_ext_hdr(nexthdr)) {
struct ipv6_opt_hdr *hdr;
DEBUGP("ipv6_frag header iteration \n");
/* Is there enough space for the next ext header? */
if (len < (int)sizeof(struct ipv6_opt_hdr))
return 0;
/* No more exthdr -> evaluate */
if (nexthdr == NEXTHDR_NONE) {
break;
}
/* ESP -> evaluate */
if (nexthdr == NEXTHDR_ESP) {
break;
}
hdr=(struct ipv6_opt_hdr *)(skb->data+ptr);
/* Calculate the header length */
if (nexthdr == NEXTHDR_FRAGMENT) {
hdrlen = 8;
} else if (nexthdr == NEXTHDR_AUTH)
hdrlen = (hdr->hdrlen+2)<<2;
else
hdrlen = ipv6_optlen(hdr);
/* FRAG -> evaluate */
if (nexthdr == NEXTHDR_FRAGMENT) {
temp |= MASK_FRAGMENT;
break;
}
/* set the flag */
switch (nexthdr){
case NEXTHDR_HOP:
case NEXTHDR_ROUTING:
case NEXTHDR_FRAGMENT:
case NEXTHDR_AUTH:
case NEXTHDR_DEST:
break;
default:
DEBUGP("ipv6_frag match: unknown nextheader %u\n",nexthdr);
return 0;
break;
}
nexthdr = hdr->nexthdr;
len -= hdrlen;
ptr += hdrlen;
if ( ptr > skb->len ) {
DEBUGP("ipv6_frag: new pointer too large! \n");
break;
}
}
/* FRAG header not found */
if ( temp != MASK_FRAGMENT ) return 0;
if (len < sizeof(struct frag_hdr)){
*hotdrop = 1;
return 0;
}
frag = (struct frag_hdr *) (skb->data + ptr);
DEBUGP("INFO %04X ", frag->frag_off);
DEBUGP("OFFSET %04X ", ntohs(frag->frag_off) & ~0x7);
DEBUGP("RES %02X %04X", frag->reserved, ntohs(frag->frag_off) & 0x6);
DEBUGP("MF %04X ", frag->frag_off & htons(IP6_MF));
DEBUGP("ID %u %08X\n", ntohl(frag->identification),
ntohl(frag->identification));
DEBUGP("IPv6 FRAG id %02X ",
(id_match(fraginfo->ids[0], fraginfo->ids[1],
ntohl(frag->identification),
!!(fraginfo->invflags & IP6T_FRAG_INV_IDS))));
DEBUGP("res %02X %02X%04X %02X ",
(fraginfo->flags & IP6T_FRAG_RES), frag->reserved,
ntohs(frag->frag_off) & 0x6,
!((fraginfo->flags & IP6T_FRAG_RES)
&& (frag->reserved || (ntohs(frag->frag_off) & 0x6))));
DEBUGP("first %02X %02X %02X ",
(fraginfo->flags & IP6T_FRAG_FST),
ntohs(frag->frag_off) & ~0x7,
!((fraginfo->flags & IP6T_FRAG_FST)
&& (ntohs(frag->frag_off) & ~0x7)));
DEBUGP("mf %02X %02X %02X ",
(fraginfo->flags & IP6T_FRAG_MF),
ntohs(frag->frag_off) & IP6_MF,
!((fraginfo->flags & IP6T_FRAG_MF)
&& !((ntohs(frag->frag_off) & IP6_MF))));
DEBUGP("last %02X %02X %02X\n",
(fraginfo->flags & IP6T_FRAG_NMF),
ntohs(frag->frag_off) & IP6_MF,
!((fraginfo->flags & IP6T_FRAG_NMF)
&& (ntohs(frag->frag_off) & IP6_MF)));
return (frag != NULL)
&&
(id_match(fraginfo->ids[0], fraginfo->ids[1],
ntohl(frag->identification),
!!(fraginfo->invflags & IP6T_FRAG_INV_IDS)))
&&
!((fraginfo->flags & IP6T_FRAG_RES)
&& (frag->reserved || (ntohs(frag->frag_off) & 0x6)))
&&
!((fraginfo->flags & IP6T_FRAG_FST)
&& (ntohs(frag->frag_off) & ~0x7))
&&
!((fraginfo->flags & IP6T_FRAG_MF)
&& !(ntohs(frag->frag_off) & IP6_MF))
&&
!((fraginfo->flags & IP6T_FRAG_NMF)
&& (ntohs(frag->frag_off) & IP6_MF));
}
static bool
frag_mt6(const struct sk_buff *skb, const struct xt_match_param *par)
{
struct frag_hdr _frag;
const struct frag_hdr *fh;
const struct ip6t_frag *fraginfo = par->matchinfo;
unsigned int ptr;
int err;
err = ipv6_find_hdr(skb, &ptr, NEXTHDR_FRAGMENT, NULL);
if (err < 0) {
if (err != -ENOENT)
*par->hotdrop = true;
return false;
}
fh = skb_header_pointer(skb, ptr, sizeof(_frag), &_frag);
if (fh == NULL) {
*par->hotdrop = true;
return false;
}
pr_debug("INFO %04X ", fh->frag_off);
pr_debug("OFFSET %04X ", ntohs(fh->frag_off) & ~0x7);
pr_debug("RES %02X %04X", fh->reserved, ntohs(fh->frag_off) & 0x6);
pr_debug("MF %04X ", fh->frag_off & htons(IP6_MF));
pr_debug("ID %u %08X\n", ntohl(fh->identification),
ntohl(fh->identification));
pr_debug("IPv6 FRAG id %02X ",
id_match(fraginfo->ids[0], fraginfo->ids[1],
ntohl(fh->identification),
!!(fraginfo->invflags & IP6T_FRAG_INV_IDS)));
pr_debug("res %02X %02X%04X %02X ",
fraginfo->flags & IP6T_FRAG_RES, fh->reserved,
ntohs(fh->frag_off) & 0x6,
!((fraginfo->flags & IP6T_FRAG_RES) &&
(fh->reserved || (ntohs(fh->frag_off) & 0x06))));
pr_debug("first %02X %02X %02X ",
fraginfo->flags & IP6T_FRAG_FST,
ntohs(fh->frag_off) & ~0x7,
!((fraginfo->flags & IP6T_FRAG_FST) &&
(ntohs(fh->frag_off) & ~0x7)));
pr_debug("mf %02X %02X %02X ",
fraginfo->flags & IP6T_FRAG_MF,
ntohs(fh->frag_off) & IP6_MF,
!((fraginfo->flags & IP6T_FRAG_MF) &&
!((ntohs(fh->frag_off) & IP6_MF))));
pr_debug("last %02X %02X %02X\n",
fraginfo->flags & IP6T_FRAG_NMF,
ntohs(fh->frag_off) & IP6_MF,
!((fraginfo->flags & IP6T_FRAG_NMF) &&
(ntohs(fh->frag_off) & IP6_MF)));
return (fh != NULL) &&
id_match(fraginfo->ids[0], fraginfo->ids[1],
ntohl(fh->identification),
!!(fraginfo->invflags & IP6T_FRAG_INV_IDS)) &&
!((fraginfo->flags & IP6T_FRAG_RES) &&
(fh->reserved || (ntohs(fh->frag_off) & 0x6))) &&
!((fraginfo->flags & IP6T_FRAG_FST) &&
(ntohs(fh->frag_off) & ~0x7)) &&
!((fraginfo->flags & IP6T_FRAG_MF) &&
!(ntohs(fh->frag_off) & IP6_MF)) &&
!((fraginfo->flags & IP6T_FRAG_NMF) &&
(ntohs(fh->frag_off) & IP6_MF));
}
