Re: Linux Hotplug Question

#include "usbutils.h"
#include <pthread.h>
#include <regex.h>
#include <dirent.h>

#define MAX_DEVFS_FILES 255 

static fd_set    readfs;
static int       maxfd, prev_maxfd;

/* Head of linked list of #driver_entry_s structs */
static struct class_entry_s *class_table_head = NULL;
static void class_add_entry ( struct class_entry_s *cur_entry); 

static void readDataFromDev(void);
static void exitHandler(int sig);

/* Check new devices Thread Variables       */
static int wakeupTimer;
pthread_mutex_t wakeup_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t wakeup_cond_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t  wakeup_cond  = PTHREAD_COND_INITIALIZER;

/* Delete Node Thread Variables */
static int deleteNode;
pthread_mutex_t delete_mutex    = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t condition_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t  condition_cond  = PTHREAD_COND_INITIALIZER;

static const char unbind_path_format[] = "/sys/bus/usb/devices/%s/driver/unbind";
static int unbind_interface_busid(char *busid)
{
	char unbind_path[PATH_MAX];
	int fd;
	int ret;

	snprintf(unbind_path, sizeof(unbind_path), unbind_path_format, busid);
	fd = open(unbind_path, O_WRONLY);
	if (fd < 0) {
		printf("Opening unbind path failed %d\n", fd);
		return -1;
	}

	ret = write(fd, busid, strnlen(busid, BUS_ID_SIZE));
	if (ret < 0) {
		printf("Write to unbind failed %d\n", ret);
		close(fd);
		return -1;
	}

	close(fd);
	return 0;
}

static int unbind_interface(char *busid, int configvalue, int interface)
{
	char inf_busid[BUS_ID_SIZE];
	printf("Unbinding interface\n" );
	snprintf(inf_busid, BUS_ID_SIZE, "%s:%d.%d", busid, configvalue, interface);

	return unbind_interface_busid(inf_busid);
}


static const char bind_path_format[] = "/sys/bus/usb/drivers/%s/bind";
static int bind_interface_busid(char *busid, char *driver)
{
	char bind_path[PATH_MAX];
	int fd;
	int ret;

	snprintf(bind_path, sizeof(bind_path), bind_path_format, driver);

	fd = open(bind_path, O_WRONLY);
	if (fd < 0)
		return -1;

	ret = write(fd, busid, strnlen(busid, BUS_ID_SIZE));
	if (ret < 0) {
		close(fd);
		return -1;
	}

	close(fd);
	return 0;
}

static int bind_interface(char *busid, int configvalue, int interface, char *driver)
{
	char inf_busid[BUS_ID_SIZE];

	snprintf(inf_busid, BUS_ID_SIZE, "%s:%d.%d", busid, configvalue, interface);

	return bind_interface_busid(inf_busid, driver);
}

static int unbind(char *busid)
{
	int configvalue = 0;
	int ninterface = 0;
	int devclass = 0;
	int i;
	int failed = 0;

	configvalue = read_bConfigurationValue(busid);
	ninterface  = read_bNumInterfaces(busid);
	devclass    = read_bDeviceClass(busid);

	if (configvalue < 0 || ninterface < 0 || devclass < 0) {
		printf("Read config value removed\n"); 
		return -1;
	}

	if (devclass == 0x09) {
		printf("Skip unbinding of hub\n");
		return -1;
	}

 	for (i = 0; i < ninterface; i++) {
 		char driver[PATH_MAX];
		int ret;

		bzero(&driver, sizeof(driver));

 		getdriver(busid, configvalue, i, driver, PATH_MAX-1);

		printf("%s:%d.%d -> %s\n", busid, configvalue, i, driver);
		if (!strncmp("none", driver, PATH_MAX))
			continue; /* unbound interface */

		/* unbinding */
		ret = unbind_interface(busid, configvalue, i);
		if (ret < 0) {
		        printf("Unbind driver at %s:%d.%d failed\n", busid, configvalue, i);
			failed = 1;
		}
	}

	if (failed)
		return -1;
	else
		return 0;
}

/* call at unbound state */
static int bind_to_usbdav(char *busid)
{
	int configvalue = 0;
	int ninterface = 0;
	int i;
	int failed = 0;

	configvalue = read_bConfigurationValue(busid);
	ninterface  = read_bNumInterfaces(busid);

	if (configvalue < 0 || ninterface < 0) {
		printf("Read config and ninf value, removed?\n");
		return -1;
	}

 	for (i = 0; i < ninterface; i++) {
		int ret;

		ret = bind_interface(busid, configvalue, i, "usbdav"); 
		if (ret < 0) {
		        printf("Bind usbdav at %s:%d.%d failed\n", busid, configvalue, i);
			failed = 1;
		}
	}

	if (failed)
		return -1;
	else
		return 0;
}

static int use_device_by_usbdav(char *busid)
{
	int ret;

	ret = unbind(busid);
	if (ret < 0) {
		printf("Unbind drivers of %s failed\n", busid);
		return -1;
	}

	ret = bind_to_usbdav(busid);
	if (ret < 0) {
		printf("Bind usbdav to %s failed\n", busid);
		return -1;
	}

	printf("Bind %s usbdav complete!\n", busid);

	return 0;
}

static int is_usb_device(char *busid)
{
	int ret;

	regex_t regex;
	regmatch_t pmatch[1];
        
	ret = regcomp(&regex, "^[0-9]+-[0-9]+(\\.[0-9]+)*$", REG_NOSUB|REG_EXTENDED);
	if (ret < 0)
		printf("regcomp: %s\n", strerror(errno));

	ret = regexec(&regex, busid, 0, pmatch, 0);
	if (ret){
		return 0;	/* not matched */
	}

	return 1;
}

static int auto_bind(void)
{
	DIR *dir;

	dir = opendir("/sys/bus/usb/devices/");
	if (!dir)
		printf("Err: Opendir%s \n", strerror(errno));

	for (;;) {
		struct dirent *dirent;
		char *busid;

		dirent = readdir(dir);
		if (!dirent)
			break;

		busid = dirent->d_name;
		if (is_usb_device(busid)) {
			char name[100] = {'\0'};
			char driver[100] =  {'\0'};
			int conf, ninf = 0;
			int i;
			int isusbhid = 0; 

			conf = read_bConfigurationValue(busid);
			ninf = read_bNumInterfaces(busid);

			getdevicename(busid, name, sizeof(name));
			for (i = 0; i < ninf; i++) {
				getdriver(busid, conf, i, driver, sizeof(driver));
	
				if (strncmp(driver, "usbhid", 6) == 0 ) {
					isusbhid = 1; 
					break;
				}
			}
			
			if(isusbhid == 1 )
				use_device_by_usbdav(busid);
		}
	}

	closedir(dir);
	return 0;
}

void initInteruptHandler(void)
{
  /* interupt handler */
  signal(SIGHUP,  exitHandler);
  //signal(SIGINT,  exitHandler);
  signal(SIGILL,  exitHandler);
  //signal(SIGABRT, exitHandler);
  //signal(SIGBUS,  exitHandler);
  signal(SIGSEGV, exitHandler);
  signal(SIGPIPE, exitHandler);
  signal(SIGTERM, exitHandler);
  signal(SIGIO,   exitHandler);
}

static const char dev_path_format[] = "/dev/%s";
static int open_devfs(void)
{
	DIR *dir;
	int openStat = -1;

	dir = opendir("/sys/class/usb");
        if(!dir)
	{
		printf("ERROR !!! Opening directory: /sys/class/usb \n");
		return openStat;
	}

	for(;;) {
		struct dirent *dirent;
		char   *name;		
		char   dev_name[20];
		struct class_entry_s cur_entry;

		dirent = readdir(dir);
		if (!dirent)
		{
			break; // return openStat;
		}

		name = dirent->d_name;
		if (strncmp(name, "." , 1)  == 0   || 
		    strncmp(name, "..", 2) == 0) {
			// Ignore the pwd "." and ".." file entries
                        continue;
                }
		else {
	        	snprintf(dev_name, sizeof(dev_name), dev_path_format, name);	
tryagain1:
			cur_entry.input_fd  = open(dev_name, O_RDWR); 		        	
			if(cur_entry.input_fd <= 0 ){
				printf("Unable to open the File Status:%d\n", cur_entry.input_fd);
				close(cur_entry.input_fd);
				goto tryagain1;
 			}

			strncpy (cur_entry.class_name, name, MAX_NAME_SIZE);
			maxfd      = MAX( cur_entry.input_fd, prev_maxfd) + 1;
			prev_maxfd = maxfd; /* Save Previous max fd for updating */
                        
			// Add the Current Class Entry to device list
			class_add_entry ( &cur_entry );
			openStat = 0;
		}
	}

	closedir(dir);
	return openStat;
}

static int check_class_name( const char* class_name )
{
	struct class_entry_s *cur_class_entry;
    	cur_class_entry  =  class_table_head;

        while( cur_class_entry  != NULL ) {
            // Compare dir entry with the link list we created
            if (strcmp ( cur_class_entry->class_name, class_name) == 0) { // Match found for the /dev file
		cur_class_entry->keep = 1;
                return 0;
	    }
            else { // No Match found
                cur_class_entry = cur_class_entry->next; /* Inc to next element */                
            }
        }

	// Adding the Current Class Entry to device list
	{
		char dev_name[MAX_NAME_SIZE];
 
		cur_class_entry = malloc (sizeof (struct class_entry_s));
    		if (cur_class_entry == NULL)
       			printf("Error Malloc: Out of memory\n");

		snprintf(dev_name, sizeof(dev_name), dev_path_format, class_name);	
tryagain:
		cur_class_entry->input_fd  = open(dev_name, O_RDWR); 
		cur_class_entry->keep      = 1;
		        	
		if(cur_class_entry->input_fd <= 0 ) {
			printf("Unable to open the File Status: %d\n", cur_class_entry->input_fd);
			goto tryagain;
 		}

		strncpy (cur_class_entry->class_name, class_name, MAX_NAME_SIZE);
		maxfd      = MAX( cur_class_entry->input_fd, prev_maxfd) + 1;
		prev_maxfd = maxfd; /* Save Previous max fd for updating */
		class_add_entry(cur_class_entry);
	}
	return 0;
}

void *delete_unpresent_class(void)
{
	struct class_entry_s 	*prev_entry, 
				*cur_entry ,
				*next_entry;

	while(1) {

		pthread_mutex_lock( &condition_mutex );
      		while ( deleteNode != 1 )
      		{
         		pthread_cond_wait( &condition_cond, &condition_mutex );
      		}
      		pthread_mutex_unlock( &condition_mutex );

                pthread_mutex_lock( &delete_mutex );
      		deleteNode = 0;

		/* Check if its not an empty list */
        	if(class_table_head) 
        	{
                	prev_entry  = NULL;
        		cur_entry   = class_table_head;

                	while(cur_entry != NULL)
                	{
                        	next_entry = cur_entry->next;
				if( cur_entry->keep == 0)
				{
					if(prev_entry != NULL) {
						prev_entry->next = next_entry;
					}
					close(cur_entry->input_fd);
					free(cur_entry);
					cur_entry = next_entry;
					continue;
				}
				else {
					cur_entry->keep = 0;
					prev_entry = cur_entry;
					cur_entry  = next_entry;
				}
                	}
        	}
      		pthread_mutex_unlock( &delete_mutex );
	}
}

static int get_Node_count(void)
{
	int NodeCount;
	struct class_entry_s *templist;

	templist = class_table_head;
	NodeCount = 0;
	if (templist == NULL )
		return NodeCount;

	while( templist != NULL )
	{
		NodeCount++;
		templist = templist->next;
	}

	return NodeCount;
}

static int refresh_exported_class(void)
{
        struct sysfs_class     *cls;  /* sysfs_class of usb_interface */
        struct dlist           *class_list;

        int 	indx = 0, 
		NodeCnt = 0;
	
	cls = sysfs_open_class("usb");
        class_list = sysfs_get_class_devices(cls);
        if (!class_list) {
                printf("No usb class list!\n");
                return 0;
        }

        /* collect unique USB devices (not interfaces) */
        dlist_for_each_data(class_list, cls, struct sysfs_class) {
		check_class_name(cls->name);
		indx++;
        }

	/* Check if there are any extra dev files are present */
        dlist_destroy(class_list);
        
        NodeCnt = get_Node_count();
        if(  NodeCnt > indx ) { 
		pthread_mutex_lock( &delete_mutex );
      		deleteNode = 1; 
      		pthread_mutex_unlock( &delete_mutex );
		pthread_cond_broadcast( &condition_cond );
	}

	indx = 0;
	return 1;
}

/* Add an entry to the class list
*
* @param cur_entry Name of the cur class entry
*
*/

static void class_add_entry ( struct class_entry_s *cur_entry)
{
    struct class_entry_s *entry;
    entry = malloc (sizeof (struct class_entry_s));

    if (entry == NULL)
        printf("Error Malloc: Out of memory\n");

    strncpy (entry->class_name, cur_entry->class_name, MAX_NAME_SIZE);
    entry->input_fd = cur_entry->input_fd;
    entry->keep = 1;
    entry->next = class_table_head;
    class_table_head = entry;
}

void *update_devices(void)
{

	while(1) {

		pthread_mutex_lock( &wakeup_cond_mutex );
      		pthread_cond_wait( &wakeup_cond, &wakeup_cond_mutex );
      		pthread_mutex_unlock( &wakeup_cond_mutex );

                pthread_mutex_lock( &wakeup_mutex );
      		wakeupTimer = 0;
		pthread_mutex_unlock( &wakeup_mutex );

		auto_bind(); // Look for new devices 
		refresh_exported_class(); // devices();
	}
}

int main(int argc, char *argv[])
{
 int result , rc1;
 pthread_t delete_thread_id, wakeup_tid;

 /* Create thread for delete Node */
 if( (rc1=pthread_create( &delete_thread_id, NULL, (void *) delete_unpresent_class, NULL)) )
 {
 	printf("Node Delete - Thread creation failed: %d\n", rc1);
 }

 /* Create wakeup thread */
 if( (rc1=pthread_create( &wakeup_tid, NULL, (void *) update_devices, NULL)) )
 {
 	printf("Wakeup Thread creation failed: %d\n", rc1);
 }

 /* Bind all the USB HID devices to daVinci usbdav at start */
 auto_bind(); 
 initInteruptHandler(); /* Initialize the Interrupt Handler */

 if((result = open_devfs()) == 0) {
 	printf("Dev File Opened: Success %d\n", maxfd);
	readDataFromDev();
 }
 else
	printf ("Error Opening dev Files %d\n", result);

 pthread_join( delete_thread_id, NULL);
 pthread_join( wakeup_tid, NULL);

 return 0;
}

#define SLEEP_TIME                      5000

static void readDataFromDev(void)
{
  char                 buf[40]; // , buf2[40];
  int                  nread;
  int                  result;
  struct timeval       tval;
  int                  devindx;
  struct class_entry_s *curclass_entry;

 /* data comes from driver */
 while (1) {
	curclass_entry = class_table_head; /* Initialize to Head of list */
	while( curclass_entry != NULL ){
		FD_ZERO(&readfs);
		FD_SET(curclass_entry->input_fd,  &readfs);		

		tval.tv_sec  = 0;
      		tval.tv_usec = SLEEP_TIME;

		result = select(maxfd, &readfs, NULL, NULL, &tval);
               	if(result > 0 ){
			if(FD_ISSET(curclass_entry->input_fd, &readfs)){
				nread = read(curclass_entry->input_fd, buf, sizeof(buf));
				/* PROCESS THE INPUT DATA HERE */
					 if (nread > 0){

					{
						int indx;
          					printf("Reading %d bytes from %s : Data - ", 
									  nread, curclass_entry->class_name);
						for(indx = 0; indx < nread; indx++)
							printf("%d ", buf[indx]);				
						printf("\n");
					}

       				}
			} // if (FD_ISSET)
		}	
		/* Increment to the next node */
		curclass_entry = curclass_entry->next;
	} // END of list parsing 
        
        wakeupTimer++; 
	
	if(wakeupTimer == 5) {
      		printf("Wake up\n");
		pthread_cond_broadcast( &wakeup_cond );
	}
 } // While End
}

void clean_all_list(struct class_entry_s *class_head_node)
{
	struct class_entry_s *temp_entry;

        if(class_head_node != NULL)
        {
        	printf("Close-Input fd: %d devName: %s\n", class_head_node->input_fd, class_head_node->class_name);
		close(class_head_node->input_fd);
		temp_entry = class_head_node->next;
		free(class_head_node);
        	clean_all_list(temp_entry);
	}

	return; // Reached the last Node
}

static void exitHandler(int sig)
{

  printf("terminate/interupt signal %d\n", sig);
  clean_all_list(class_table_head);
  exit(1);
}