Moved usb function in usb.c, i2c and mcp23 are working