static void rtmac_vnic_srq(void)
{
struct rtskb *rtskb;
struct sk_buff *skb;
unsigned hdrlen;
struct net_device_stats *stats;
while (1)
{
rtskb = rtskb_dequeue(&rx_queue);
if (!rtskb)
break;
hdrlen = rtskb->rtdev->hard_header_len;
skb = dev_alloc_skb(hdrlen + rtskb->len + 2);
if (skb) {
skb_reserve(skb, 2); /* Align IP on 16 byte boundaries */
/* copy Ethernet header */
memcpy(skb_put(skb, hdrlen),
rtskb->data - hdrlen - sizeof(struct rtmac_hdr), hdrlen);
/* patch the protocol field in the original Ethernet header */
((struct ethhdr*)skb->data)->h_proto = rtskb->protocol;
/* copy data */
memcpy(skb_put(skb, rtskb->len), rtskb->data, rtskb->len);
skb->dev = &rtskb->rtdev->mac_priv->vnic;
skb->protocol = eth_type_trans(skb, skb->dev);
count2timeval(rtskb->rx, &skb->stamp);
stats = &rtskb->rtdev->mac_priv->vnic_stats;
kfree_rtskb(rtskb);
stats->rx_packets++;
stats->rx_bytes += skb->len;
netif_rx(skb);
}
else {
printk("RTmac: VNIC fails to allocate linux skb\n");
kfree_rtskb(rtskb);
}
}
}
static void* rt_system_thread(void * arg)
{
struct timeval tv;
int64_t ts1, ts2;
SEM * shm_sem;
SEM * sync_sem;
RT_TASK *task;
M3EcSystemShm * sys = (M3EcSystemShm *)arg;
printf("Starting real-time thread\n",0);
RTIME t_last;
int cntr=0;
task = rt_task_init_schmod(nam2num("M3SYSP"), 0, 0, 0, SCHED_FIFO, 0xF);
rt_allow_nonroot_hrt();
if (task==NULL)
{
printf("Failed to create RT-TASK M3SYSP\n",0);
return 0;
}
shm_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3LSHM));
if (!shm_sem)
{
printf("Unable to find the SEMNAM_M3LSHM semaphore.\n",0);
rt_task_delete(task);
return 0;
}
//else
// printf("Allocated shm_sem semaphore %08x \n",shm_sem);
sync_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3SYNC));
if (!sync_sem)
{
printf("Unable to find the SEMNAM_M3SYNC semaphore.\n",0);
rt_task_delete(task);
rt_sem_delete(shm_sem);
return 0;
}
//else
// printf("Allocated sync_sem semaphore %08x \n",sync_sem);
RTIME tick_period = nano2count(RT_TIMER_TICKS_NS);
RTIME now = rt_get_time();
rt_task_make_periodic(task, now + tick_period, tick_period);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
t_last=now;
sys_thread_active=1;
uint64_t tl;
while(!sys_thread_end)
{
rt_sem_wait(sync_sem);
rt_sem_wait(shm_sem);
if (cntr%200==0)
{
now=rt_get_time_ns();
float dt = (now-t_last)/1000000.0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
printf("\n\nM3 Cycle: %d: 200 cycles in %4.3f ms. EC cycles: %d\n", cntr,dt, sys->counter);
printf("DT: timestamp_dt (uS) : %lld\n",(sys->timestamp_ns-tl)/1000);
t_last=now;
SysEcShmPrettyPrint(sys);
}
tl=sys->timestamp_ns;
cntr++;
rt_sem_signal(shm_sem);
rt_task_wait_period();
}
printf("Exiting RealTime Thread...\n",0);
rt_make_soft_real_time();
rt_task_delete(task);
sys_thread_active=0;
return 0;
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
while (1) {
t_last_cycle = get_cycles();
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
rt_sem_signal(&master_sem);
// check process data state (optional)
check_domain1_state();
if (counter) {
counter--;
} else {
u32 c;
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
c = EC_READ_U32(domain1_pd + off_counter_in);
if (counter_value != c) {
counter_value = c;
printk(KERN_INFO PFX "counter=%u\n", counter_value);
}
}
if (blink_counter) {
blink_counter--;
} else {
blink_counter = 9;
// calculate new process data
blink = !blink;
}
// write process data
for (i = 0; i < NUM_DIG_OUT; i++) {
EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);
}
EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 9;
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
// while (deactive!=20) {
while (1) {
t_last_cycle = get_cycles();
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
rt_sem_signal(&master_sem);
// check process data state (optional)
//check_domain1_state();
inpu[0]=EC_READ_U16(domain1_pd + status_word);
inpu[1]=EC_READ_U32(domain1_pd + pos_act);
// if(servooff==1){//servo off
// if(stop==1){
// if( ( inpu[0] == 0x1637 ) && ( inpu[2] == 0x1637 ) ){
// EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
// EC_WRITE_U16(domain1_pd+ctrl_word2, 0x0006 );
// }
// else if( ( inpu[0] == 0x0650 ) && ( inpu[2] == 0x0650 ) ){
// printk(KERN_INFO PFX "want to PREOP");
// deactive++;
// }
// }
if( (inpu[0]&0x004f) == 0x0040 ){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( (inpu[0]&0x006f) == 0x0021){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
}
else if( (inpu[0]&0x006f) == 0x0023){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
EC_WRITE_S32(domain1_pd+tar_pos, 0);
EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
}
else if( (inpu[0]&0x006f) == 0x0027){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
EC_WRITE_S32(domain1_pd+tar_pos , value ); //for mode 8 no sin
if(value==180000){
speedup=0;
speeddown=1;
//printk(KERN_INFO PFX "top");
value=value-1;
}
else if(speeddown==1 && value!=0){
value=value-1;
//printk(KERN_INFO PFX "slow down");
}
else if(speeddown==1 && value==0){
speedup=0;
speeddown=0;
// stop=1;
//printk(KERN_INFO PFX "stop");
}
else if(!stop){
speedup=1;
speeddown=0;
value=value+1;
//printk(KERN_INFO PFX "fast up ");
}
// change++;
// }
// else
// change = 0;
}
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; //original = 9
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
void run(long data)
{
int i;
struct timeval tv;
unsigned int sync_ref_counter = 0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
// while (deactive!=20) {
while (1) {
t_last_cycle = get_cycles();
/*
if ( (inpu[0]==0x0650) &&(stop==1) )
break;
*/
// receive process data
rt_sem_wait(&master_sem);
ecrt_master_receive(master);
ecrt_domain_process(domain1);
ecrt_domain_process(domain2);
rt_sem_signal(&master_sem);
// check process data state (optional)
//check_domain1_state();
inpu[0]=EC_READ_U16(domain2_pd + status_word);
inpu[1]=EC_READ_U32(domain2_pd + actual_pos);
/*
if (counter) {
counter--;
} else {
u32 c;
counter = FREQUENCY;
// check for master state (optional)
check_master_state();
c = EC_READ_U32(domain1_pd + off_counter_in);
if (counter_value != c) {
counter_value = c;
printk(KERN_INFO PFX "counter=%u\n", counter_value);
}
}
*/
/*
if (blink_counter) {
blink_counter--;
} else {
blink_counter = 9;
// calculate new process data
blink = !blink;
}
*/
// write process data
/*
for (i = 0; i < NUM_DIG_OUT; i++) {
EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99);
}
EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02);
*/
// if(servooff==1){//servo off
if(stop==1){
if ( inpu[0] == 0x1637 ) {
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( inpu[0] == 0x0650 ){
//++deactive ;
//EC_WRITE_U16(domain1_pd+alstat, 0x0002 );
printk(KERN_INFO PFX "want to PREOP");
deactive++;
}
/*
else{
EC_WRITE_U16(domain1_pd+alstat, 0x0002 );
printk(KERN_INFO PFX "want to PREOP");
break;
}
*/
}
else if( (inpu[0]&0x0040) == 0x0040){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 );
}
else if( (inpu[0]&0x006f) == 0x0021 ){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 );
}
else if( (inpu[0]&0x027f) == 0x0233){
EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f);
EC_WRITE_S32(domain1_pd+interpolateddata, 0);
//EC_WRITE_S32(domain1_pd+tar_velo, 0xffffff);
EC_WRITE_S32(domain1_pd+max_torq, 0xf00);
EC_WRITE_S32(domain1_pd+modeofoper, 8);
}
else if( (inpu[0]&0x027f) == 0x0237){
//if(change >= 0 && change<2 ){
if( change<1 ){
//start=1;
//if(i==0){
//EC_WRITE_S32(domain1_pd+interpolateddata , 0 );
//EC_WRITE_S32(domain1_pd+interpolateddata2 , 0 );
//EC_WRITE_S32(domain1_pd+target_pos , 0 );
//EC_WRITE_S32(domain1_pd+target_pos2 , 0 );
//EC_WRITE_S32(domain1_pd+tar_velo , 0 );
//EC_WRITE_S32(domain1_pd+tar_velo2 , 0 );
//}
//else {
//EC_WRITE_S32(domain1_pd+interpolateddata , (sin(i)*180000) ); //for mode 7
//EC_WRITE_S32(domain1_pd+interpolateddata2 , (sin(i)*180000) );
//EC_WRITE_S32(domain1_pd+target_pos , (sin(i)*180000) ); //for mode 8 with sin
//EC_WRITE_S32(domain1_pd+target_pos2 , (sin(i)*180000) );
EC_WRITE_S32(domain1_pd+target_pos , inpu[7] ); //for mode 8 no sin
//EC_WRITE_S32(domain1_pd+tar_velo , 500000 ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , 500000 );
//if(1){
if(inpu[7]==1800000){
speedup=0;
speeddown=1;
//printk(KERN_INFO PFX "top");
inpu[7]=inpu[7]-200;
}
else if(speeddown==1 && inpu[7]!=0){
inpu[7]=inpu[7]-200;
//printk(KERN_INFO PFX "slow down");
}
else if(speeddown==1 && inpu[7]==0){
speedup=0;
speeddown=0;
stop=1;
//printk(KERN_INFO PFX "stop");
}
else if(!stop){
speedup=1;
speeddown=0;
inpu[7]=inpu[7]+2000;
//printk(KERN_INFO PFX "fast up ");
}
/*
if(speedup==1)
inpu[7]+=500;
else if(speeddown==1)
inpu[7]-=1000;
else{
inpu[7]=0;
servooff=1;
}
*/
//EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] );
//}
//else{
//EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9
//EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] );
//}
//}
EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f);
change++;
/*
if(datacount<10001){
data[datacount][0]=(sin(i)*360000);
data[datacount][1]=inpu[1];
data[datacount][2]=inpu[3];
data[datacount][3]=(inpu[1] - inpu[3]);
datacount++;
}
*/
}
else
change = 0;
}
//printk(KERN_INFO PFX "pos1=%d pos2=%d inpu7=%d\n",inpu[1],inpu[3],inpu[7]);
rt_sem_wait(&master_sem);
tv.tv_usec += 1000;
if (tv.tv_usec >= 1000000) {
tv.tv_usec -= 1000000;
tv.tv_sec++;
}
ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv));
if (sync_ref_counter) {
sync_ref_counter--;
} else {
sync_ref_counter = 1; //original = 9
ecrt_master_sync_reference_clock(master);
}
ecrt_master_sync_slave_clocks(master);
ecrt_domain_queue(domain1);
ecrt_domain_queue(domain2);
ecrt_master_send(master);
rt_sem_signal(&master_sem);
rt_task_wait_period();
}
}
