static void read_port(void *arg, long period)
{
int n;
for (n = 0; n < npins; n++) {
if ((~dir_map & RTAPI_BIT(n)) && (~exclude_map & RTAPI_BIT(n)))
*port_data[n] = bcm2835_gpio_lev(gpios[n]);
}
}
static void write_port(void *arg, long period)
{
int n;
for (n = 0; n < npins; n++) {
if (exclude_map & RTAPI_BIT(n))
continue;
if (dir_map & RTAPI_BIT(n)) {
if (*(port_data[n])) {
bcm2835_gpio_set(gpios[n]);
} else {
bcm2835_gpio_clr(gpios[n]);
}
}
}
}
int rtapi_app_main(void)
{
int n, retval = 0;
int rev, pinno;
char *endptr;
if ((rev = rpi_revision()) < 0)
return -1;
switch (rev) {
case 3:
rtapi_print_msg(RTAPI_MSG_INFO,
"Raspberry Model B Revision 1.0 + ECN0001 (no fuses, D14 removed)\n");
pins = rev1_pins;
gpios = rev1_gpios;
npins = sizeof(rev1_pins);
break;
case 2:
rtapi_print_msg(RTAPI_MSG_INFO,
"Raspberry Model B Revision 1.0\n");
pins = rev1_pins;
gpios = rev1_gpios;
npins = sizeof(rev1_pins);
break;
case 4:
case 5:
case 6:
rtapi_print_msg(RTAPI_MSG_INFO,
"Raspberry Model B Revision 2.0\n");
pins = rev2_pins;
gpios = rev2_gpios;
npins = sizeof(rev2_pins);
break;
default:
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: board revision %d not supported\n", rev);
return -1;
}
port_data = hal_malloc(npins * sizeof(void *));
if (port_data == 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: hal_malloc() failed\n");
hal_exit(comp_id);
return -1;
}
if (dir == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: no config string\n");
return -1;
}
dir_map = strtoul(dir, &endptr,0);
if (*endptr) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: dir=%s - trailing garbage: '%s'\n",
dir, endptr);
return -1;
}
if (exclude == 0) {
rtapi_print_msg(RTAPI_MSG_ERR, "HAL_GPIO: ERROR: no exclude string\n");
return -1;
}
exclude_map = strtoul(exclude, &endptr,0);
if (*endptr) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: exclude=%s - trailing garbage: '%s'\n",
exclude, endptr);
return -1;
}
if (setup_gpio_access())
return -1;
comp_id = hal_init("hal_gpio");
if (comp_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: hal_init() failed\n");
return -1;
}
for (n = 0; n < npins; n++) {
if (exclude_map & RTAPI_BIT(n))
continue;
pinno = pins[n];
if (dir_map & RTAPI_BIT(n)) {
bcm2835_gpio_fsel(gpios[n], BCM2835_GPIO_FSEL_OUTP);
if ((retval = hal_pin_bit_newf(HAL_IN, &port_data[n],
comp_id, "hal_gpio.pin-%02d-out", pinno)) < 0)
break;
} else {
bcm2835_gpio_fsel(gpios[n], BCM2835_GPIO_FSEL_INPT);
if ((retval = hal_pin_bit_newf(HAL_OUT, &port_data[n],
comp_id, "hal_gpio.pin-%02d-in", pinno)) < 0)
break;
}
}
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: pin %d export failed with err=%i\n",
n,retval);
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("hal_gpio.write", write_port, 0,
0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: write funct export failed\n");
hal_exit(comp_id);
return -1;
}
retval = hal_export_funct("hal_gpio.read", read_port, 0,
0, 0, comp_id);
if (retval < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"HAL_GPIO: ERROR: read funct export failed\n");
hal_exit(comp_id);
return -1;
}
rtapi_print_msg(RTAPI_MSG_INFO,
"HAL_GPIO: installed driver\n");
hal_ready(comp_id);
return 0;
}
