static int samsung_driver_remove(struct vmm_device *dev)
{
struct samsung_port *port = dev->priv;
if (!port) {
return VMM_OK;
}
serial_destroy(port->p);
vmm_host_irq_unregister(port->irq, port);
vmm_devtree_regunmap_release(dev->of_node, port->base, 0);
vmm_free(port);
dev->priv = NULL;
return VMM_OK;
}
static int omap_uart_driver_remove(struct vmm_device *dev)
{
struct omap_uart_port *port = dev->priv;
if (!port) {
return VMM_OK;
}
/* Mask Rx interrupt */
port->ier &= ~(UART_IER_RDI | UART_IER_RLSI);
omap_serial_out(port, UART_IER, port->ier);
/* Free-up resources */
serial_destroy(port->p);
vmm_host_irq_unregister(port->irq, port);
vmm_devtree_regunmap_release(dev->of_node, port->base, 0);
vmm_free(port);
dev->priv = NULL;
return VMM_OK;
}
int
serial_posix_setup(const char *device, int baud, serial **serialobj)
{
/* open serial */
*serialobj = serial_create();
if (*serialobj == NULL) {
printf("Unable to instantiate object: serial\n");
goto fail;
}
if (serial_open(*serialobj, device) < 0) {
printf("Unable to open serial devicer\n");
goto fail;
}
if (serial_set_speed(*serialobj, baud) < 0) {
printf("Unable to set serial speedr\n");
goto fail;
}
if (serial_set_timeout(*serialobj, 10000) < 0) {
printf("Unable to set timeoutr\n");
goto fail;
}
if (serial_set_cts_flow(*serialobj, 1) < 0) {
printf("Unable to set flow controlr\n");
goto fail;
}
printf("%s is opened\n", device);
return (0);
fail:
serial_close(*serialobj);
serial_destroy(*serialobj);
return (1);
}
int main(int argc, char *argv[])
{
uint8_t i;
for(i = 0; i < argc; ++i)
{
if((strcmp(argv[i], "-h") == 0) || (strcmp(argv[i], "--help") == 0))
{
fprintf(stdout, "%s\n", USAGE);
exit(0);
}
if(strcmp(argv[i], "-v") == 0)
{
verbose = true;
fprintf(stdout, "Running in verbose mode\n");
}
if(strcmp(argv[i], "-l") == 0)
{
loopback_mode = true;
fprintf(stdout, "Running in loopback mode\n");
}
}
(void) signal(SIGINT, interrupt);
(void) signal(SIGTERM, interrupt);
lcm = lcm_create(NULL);
while(lcm == NULL)
{
fprintf(stderr, "LCM failed to initialize. Reattempting....\n");
usleep(1000000);
lcm = lcm_create(NULL);
}
// Create Serial Device for XBee
char *xbee_dev_name = "/dev/XBee";
for(i = 0; i < argc; ++i)
{
if(strcmp(argv[i], "-x") == 0)
{
if(i + 1 < argc)
xbee_dev_name = argv[i+1];
break;
}
}
xbee = serial_create(xbee_dev_name, B9600);
if(xbee == NULL)
fprintf(stderr, "XBee device does not exist at %s\n", xbee_dev_name);
else
fprintf(stdout, "XBee device successfully opened at 9600 baud on %s\n", xbee_dev_name);
// Create Serial Device for USB
char *usb_dev_name = "/dev/stack";
for(i = 0; i < argc; ++i)
{
if(strcmp(argv[i], "-u") == 0)
{
if(i + 1 < argc)
usb_dev_name = argv[i+1];
break;
}
}
usb = serial_create(usb_dev_name, B115200);
if(usb == NULL)
fprintf(stderr, "Usb device does not exist at %s\n", usb_dev_name);
else
fprintf(stdout, "Usb device successfully opened at 115200 baud on %s\n", usb_dev_name);
// If no open comm ports and not in loopback mode, quit.
if(!xbee && !usb && !loopback_mode)
{
fprintf(stderr, "Failed to open any comms devices. Exiting...\n");
lcm_destroy(lcm);
exit(1);
}
// Create comms devices
if(xbee)
{
xbee_comms = comms_create(1000, 1000, 1, publish_xbee);
comms_subscribe(xbee_comms, CHANNEL_KILL, handler_kill, NULL);
comms_subscribe(xbee_comms, CHANNEL_CHANNELS, handler_channels, NULL);
comms_subscribe(xbee_comms, CHANNEL_CFG_USB_SN, handler_cfg_usb_serial_num, NULL);
comms_subscribe(xbee_comms, CHANNEL_CFG_DATA_FREQUENCY, handler_cfg_data_frequency, NULL);
pthread_create(&xbee_thread, NULL, xbee_run, NULL);
}
// Create comms devices
if(usb)
{
usb_comms = comms_create(1000, 1000, 1, publish_usb);
comms_subscribe(usb_comms, CHANNEL_KILL, handler_kill, NULL);
comms_subscribe(usb_comms, CHANNEL_CHANNELS, handler_channels, NULL);
comms_subscribe(usb_comms, CHANNEL_CFG_USB_SN, handler_cfg_usb_serial_num, NULL);
comms_subscribe(usb_comms, CHANNEL_CFG_DATA_FREQUENCY, handler_cfg_data_frequency, NULL);
pthread_create(&usb_thread, NULL, usb_run, NULL);
}
kill_t_subscription_t *kill_subs =
kill_t_subscribe(lcm, "KILL_.*_TX", handler_kill_lcm, NULL);
channels_t_subscription_t *channels_subs =
channels_t_subscribe(lcm, "CHANNELS_.*_TX", handler_channels_lcm, NULL);
cfg_data_frequency_t_subscription_t *data_freq_subs =
cfg_data_frequency_t_subscribe(lcm, "CFG_DATA_FREQUENCY_.*_TX",
handler_cfg_data_frequency_lcm, NULL);
cfg_usb_serial_num_t_subscription_t *usb_serial_num_subs =
cfg_usb_serial_num_t_subscribe(lcm, "CFG_USB_SERIAL_NUM_.*_TX",
handler_cfg_usb_serial_num_lcm, NULL);
while(!done)
{
// Set up timeout on lcm_handle to prevent blocking at program exit
int lcm_fd = lcm_get_fileno(lcm);
fd_set fds;
FD_ZERO(&fds);
FD_SET(lcm_fd, &fds);
struct timeval timeout = { 0, 100000 }; // wait 100ms
int status = select(lcm_fd + 1, &fds, NULL, NULL, &timeout);
if(done) break;
if(status != 0 && FD_ISSET(lcm_fd, &fds)) {
if ( lcm_handle(lcm) ) {
fprintf( stderr, "LCM handle error, aborting\n" );
done = true;
}
}
}
fprintf(stdout, "Lcm driver destroyed.\n");
kill_t_unsubscribe(lcm, kill_subs);
channels_t_unsubscribe(lcm, channels_subs);
cfg_data_frequency_t_unsubscribe(lcm, data_freq_subs);
cfg_usb_serial_num_t_unsubscribe(lcm, usb_serial_num_subs);
if(xbee)
{
pthread_cancel(xbee_thread);
comms_destroy(xbee_comms);
serial_destroy(xbee);
fprintf(stdout, "Xbee driver destroyed.\n");
}
if(usb)
{
pthread_cancel(usb_thread);
comms_destroy(usb_comms);
serial_destroy(usb);
fprintf(stdout, "Usb driver destroyed.\n");
}
lcm_destroy(lcm);
return 0;
}
