int assure_module_loaded(const char *module)
{
FILE *fd;
char line[100];
int len = strlen(module);
int retval;
fd = fopen("/proc/modules", "r");
if (fd == NULL) {
HPG_ERR("ERROR: cannot read /proc/modules\n");
return -1;
}
while (fgets(line, sizeof(line), fd)) {
if (!strncmp(line, module, len)) {
HPG_DBG("module '%s' already loaded\n", module);
fclose(fd);
return 0;
}
}
fclose(fd);
HPG_DBG("loading module '%s'\n", module);
rtapi_snprintf(line, sizeof(line), "/sbin/modprobe %s", module);
if ((retval = system(line))) {
HPG_ERR("ERROR: executing '%s' %d - %s\n", line, errno, strerror(errno));
return -1;
}
return 0;
}
int fixup_pin(u32 hal_pin) {
int ret = 0;
u32 type, p89, index;
// Original brain-dead pin numbering
if (hal_pin < 192) {
ret = hal_pin;
} else {
type = hal_pin / 100;
p89 = (hal_pin % 1000) / 100 ;
index = hal_pin % 100;
// Fixup index value for P9 pins with two CPU pins attached
if (p89 == 9) {
if ((index == 91) || (index == 92)) {
index = index - 50 + (47 - 41);
} else if (index > 46) {
index = 0;
}
} else if (index > 46) {
index = 0;
}
switch (type) {
case 8 :
ret = p8_pins[index].gpio_pin_num;
break;
case 9 :
ret = p9_pins[index].gpio_pin_num;
break;
case 18 :
ret = p8_pins[index].pruO_pin_num;
break;
case 19 :
ret = p9_pins[index].pruO_pin_num;
break;
case 28 :
ret = p8_pins[index].pruI_pin_num;
break;
case 29 :
ret = p9_pins[index].pruI_pin_num;
break;
default:
ret = 0;
}
if (ret == 0)
HPG_ERR("Unknown pin: %d\n",(int)hal_pin);
if (ret < 0) {
HPG_ERR("Requested pin unavailable: %d\n",(int)hal_pin);
ret = 0;
}
}
return ret;
}
int hpg_encoder_init(hal_pru_generic_t *hpg){
int r,i;
if (hpg->config.num_encoders <= 0)
return 0;
rtapi_print("hpg_encoder_init\n");
// FIXME: Support multiple encoder tasks like so: num_encoders=3,4,2,5
// hpg->encoder.num_instances = hpg->config.num_encoders;
hpg->encoder.num_instances = 1;
// Allocate HAL shared memory for instance state data
hpg->encoder.instance = (hpg_encoder_instance_t *) hal_malloc(sizeof(hpg_encoder_instance_t) * hpg->encoder.num_instances);
if (hpg->encoder.instance == 0) {
HPG_ERR("ERROR: hal_malloc() failed\n");
return -1;
}
rtapi_print("malloc: hpg_encoder_instance_t = %p\n",hpg->encoder.instance);
// Clear memory
memset(hpg->encoder.instance, 0, (sizeof(hpg_encoder_instance_t) * hpg->encoder.num_instances) );
for (i=0; i < hpg->encoder.num_instances; i++) {
// FIXME: Support multiple instances like so: num_encoders=3,4,2,5
hpg->encoder.instance[i].num_channels = hpg->config.num_encoders;
// Allocate HAL shared memory for channel state data
hpg->encoder.instance[i].chan = (hpg_encoder_channel_instance_t *) hal_malloc(sizeof(hpg_encoder_channel_instance_t) * hpg->encoder.instance[i].num_channels);
if (hpg->encoder.instance[i].chan == 0) {
HPG_ERR("ERROR: hal_malloc() failed\n");
return -1;
}
rtapi_print("malloc: hpg_encoder_channel_instance_t = %p\n",hpg->encoder.instance[i].chan);
int len = sizeof(hpg->encoder.instance[i].pru) + (sizeof(PRU_encoder_chan_t) * hpg->encoder.instance[i].num_channels);
hpg->encoder.instance[i].task.addr = pru_malloc(hpg, len);
hpg->encoder.instance[i].pru.task.hdr.mode = eMODE_ENCODER;
hpg->encoder.instance[i].LUT = pru_malloc(hpg, sizeof(Counter_LUT));
pru_task_add(hpg, &(hpg->encoder.instance[i].task));
if ((r = export_encoder(hpg,i)) != 0){
HPG_ERR("ERROR: failed to export encoder %i: %i\n",i,r);
return -1;
}
}
return 0;
}
static void *pruevent_thread(void *arg)
{
int event = (int) arg;
int event_count;
do {
if (prussdrv_pru_wait_event(event, &event_count) < 0)
continue;
HPG_ERR("PRU event %d received\n",event);
prussdrv_pru_clear_event(pru ? PRU1_ARM_INTERRUPT : PRU0_ARM_INTERRUPT);
} while (1);
HPG_ERR("pruevent_thread exiting\n");
return NULL; // silence compiler warning
}
int hpg_pwmgen_init(hal_pru_generic_t *hpg){
int r,i;
if (hpg->config.num_pwmgens <= 0)
return 0;
rtapi_print("hpg_pwm_init\n");
// FIXME: Support multiple PWMs like so: num_pwmgens=3,4,2,5
// hpg->pwmgen.num_instances = hpg->config.num_pwmgens;
hpg->pwmgen.num_instances = 1;
// Allocate HAL shared memory for instance state data
hpg->pwmgen.instance = (hpg_pwmgen_instance_t *) hal_malloc(sizeof(hpg_pwmgen_instance_t) * hpg->pwmgen.num_instances);
if (hpg->pwmgen.instance == 0) {
HPG_ERR("ERROR: hal_malloc() failed\n");
return -1;
}
// Clear memory
memset(hpg->pwmgen.instance, 0, (sizeof(hpg_pwmgen_instance_t) * hpg->pwmgen.num_instances) );
for (i=0; i < hpg->pwmgen.num_instances; i++) {
// FIXME: Support multiple PWMs like so: num_pwmgens=3,4,2,5
hpg->pwmgen.instance[i].num_outputs = hpg->config.num_pwmgens;
// Allocate HAL shared memory for output state data
hpg->pwmgen.instance[i].out = (hpg_pwmgen_output_instance_t *) hal_malloc(sizeof(hpg_pwmgen_output_instance_t) * hpg->pwmgen.instance[i].num_outputs);
if (hpg->pwmgen.instance[i].out == 0) {
HPG_ERR("ERROR: hal_malloc() failed\n");
return -1;
}
int len = sizeof(hpg->pwmgen.instance[i].pru) + (sizeof(PRU_pwm_output_t) * hpg->pwmgen.instance[i].num_outputs);
hpg->pwmgen.instance[i].task.addr = pru_malloc(hpg, len);
hpg->pwmgen.instance[i].pru.task.hdr.mode = eMODE_PWM;
pru_task_add(hpg, &(hpg->pwmgen.instance[i].task));
if ((r = export_pwmgen(hpg,i)) != 0){
HPG_ERR("ERROR: failed to export pwmgen %i: %i\n",i,r);
return -1;
}
}
return 0;
}
void hpg_encoder_read_chan(hal_pru_generic_t *hpg, int instance, int channel) {
u16 reg_count;
s32 reg_count_diff;
s32 prev_rawcounts;
hpg_encoder_instance_t *inst;
hpg_encoder_channel_instance_t *e;
inst = &hpg->encoder.instance[instance];
e = &hpg->encoder.instance[instance].chan[channel];
prev_rawcounts = *e->hal.pin.rawcounts;
// sanity check
if (e->hal.param.scale == 0.0) {
HPG_ERR("encoder.%02d.scale == 0.0, bogus, setting to 1.0\n", instance);
e->hal.param.scale = 1.0;
}
PRU_encoder_chan_t *pruchan = (PRU_encoder_chan_t *) ((u32) hpg->pru_data + (u32) inst->task.addr + sizeof(inst->pru));
e->pru.raw.dword[1] = pruchan[channel].raw.dword[1]; // Encoder count
e->pru.raw.dword[2] = pruchan[channel].raw.dword[2]; // Index count and latched count
HPG_ERR("rawenc:%08x %08x %08x\n", pruchan[channel].raw.dword[0],pruchan[channel].raw.dword[1],pruchan[channel].raw.dword[2]);
//
// figure out current rawcounts accumulated by the driver
//
reg_count = e->pru.hdr.count;
reg_count_diff = (s32)reg_count - (s32)e->prev_reg_count;
if (reg_count_diff > 32768) reg_count_diff -= 65536;
if (reg_count_diff < -32768) reg_count_diff += 65536;
*(e->hal.pin.rawcounts) += reg_count_diff;
*(e->hal.pin.rawlatch) = e->pru.hdr.Z_capture;
*(e->hal.pin.count) += 1;
e->prev_reg_count = reg_count;
}
int export_pwmgen(hal_pru_generic_t *hpg, int i)
{
int r, j;
// HAL values common to all outputs in this instance
r = hal_pin_u32_newf(HAL_IN, &(hpg->pwmgen.instance[i].hal.pin.pwm_period), hpg->config.comp_id, "%s.pwmgen.%02d.pwm_period", hpg->config.halname, i);
if (r != 0) { return r; }
*(hpg->pwmgen.instance[i].hal.pin.pwm_period) = 10000000; // Default to 10 mS period, or 100 Hz
for (j=0; j < hpg->pwmgen.instance[i].num_outputs; j++) {
// Export HAL Pins
r = hal_pin_bit_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.enable), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.enable", hpg->config.halname, i, j);
if (r != 0) {
HPG_ERR("pwmgen %02d out %02d: error adding pin 'enable', aborting\n", i, j);
return r;
}
r = hal_pin_float_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.value), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.value", hpg->config.halname, i, j);
if (r != 0) {
HPG_ERR("pwmgen %02d out %02d: error adding pin 'value', aborting\n", i, j);
return r;
}
r = hal_pin_float_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.scale), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.scale", hpg->config.halname, i, j);
if (r != 0) {
HPG_ERR("pwmgen %02d out %02d: error adding pin 'scale', aborting\n", i, j);
return r;
}
r = hal_pin_u32_newf(HAL_IN, &(hpg->pwmgen.instance[i].out[j].hal.pin.pin), hpg->config.comp_id, "%s.pwmgen.%02d.out.%02d.pin", hpg->config.halname, i, j);
if (r != 0) {
HPG_ERR("pwmgen %02d out %02d: error adding pin 'pin', aborting\n", i, j);
return r;
}
// Initialize HAL Pins
*(hpg->pwmgen.instance[i].out[j].hal.pin.enable) = 0;
*(hpg->pwmgen.instance[i].out[j].hal.pin.value) = 0.0;
*(hpg->pwmgen.instance[i].out[j].hal.pin.pin) = PRU_DEFAULT_PIN;
*(hpg->pwmgen.instance[i].out[j].hal.pin.scale) = 1.0;
}
return 0;
}
int export_pru(hal_pru_generic_t *hpg)
{
int r;
char name[HAL_NAME_LEN + 1];
// Export functions
rtapi_snprintf(name, sizeof(name), "%s.update", halname);
r = hal_export_funct(name, hpg_write, hpg, 1, 0, comp_id);
if (r != 0) {
HPG_ERR("ERROR: function export failed: %s\n", name);
hal_exit(comp_id);
return -1;
}
rtapi_snprintf(name, sizeof(name), "%s.capture-position", halname);
r = hal_export_funct(name, hpg_read, hpg, 1, 0, comp_id);
if (r != 0) {
HPG_ERR("ERROR: function export failed: %s\n", name);
hal_exit(comp_id);
return -1;
}
return 0;
}
int setup_pru(int pru, char *filename, int disabled, hal_pru_generic_t *hpg) {
int retval;
if (event > -1) {
prussdrv_start_irqthread (event, sched_get_priority_max(SCHED_FIFO) - 2,
pruevent_thread, (void *) event);
HPG_ERR("PRU event %d listener started\n",event);
}
// Load and execute binary on PRU
// search for .bin files as passed in and under fw_path
char pru_binpath[PATH_MAX];
// default the .bin filename if not given
if (!strlen(filename))
filename = DEFAULT_CODE;
strcpy(pru_binpath, filename);
struct stat statb;
if (!((stat(pru_binpath, &statb) == 0) &&
S_ISREG(statb.st_mode))) {
// filename not found, prefix fw_path & try that:
strcpy(pru_binpath, fw_path);
strcat(pru_binpath, filename);
if (!((stat(pru_binpath, &statb) == 0) &&
S_ISREG(statb.st_mode))) {
// nyet, complain
getcwd(pru_binpath, sizeof(pru_binpath));
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: cant find %s in %s or %s\n",
modname, filename, pru_binpath, fw_path);
return -ENOENT;
}
}
retval = prussdrv_exec_program (pru, pru_binpath, disabled);
return retval;
}
int export_encoder(hal_pru_generic_t *hpg, int i)
{
char name[HAL_NAME_LEN + 1];
int r, j;
// HAL values common to all channels in this instance
// ...nothing to do here...
// HAL values for individual channels
for (j=0; j < hpg->encoder.instance[i].num_channels; j++) {
// Export HAL Pins
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawcounts", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawcounts), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.rawlatch", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.rawlatch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.count-latched", hpg->config.name, i, j);
r = hal_pin_s32_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.count_latch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.position-latched", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.position_latch), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.velocity", hpg->config.name, i, j);
r = hal_pin_float_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.velocity), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.reset", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.reset), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-enable", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IO, &(hpg->encoder.instance[i].chan[j].hal.pin.index_enable), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-enable", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_enable), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.latch-polarity", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_IN, &(hpg->encoder.instance[i].chan[j].hal.pin.latch_polarity), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.quadrature-error", hpg->config.name, i, j);
r = hal_pin_bit_new(name, HAL_OUT, &(hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding pin '%s', aborting\n", name);
return r;
}
// Export HAL Parameters
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.scale", hpg->config.name, i, j);
r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.scale), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.A-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.A_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.B-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.B_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-pin", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_pin), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.index-mask-invert", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.counter-mode", hpg->config.name, i, j);
r = hal_param_u32_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.counter_mode), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.filter", hpg->config.name, i, j);
r = hal_param_bit_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.filter), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
rtapi_snprintf(name, sizeof(name), "%s.encoder.%02d.chan.%02d.vel-timeout", hpg->config.name, i, j);
r = hal_param_float_new(name, HAL_RW, &(hpg->encoder.instance[i].chan[j].hal.param.vel_timeout), hpg->config.comp_id);
if (r < 0) {
HPG_ERR("error adding param '%s', aborting\n", name);
return r;
}
//
// init the hal objects that need it
//
hpg->encoder.instance[i].chan[j].hal.param.scale = 1.0;
hpg->encoder.instance[i].chan[j].hal.param.index_invert = 0;
hpg->encoder.instance[i].chan[j].hal.param.index_mask = 0;
hpg->encoder.instance[i].chan[j].hal.param.index_mask_invert = 0;
hpg->encoder.instance[i].chan[j].hal.param.counter_mode = 0;
// hpg->encoder.instance[i].chan[j].hal.param.filter = 1;
hpg->encoder.instance[i].chan[j].hal.param.vel_timeout = 0.5;
*hpg->encoder.instance[i].chan[j].hal.pin.rawcounts = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.rawlatch = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.count = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.count_latch = 0;
*hpg->encoder.instance[i].chan[j].hal.pin.position = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.position_latch = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.velocity = 0.0;
*hpg->encoder.instance[i].chan[j].hal.pin.quadrature_error = 0;
hpg->encoder.instance[i].chan[j].zero_offset = 0;
hpg->encoder.instance[i].chan[j].prev_reg_count = 0;
hpg->encoder.instance[i].chan[j].state = HM2_ENCODER_STOPPED;
}
return 0;
}
int pru_init(int pru, char *filename, int disabled, hal_pru_generic_t *hpg) {
int i;
int retval;
if (geteuid()) {
HPG_ERR("ERROR: not running as root - need to 'sudo make setuid'?\n");
return -1;
}
if ((retval = assure_module_loaded(UIO_PRUSS)))
return retval;
rtapi_print("prussdrv_init\n");
// Allocate and initialize memory
prussdrv_init ();
// opens an event out and initializes memory mapping
rtapi_print("prussdrv_open\n");
if (prussdrv_open(event > -1 ? event : PRU_EVTOUT_0) < 0)
return -1;
// expose the driver data, filled in by prussdrv_open
pruss = &prussdrv;
// Map PRU's INTC
rtapi_print("prussdrv_pruintc_init\n");
if (prussdrv_pruintc_init(&pruss_intc_initdata) < 0)
return -1;
// Maps the PRU DRAM memory to input pointer
rtapi_print("prussdrv_map_prumem\n");
if (prussdrv_map_prumem(pru ? PRUSS0_PRU1_DATARAM : PRUSS0_PRU0_DATARAM,
(void **) &pru_data_ram) < 0)
return -1;
rtapi_print("PRU data ram mapped\n");
rtapi_print_msg(RTAPI_MSG_DBG, "%s: PRU data ram mapped at %p\n",
modname, pru_data_ram);
hpg->pru_data = (u32 *) pru_data_ram;
// Zero PRU data memory
for (i = 0; i < 8192/4; i++) {
hpg->pru_data[i] = 0;
}
// Reserve PRU memory for static configuration variables
hpg->pru_stat_addr = PRU_DATA_START;
hpg->pru_data_free = hpg->pru_stat_addr + sizeof(PRU_statics_t);
// Setup PRU globals
hpg->pru_stat.task.hdr.dataX = 0xAB;
hpg->pru_stat.task.hdr.dataY = 0xFE;
hpg->pru_stat.period = pru_period;
hpg->config.pru_period = pru_period;
PRU_statics_t *stat = (PRU_statics_t *) ((u32) hpg->pru_data + (u32) hpg->pru_stat_addr);
*stat = hpg->pru_stat;
return 0;
}
int rtapi_app_main(void)
{
hal_pru_generic_t *hpg;
int retval;
comp_id = hal_init("hal_pru_generic");
if (comp_id < 0) {
HPG_ERR("ERROR: hal_init() failed\n");
return -1;
}
// Allocate HAL shared memory for state data
hpg = hal_malloc(sizeof(hal_pru_generic_t));
if (hpg == 0) {
HPG_ERR("ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
// Clear memory
memset(hpg, 0, sizeof(hal_pru_generic_t));
// Initialize PRU and map PRU data memory
if ((retval = pru_init(pru, prucode, disabled, hpg))) {
HPG_ERR("ERROR: failed to initialize PRU\n");
hal_exit(comp_id);
return -1;
}
// Setup global state
hpg->config.num_pwmgens = num_pwmgens;
hpg->config.num_stepgens = num_stepgens;
hpg->config.num_encoders = num_encoders;
hpg->config.comp_id = comp_id;
hpg->config.pru_period = pru_period;
hpg->config.name = modname;
hpg->config.halname = halname;
rtapi_print("num_pwmgens : %d\n",num_pwmgens);
rtapi_print("num_stepgens: %d\n",num_stepgens);
rtapi_print("num_encoders: %d\n",num_encoders);
rtapi_print("Init pwm\n");
// Initialize various functions and generate PRU data ram contents
if ((retval = hpg_pwmgen_init(hpg))) {
HPG_ERR("ERROR: pwmgen init failed: %d\n", retval);
hal_exit(comp_id);
return -1;
}
rtapi_print("Init stepgen\n");
if ((retval = hpg_stepgen_init(hpg))) {
HPG_ERR("ERROR: stepgen init failed: %d\n", retval);
hal_exit(comp_id);
return -1;
}
rtapi_print("Init encoder\n");
if ((retval = hpg_encoder_init(hpg))) {
HPG_ERR("ERROR: encoder init failed: %d\n", retval);
hal_exit(comp_id);
return -1;
}
if ((retval = hpg_wait_init(hpg))) {
HPG_ERR("ERROR: global task init failed: %d\n", retval);
hal_exit(comp_id);
return -1;
}
if ((retval = export_pru(hpg))) {
HPG_ERR("ERROR: var export failed: %d\n", retval);
hal_exit(comp_id);
return -1;
}
hpg_stepgen_force_write(hpg);
hpg_pwmgen_force_write(hpg);
hpg_encoder_force_write(hpg);
hpg_wait_force_write(hpg);
if ((retval = setup_pru(pru, prucode, disabled, hpg))) {
HPG_ERR("ERROR: failed to initialize PRU\n");
hal_exit(comp_id);
return -1;
}
HPG_INFO("installed\n");
hal_ready(comp_id);
return 0;
}
