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; }