Writing modules for PREEMPT_RT

dabit@linuxcnc-vm:~/usb_tests$ sudo ./udev_test
Device found at /dev/hidraw1
Packet transmission time since last: 1.838086 msec
Packet transmission time since last: 2.992347 msec
Packet transmission time since last: 2.844372 msec
Packet transmission time since last: 3.039186 msec
Packet transmission time since last: 1.987344 msec
Packet transmission time since last: 1.982596 msec
Packet transmission time since last: 1.055455 msec
Packet transmission time since last: 2.027151 msec
Packet transmission time since last: 1.955042 msec
Packet transmission time since last: 1.839815 msec
Packet transmission time since last: 0.989827 msec
Packet transmission time since last: 0.996477 msec
Packet transmission time since last: 2.967388 msec
Packet transmission time since last: 2.066263 msec
..
..
Packet transmission time since last: 3.011264 msec
Packet transmission time since last: 1.981308 msec
Packet transmission time since last: 2.008413 msec
dabit@linuxcnc-vm:~/usb_tests$ 

#include <linux/input.h>
#include <linux/hidraw.h>
#include <sys/fcntl.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/wait.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <poll.h>

#include <rtapi_slab.h>
#include <rtapi_ctype.h>
#include <rtapi_math64.h>

#include "rtapi.h"
#include "rtapi_app.h"
#include "rtapi_string.h"

#include "hal.h"

MODULE_LICENSE("GPL");
MODULE_AUTHOR("Bart van Hest");
MODULE_DESCRIPTION("Test module");
#ifdef MODULE_SUPPORTED_DEVICE
MODULE_SUPPORTED_DEVICE("test");
#endif

/* USB device to search for */
#define USBDEV_VID 0x0483
#define USBDEV_PID 0x5751

/* USB transmit and receive buffers */
static unsigned char xmitbuf[65], rcvbuf[65];

/* Component id */
static int comp_id;

/* USB device file descriptor */
static int usbfd=-1;

struct __comp_state {
    struct __comp_state *_next;
    hal_float_t *position_cmd_x;
    hal_float_t *position_fb_x;
    hal_float_t maxaccel_x;
    hal_float_t maxvel_x;
    hal_float_t position_scale_x;
};

struct __comp_state *__comp_first_inst=0, *__comp_last_inst=0;

static void _(struct __comp_state *__comp_inst, long period);
static int __comp_get_data_size(void);
#undef TRUE
#define TRUE (1)
#undef FALSE
#define FALSE (0)
#undef true
#define true (1)
#undef false
#define false (0)

static int export(char *prefix, long extra_arg) {
    char buf[HAL_NAME_LEN + 1];
    int r = 0;
    int sz = sizeof(struct __comp_state) + __comp_get_data_size();
    struct __comp_state *inst = hal_malloc(sz);
    memset(inst, 0, sz);
    r = hal_pin_float_newf(HAL_IN, &(inst->position_cmd_x), comp_id,
        "%s.position-cmd-x", prefix);
    if(r != 0) return r;
    r = hal_pin_float_newf(HAL_OUT, &(inst->position_fb_x), comp_id,
        "%s.position-fb-x", prefix);
    if(r != 0) return r;
    r = hal_param_float_newf(HAL_RW, &(inst->maxaccel_x), comp_id,
        "%s.maxaccel-x", prefix);
    if(r != 0) return r;
    r = hal_param_float_newf(HAL_RW, &(inst->maxvel_x), comp_id,
        "%s.maxvel-x", prefix);
    if(r != 0) return r;
    r = hal_param_float_newf(HAL_RW, &(inst->position_scale_x), comp_id,
        "%s.position-scale-x", prefix);
    if(r != 0) return r;
    rtapi_snprintf(buf, sizeof(buf), "%s", prefix);
    r = hal_export_funct(buf, (void(*)(void *inst, long))_, inst, 1, 0, comp_id);
    if(r != 0) return r;
    if(__comp_last_inst) __comp_last_inst->_next = inst;
    __comp_last_inst = inst;
    if(!__comp_first_inst) __comp_first_inst = inst;
    return 0;
}

static int __comp_get_data_size(void) { return 0; }

/* Brute-force open a couple of /dev/hidrawX devices and check if it is our motion controller */
static int FindAndOpenUSBdevice(void) {
	char buf[16];
	int i, fd, res;
	struct hidraw_devinfo info;
	for (i=0;i<16;i++) {
		rtapi_snprintf(buf, sizeof(buf), "/dev/hidraw%d", i);
		fd = open(buf, O_RDWR|O_NONBLOCK);
//		fd = open(buf, O_RDWR);	/* Note: do not rely on O_NONBLOCK; there is a kernel bug that prevents nonblock on disconnect */
		if (fd) {
			/* Opening the device succeeded. Extract VID/PID and compare */
			memset(&info, 0x0, sizeof(info));
			res = ioctl(fd, HIDIOCGRAWINFO, &info);
			if (res >= 0) {
				if (info.vendor == USBDEV_VID && info.product == USBDEV_PID) {
					/* We found our device. Return the fd. */
					rtapi_print("Found device at %s\n",buf);
					return fd;
				}
			}
			/* Not the correct device or we could not extract VID/PID. Close fd */
			close (fd);
		}
	}
	return -1;
}

int rtapi_app_main(void) {

    int ret;
    rtapi_print("loading test module\n");
    ret = hal_init("test");
    if (ret < 0)
        return ret;
    comp_id = ret;
    usbfd = FindAndOpenUSBdevice();
    
	if (usbfd < 0) {
		rtapi_print("ERROR opening device, fd=%d\n", usbfd);
	} else {
		rtapi_print("SUCCESS opening device\n");
	}
	ret = export("test", 0);
    if(ret) {
        hal_exit(comp_id);
    } else {
        hal_ready(comp_id);
    }
    return ret;
}

void rtapi_app_exit(void) {
	if (usbfd > 0) {
		close (usbfd);
	}
    hal_exit(comp_id);
    rtapi_print("test module unloaded\n");
}

static void _(struct __comp_state *__comp_inst, long period) {
	int ret;
	ret=0;
	if (ret==0) {
		xmitbuf[0] = 0x0;
		xmitbuf[1] = 0xde;
		xmitbuf[2] = 0xad;
		ret=write(usbfd, xmitbuf, 3);
		//ret=send(usbfd, xmitbuf, 3,0);
	}
	*__comp_inst->position_fb_x = *__comp_inst->position_cmd_x;
}