Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 1 | pub mod gap_types { |
| 2 | pub static COMPLETE_LOCAL_NAME: u8 = 0x09; |
| 3 | pub static SERVICE_DATA: u8 = 0x16; |
| 4 | pub static UUID: u8 = 0x02; |
| 5 | pub static FLAGS: u8 = 0x01; |
| 6 | } |
| 7 | |
| 8 | pub mod flags { |
| 9 | pub static LE_GENERAL_DISCOVERABLE: u8 = 0x01; |
| 10 | } |
| 11 | |
| 12 | pub struct BlePayload { |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 13 | bytes: [u8; 39], |
| 14 | occupied: usize, |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 15 | } |
| 16 | |
| 17 | impl BlePayload { |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 18 | pub fn add(&mut self, kind: u8, content: &[u8]) -> Result<(), ()> { |
| 19 | self.check_can_write_num_bytes(content.len() + 2)?; |
| 20 | |
| 21 | self.bytes[self.occupied] = (content.len() + 1) as u8; |
| 22 | self.bytes[self.occupied + 1] = kind; |
| 23 | let write = &mut self.bytes[self.occupied + 2..(self.occupied + content.len() + 2)]; |
JOE1994 | d1ec41e | 2020-07-02 18:37:36 -0400 | [diff] [blame] | 24 | write.copy_from_slice(content); |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 25 | self.occupied += 2 + content.len(); |
| 26 | Ok(()) |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 27 | } |
| 28 | |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 29 | pub fn add_flag(&mut self, flag: u8) -> Result<(), ()> { |
| 30 | self.check_can_write_num_bytes(3)?; |
| 31 | |
| 32 | self.bytes[self.occupied] = 2; |
| 33 | self.bytes[self.occupied + 1] = gap_types::FLAGS; |
| 34 | self.bytes[self.occupied + 2] = flag; |
| 35 | self.occupied += 3; |
| 36 | Ok(()) |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 37 | } |
| 38 | |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 39 | pub fn add_service_payload(&mut self, uuid: [u8; 2], content: &[u8]) -> Result<(), ()> { |
| 40 | self.check_can_write_num_bytes(4 + content.len())?; |
| 41 | self.bytes[self.occupied] = (content.len() + 3) as u8; |
| 42 | self.bytes[self.occupied + 1] = gap_types::SERVICE_DATA; |
| 43 | self.bytes[self.occupied + 2] = uuid[0]; |
| 44 | self.bytes[self.occupied + 3] = uuid[1]; |
| 45 | |
| 46 | let write = &mut self.bytes[self.occupied + 4..(self.occupied + content.len() + 4)]; |
JOE1994 | d1ec41e | 2020-07-02 18:37:36 -0400 | [diff] [blame] | 47 | write.copy_from_slice(content); |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 48 | self.occupied += 4 + content.len(); |
| 49 | Ok(()) |
| 50 | } |
| 51 | |
| 52 | fn check_can_write_num_bytes(&self, number: usize) -> Result<(), ()> { |
| 53 | if self.occupied + number <= self.bytes.len() { |
| 54 | Ok(()) |
| 55 | } else { |
| 56 | Err(()) |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 57 | } |
| 58 | } |
| 59 | } |
| 60 | |
torfmaster | e6c4ecb | 2020-01-04 20:34:33 +0100 | [diff] [blame] | 61 | impl Default for BlePayload { |
| 62 | fn default() -> Self { |
| 63 | BlePayload { |
| 64 | bytes: [0; 39], |
| 65 | occupied: 0, |
| 66 | } |
| 67 | } |
| 68 | } |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 69 | impl AsRef<[u8]> for BlePayload { |
| 70 | fn as_ref(&self) -> &[u8] { |
| 71 | &self.bytes[0..self.occupied] |
| 72 | } |
| 73 | } |
| 74 | |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 75 | #[cfg(test)] |
| 76 | mod test { |
Woyten | 012fe57 | 2018-08-24 21:46:51 +0200 | [diff] [blame] | 77 | use super::*; |
| 78 | |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 79 | #[test] |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 80 | fn test_add() { |
torfmaster | e6c4ecb | 2020-01-04 20:34:33 +0100 | [diff] [blame] | 81 | let mut pld = BlePayload::default(); |
Woyten | 8bedbd7 | 2019-12-17 17:08:29 +0100 | [diff] [blame] | 82 | pld.add(1, &[2]).unwrap(); |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 83 | assert_eq!(pld.as_ref().len(), 3); |
| 84 | assert_eq!(pld.as_ref(), &mut [2, 1, 2]) |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 85 | } |
| 86 | |
| 87 | #[test] |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 88 | fn test_add_service_payload() { |
torfmaster | e6c4ecb | 2020-01-04 20:34:33 +0100 | [diff] [blame] | 89 | let mut pld = BlePayload::default(); |
Woyten | 8bedbd7 | 2019-12-17 17:08:29 +0100 | [diff] [blame] | 90 | pld.add_service_payload([1, 2], &[2]).unwrap(); |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 91 | assert_eq!(pld.as_ref(), &[4, 0x16, 1, 2, 2]) |
| 92 | } |
| 93 | |
| 94 | #[test] |
| 95 | fn test_add_service_payload_two_times() { |
torfmaster | e6c4ecb | 2020-01-04 20:34:33 +0100 | [diff] [blame] | 96 | let mut pld = BlePayload::default(); |
Woyten | 8bedbd7 | 2019-12-17 17:08:29 +0100 | [diff] [blame] | 97 | pld.add_service_payload([1, 2], &[2]).unwrap(); |
| 98 | pld.add_service_payload([1, 2], &[2, 3]).unwrap(); |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 99 | |
| 100 | assert_eq!(pld.as_ref(), &[4, 0x16, 1, 2, 2, 5, 0x16, 1, 2, 2, 3]) |
| 101 | } |
| 102 | |
| 103 | #[test] |
| 104 | fn big_data_causes_error() { |
torfmaster | e6c4ecb | 2020-01-04 20:34:33 +0100 | [diff] [blame] | 105 | let mut pld = BlePayload::default(); |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 106 | assert!(pld.add_service_payload([1, 2], &[0; 36]).is_err()); |
| 107 | } |
| 108 | |
| 109 | #[test] |
| 110 | fn initial_size_is_zero() { |
torfmaster | e6c4ecb | 2020-01-04 20:34:33 +0100 | [diff] [blame] | 111 | let pld = BlePayload::default(); |
torfmaster | 906d297 | 2019-02-15 23:38:23 +0100 | [diff] [blame] | 112 | assert_eq!(pld.as_ref().len(), 0); |
Philipp Vollmer | 090398c | 2018-06-29 15:14:54 +0200 | [diff] [blame] | 113 | } |
| 114 | } |