// simple interface to hal_create_thread()/hal_thread_delete()
// through /proc/rtapi/hal/threadcmd (kernel threadstyles only)
//
// to start a thread, write 'newthread' <threadname> <period> <fp> <cpu>'
// example:
// echo newthread servo-thread 1000000 1 -1 >/proc/rtapi/hal/threadcmd
//
// to delete a thread, write 'delthread <threadname>'
// echo delthread servo-thread >/proc/rtapi/hal/threadcmd
//
// HAL return values are reflected in the return value to write()
//
static int proc_write_threadcmd(struct file *file,
const char *buffer, unsigned long count, void *data)
{
char cmd[20], name[HAL_NAME_LEN + 1];
unsigned long period;
int fp, cpu, retval;
if (!strncmp(buffer,"newthread", 9)) {
if ((retval = sscanf(buffer, "%s %s %lu %d %d",
cmd, name, &period, &fp, &cpu)) != 5) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:newthread: expecting 5 items (s:cmd s:name d:period d:fp d:cpu), got %d\n",
retval);
return -EINVAL;
}
if ((period > 0) &&
(strlen(name) > 0)) {
retval = hal_create_thread(name, period, fp, cpu);
if (retval < 0) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:newthread: could not create thread '%s' - error %d\n",
name, retval);
return retval;
} else {
hal_print_msg(RTAPI_MSG_INFO,
"HAL:newthread: created %ld uS thread '%s' fp=%d cpu=%d\n",
period / 1000, name, fp, cpu);
}
}
} else if (!strncmp(buffer, "delthread", 9)) {
if ((retval = sscanf(buffer, "%s %s", cmd, name)) != 2) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:delthread: expecting 2 items: 'delthread <threadname>'\n");
return -EINVAL;
}
if ((retval = hal_thread_delete(name))) {
hal_print_msg(RTAPI_MSG_ERR,
"HAL:delthread '%s' error %d\n", name, retval);
return retval;
}
hal_print_msg(RTAPI_MSG_INFO,
"HAL:delthread - thread '%s' deleted\n", name);
} else {
hal_print_msg(RTAPI_MSG_ERR,
"HAL: unrecognized threadcmd: '%s'\n", cmd);
return -EINVAL;
}
return count;
}
int _osc_drv_init(void (*handler)(unsigned int ipc),
void (*osc_hisr)(void *arg),
OSC_DRV_INFO *osc_info)
{
hal_queue_t *queue = &osc_info->queue;
hal_thread_t *th = &osc_info->th;
// Initial the Fixed/Overlap regions.
_osc_init();
// Register a user-define handler which is called from OSC exception handler.
register_exception_handler(GE_RESERVED_INST, handler);
// Register a user-define hisr which will be woken up by lisr sending msg to queue.
th->fn = osc_hisr;
th->name = "bh_osc";
th->stack_size = 0x400;
th->arg = queue;
th->prio = osc_hisr_TASK_PRIORITY;
th->task = NULL;
th->ptos = NULL;
// Create a bottom half.
// The bottom half is a thread task with a sync queue.
queue->size = 1;
if(hal_create_queue(queue) == HAL_FAILURE)
return HAL_FAILURE;
if(hal_create_thread(th) != HAL_SUCCESS)
return HAL_FAILURE;
puts("OSC driver init success!\n");
return HAL_SUCCESS;
}
/* init_threads() creates realtime threads, exports functions to
do the realtime control, and adds the functions to the threads.
*/
static int init_threads(void)
{
double base_period_sec, servo_period_sec;
int servo_base_ratio;
int retval;
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() starting...\n");
/* if base_period not specified, assume same as servo_period */
if (base_period_nsec == 0) {
base_period_nsec = servo_period_nsec;
}
if (traj_period_nsec == 0) {
traj_period_nsec = servo_period_nsec;
}
/* servo period must be greater or equal to base period */
if (servo_period_nsec < base_period_nsec) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: bad servo period %ld nsec\n", servo_period_nsec);
return -1;
}
/* convert desired periods to floating point */
base_period_sec = base_period_nsec * 0.000000001;
servo_period_sec = servo_period_nsec * 0.000000001;
/* calculate period ratios, round to nearest integer */
servo_base_ratio = (servo_period_sec / base_period_sec) + 0.5;
/* revise desired periods to be integer multiples of each other */
servo_period_nsec = base_period_nsec * servo_base_ratio;
/* create HAL threads for each period */
/* only create base thread if it is faster than servo thread */
if (servo_base_ratio > 1) {
retval = hal_create_thread("base-thread", base_period_nsec, base_thread_fp, base_cpu);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to create %ld nsec base thread\n",
base_period_nsec);
return -1;
}
}
retval = hal_create_thread("servo-thread", servo_period_nsec, 1, servo_cpu);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to create %ld nsec servo thread\n",
servo_period_nsec);
return -1;
}
/* export realtime functions that do the real work */
retval = hal_export_funct("motion-controller", emcmotController, 0 /* arg
*/ , 1 /* uses_fp */ , 0 /* reentrant */ , mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to export controller function\n");
return -1;
}
retval = hal_export_funct("motion-command-handler", emcmotCommandHandler, 0 /* arg
*/ , 1 /* uses_fp */ , 0 /* reentrant */ , mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to export command handler function\n");
return -1;
}
/*! \todo Another #if 0 */
#if 0
/*! \todo FIXME - currently the traj planner is called from the controller */
/* eventually it will be a separate function */
retval = hal_export_funct("motion-traj-planner", emcmotTrajPlanner, 0 /* arg
*/ , 1 /* uses_fp */ ,
0 /* reentrant */ , mot_comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"MOTION: failed to export traj planner function\n");
return -1;
}
#endif
// if we don't set cycle times based on these guesses, emc doesn't
// start up right
setServoCycleTime(servo_period_nsec * 1e-9);
setTrajCycleTime(traj_period_nsec * 1e-9);
rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() complete\n");
return 0;
}
