#include "usb.h" static u16 usb_read_le16(const u8 *bytes) { return (u16)(bytes[0]|((u16)bytes[1]<<8)); } static u32 usb_read_le32(const u8 *bytes) { return (u32)bytes[0]|((u32)bytes[1]<<8)|((u32)bytes[2]<<16)| ((u32)bytes[3]<<24); } static void usb_write_le32(u8 *bytes,u32 value) { bytes[0]=(u8)value;bytes[1]=(u8)(value>>8); bytes[2]=(u8)(value>>16);bytes[3]=(u8)(value>>24); } static u32 usb_read_be32(const u8 *bytes) { return ((u32)bytes[0]<<24)|((u32)bytes[1]<<16)| ((u32)bytes[2]<<8)|bytes[3]; } static u64 usb_read_be64(const u8 *bytes) { return ((u64)usb_read_be32(bytes)<<32)|usb_read_be32(bytes+4); } static void usb_write_be32(u8 *bytes,u32 value) { bytes[0]=(u8)(value>>24);bytes[1]=(u8)(value>>16); bytes[2]=(u8)(value>>8);bytes[3]=(u8)value; } static void usb_write_be64(u8 *bytes,u64 value) { usb_write_be32(bytes,(u32)(value>>32)); usb_write_be32(bytes+4,(u32)value); } int bob64_scsi_build_read_write_cdb(u64 lba,u32 blocks,int write, u8 cdb[16],u8 *cdb_length) { if(!cdb||!cdb_length||!blocks||(write!=0&&write!=1)|| (u64)blocks-1u>0xffffffffffffffffULL-lba)return -1; for(u32 i=0;i<16;i++)cdb[i]=0; if(lba<=0xffffffffULL&&blocks<=0xffffu&& (u64)blocks<=0x100000000ULL-lba) { cdb[0]=write?0x2a:0x28; usb_write_be32(cdb+2,(u32)lba); cdb[7]=(u8)(blocks>>8);cdb[8]=(u8)blocks; *cdb_length=10; } else { cdb[0]=write?0x8a:0x88; usb_write_be64(cdb+2,lba); usb_write_be32(cdb+10,blocks); *cdb_length=16; } return 0; } int bob64_scsi_build_read_capacity16_cdb(u8 cdb[16]) { if(!cdb)return -1; for(u32 i=0;i<16;i++)cdb[i]=0; cdb[0]=0x9e;cdb[1]=0x10; usb_write_be32(cdb+10,32); return 0; } int bob64_scsi_parse_read_capacity10(const void *response,usize length, u64 *block_count,u32 *block_size,int *needs_capacity16) { const u8 *bytes=(const u8 *)response; if(!bytes||length<8||!block_count||!block_size||!needs_capacity16) return -1; u32 last=usb_read_be32(bytes),size=usb_read_be32(bytes+4); if(!size)return -1; *block_size=size; *needs_capacity16=last==0xffffffffu; *block_count=*needs_capacity16?0:(u64)last+1u; return 0; } int bob64_scsi_parse_read_capacity16(const void *response,usize length, u64 *block_count,u32 *block_size) { const u8 *bytes=(const u8 *)response; if(!bytes||length<32||!block_count||!block_size)return -1; u64 last=usb_read_be64(bytes); u32 size=usb_read_be32(bytes+8); if(last==0xffffffffffffffffULL||!size)return -1; *block_count=last+1u;*block_size=size; return 0; } u64 bob64_usb_control_setup(u8 request_type,u8 request,u16 value,u16 index, u16 length) { return (u64)request_type|((u64)request<<8)|((u64)value<<16)| ((u64)index<<32)|((u64)length<<48); } static int usb_find_hid_boot_device(const void *configuration,usize length, u8 protocol,u16 minimum_packet,BOB64_USB_HID_BOOT_DEVICE *device) { const u8 *bytes=(const u8 *)configuration; usize total,offset; int matching_interface=0; BOB64_USB_HID_BOOT_DEVICE candidate={0}; if(!bytes||!device||length<9||bytes[0]<9||bytes[1]!=2)return -1; total=usb_read_le16(bytes+2); if(totallength)return -1; candidate.ConfigurationValue=bytes[5]; for(offset=bytes[0];offsettotal-offset)return -1; if(descriptor_type==4) { matching_interface=descriptor_length>=9&& bytes[offset+3]==0&&bytes[offset+5]==3&& bytes[offset+6]==1&&bytes[offset+7]==protocol; if(matching_interface) { candidate.InterfaceNumber=bytes[offset+2]; candidate.AlternateSetting=bytes[offset+3]; candidate.InterfaceProtocol=bytes[offset+7]; } } else if(descriptor_type==5&&matching_interface) { u8 address,attributes; u16 packet_size; if(descriptor_length<7)return -1; address=bytes[offset+2]; attributes=bytes[offset+3]; packet_size=(u16)(usb_read_le16(bytes+offset+4)&0x07ffu); if((address&0x80u)&&(address&0x0fu)&& (attributes&3u)==3u&&packet_size>=minimum_packet) { candidate.EndpointAddress=address; candidate.EndpointAttributes=attributes; candidate.EndpointMaxPacketSize=packet_size; candidate.EndpointInterval=bytes[offset+6]; *device=candidate; return 0; } } offset+=descriptor_length; } return 1; } int bob64_usb_find_hid_boot_keyboard(const void *configuration,usize length, BOB64_USB_HID_BOOT_KEYBOARD *keyboard) { return usb_find_hid_boot_device(configuration,length,1,8,keyboard); } int bob64_usb_find_hid_boot_mouse(const void *configuration,usize length, BOB64_USB_HID_BOOT_DEVICE *mouse) { return usb_find_hid_boot_device(configuration,length,2,3,mouse); } int bob64_usb_find_mass_storage(const void *configuration,usize length, BOB64_USB_MASS_STORAGE *storage) { const u8 *bytes=(const u8 *)configuration; BOB64_USB_MASS_STORAGE candidate={0}; usize total,offset; int matching_interface=0; if(!bytes||!storage||length<9||bytes[0]<9||bytes[1]!=2)return -1; total=usb_read_le16(bytes+2); if(totallength)return -1; candidate.ConfigurationValue=bytes[5]; for(offset=bytes[0];offsettotal-offset)return -1; if(descriptor_type==4) { if(matching_interface&&candidate.BulkInEndpoint&& candidate.BulkOutEndpoint) { *storage=candidate; return 0; } matching_interface=descriptor_length>=9&& bytes[offset+5]==8&&bytes[offset+6]==6&& bytes[offset+7]==0x50; candidate.BulkInEndpoint=0; candidate.BulkOutEndpoint=0; candidate.BulkInMaxPacketSize=0; candidate.BulkOutMaxPacketSize=0; if(matching_interface) { candidate.InterfaceNumber=bytes[offset+2]; candidate.AlternateSetting=bytes[offset+3]; } } else if(descriptor_type==5&&matching_interface) { u8 address,attributes; u16 packet_size; if(descriptor_length<7)return -1; address=bytes[offset+2];attributes=bytes[offset+3]; packet_size=(u16)(usb_read_le16(bytes+offset+4)&0x07ffu); if(!(address&0x70u)&&(address&0x0fu)!=0&& (attributes&3u)==2u&&packet_size) { if(address&0x80u) { candidate.BulkInEndpoint=address; candidate.BulkInMaxPacketSize=packet_size; } else { candidate.BulkOutEndpoint=address; candidate.BulkOutMaxPacketSize=packet_size; } } } offset+=descriptor_length; } if(matching_interface&&candidate.BulkInEndpoint&&candidate.BulkOutEndpoint) { *storage=candidate; return 0; } return 1; } int bob64_usb_msc_build_cbw(void *buffer,usize capacity,u32 tag, u32 transfer_length,int device_to_host,u8 lun,const void *cdb, u8 cdb_length) { u8 *bytes=(u8 *)buffer; const u8 *command=(const u8 *)cdb; if(!bytes||capacity16||lun>15|| (device_to_host!=0&&device_to_host!=1))return -1; for(usize i=0;iexpected_transfer_length|| parsed_status>2u)return -1; *residue=parsed_residue; *status=parsed_status; return 0; } int bob64_usb_parse_hub_descriptor(const void *descriptor,usize length, BOB64_USB_HUB_DESCRIPTOR *hub) { const u8 *bytes=(const u8 *)descriptor; BOB64_USB_HUB_DESCRIPTOR candidate={0}; usize removable_bytes,power_mask_bytes,required; if(!bytes||!hub||length<7||bytes[0]<7||bytes[1]!=0x29|| bytes[0]>length)return -1; candidate.PortCount=bytes[2]; if(!candidate.PortCount||candidate.PortCount>BOB64_USB_HUB_MAX_PORTS) return -1; removable_bytes=((usize)candidate.PortCount+1u+7u)/8u; power_mask_bytes=((usize)candidate.PortCount+7u)/8u; required=7u+removable_bytes+power_mask_bytes; if(bytes[0]length)return -1; candidate.ConfigurationValue=bytes[5]; for(offset=bytes[0];offsettotal-offset)return -1; if(descriptor_type==4) { matching_interface=descriptor_length>=9&& bytes[offset+5]==9&&bytes[offset+6]==0; if(matching_interface) { candidate.InterfaceNumber=bytes[offset+2]; candidate.AlternateSetting=bytes[offset+3]; } } else if(descriptor_type==5&&matching_interface) { u8 address,attributes; u16 packet_size; if(descriptor_length<7)return -1; address=bytes[offset+2];attributes=bytes[offset+3]; packet_size=(u16)(usb_read_le16(bytes+offset+4)&0x07ffu); if((address&0x80u)&&(address&0x0fu)&& (attributes&3u)==3u&&packet_size) { candidate.EndpointAddress=address; candidate.EndpointAttributes=attributes; candidate.EndpointMaxPacketSize=packet_size; candidate.EndpointInterval=bytes[offset+6]; *hub=candidate; return 0; } } offset+=descriptor_length; } return 1; } int bob64_usb_route_string_append(u32 route,u8 depth,u8 port,u32 *result) { u32 preceding_mask,shift; u8 route_port; if(!result||!port||depth>=5)return -1; shift=(u32)depth*4u; preceding_mask=shift?(1u<0x000fffffu)return -1; route_port=port>14u?15u:port; *result=route|((u32)route_port<