Adds the capability `cap_data` to the list of capabilities. `cap_data` should include the two-byte PCI capability header (type, next), but not populate it. Correct values will be generated automatically based on `cap_data.id()`.
(&mut self, cap_data: &dyn PciCapability)
| 860 | /// but not populate it. Correct values will be generated automatically based |
| 861 | /// on `cap_data.id()`. |
| 862 | pub fn add_capability(&mut self, cap_data: &dyn PciCapability) -> Result<usize> { |
| 863 | let total_len = cap_data.bytes().len(); |
| 864 | // Check that the length is valid. |
| 865 | if cap_data.bytes().is_empty() { |
| 866 | return Err(Error::CapabilityEmpty); |
| 867 | } |
| 868 | let (cap_offset, tail_offset) = match self.last_capability { |
| 869 | Some((offset, len)) => (Self::next_dword(offset, len), offset + 1), |
| 870 | None => (FIRST_CAPABILITY_OFFSET, CAPABILITY_LIST_HEAD_OFFSET), |
| 871 | }; |
| 872 | let end_offset = cap_offset |
| 873 | .checked_add(total_len) |
| 874 | .ok_or(Error::CapabilitySpaceFull(total_len))?; |
| 875 | if end_offset > CAPABILITY_MAX_OFFSET { |
| 876 | return Err(Error::CapabilitySpaceFull(total_len)); |
| 877 | } |
| 878 | self.registers[STATUS_REG] |= STATUS_REG_CAPABILITIES_USED_MASK; |
| 879 | self.write_byte_internal(tail_offset, cap_offset as u8, false); |
| 880 | self.write_byte_internal(cap_offset, cap_data.id() as u8, false); |
| 881 | self.write_byte_internal(cap_offset + 1, 0, false); // Next pointer. |
| 882 | for (i, byte) in cap_data.bytes().iter().enumerate() { |
| 883 | self.write_byte_internal(cap_offset + i + 2, *byte, false); |
| 884 | } |
| 885 | self.last_capability = Some((cap_offset, total_len)); |
| 886 | |
| 887 | match cap_data.id() { |
| 888 | PciCapabilityId::MessageSignalledInterrupts => { |
| 889 | self.writable_bits[cap_offset / 4] = MSI_CAPABILITY_REGISTER_MASK; |
| 890 | } |
| 891 | PciCapabilityId::MsiX => { |
| 892 | self.msix_cap_reg_idx = Some(cap_offset / 4); |
| 893 | self.writable_bits[self.msix_cap_reg_idx.unwrap()] = MSIX_CAPABILITY_REGISTER_MASK; |
| 894 | } |
| 895 | _ => {} |
| 896 | } |
| 897 | |
| 898 | Ok(cap_offset) |
| 899 | } |
| 900 | |
| 901 | // Find the next aligned offset after the one given. |
| 902 | fn next_dword(offset: usize, len: usize) -> usize { |