Coverage for /pythoncovmergedfiles/medio/medio/usr/local/lib/python3.11/site-packages/scapy/contrib/rtps/rtps.py: 85%

1# SPDX-License-Identifier: GPL-2.0-or-later 

2# This file is part of Scapy 

3# See https://scapy.net/ for more information 

4# Copyright (C) 2021 Trend Micro Incorporated 

5# Copyright (C) 2021 Alias Robotics S.L. 

6 

7""" 

8Real-Time Publish-Subscribe Protocol (RTPS) dissection 

9""" 

10 

11# scapy.contrib.description = RTPS abstractions 

12# scapy.contrib.status = library 

13 

14import struct 

15 

16from scapy.fields import ( 

17 ConditionalField, 

18 IntField, 

19 PacketField, 

20 PacketListField, 

21 ShortField, 

22 StrField, 

23 StrFixedLenField, 

24 StrLenField, 

25 X3BytesField, 

26 XByteField, 

27 XIntField, 

28 XNBytesField, 

29 XShortField, 

30 XStrLenField, 

31 FlagsField, 

32 Field, 

33 EnumField, 

34) 

35from scapy.packet import Packet, bind_layers 

36 

37from scapy.contrib.rtps.common_types import ( 

38 EField, 

39 EPacket, 

40 EPacketField, 

41 InlineQoSPacketField, 

42 ProtocolVersionPacket, 

43 DataPacketField, 

44 STR_MAX_LEN, 

45 SerializedDataField, 

46 VendorIdPacket, 

47 e_flags, 

48) 

49from scapy.contrib.rtps.pid_types import ( 

50 ParameterListPacket, 

51 get_pid_class, 

52 PID_SENTINEL 

53) 

54 

55 

56_rtps_reserved_entity_ids = { 

57 b"\x00\x00\x00\x00": "ENTITY_UNKNOWN", 

58 b"\x00\x00\x01\xc1": "ENTITYID_PARTICIPANT", 

59 b"\x00\x00\x02\xc2": "ENTITYID_SEDP_BUILTIN_TOPIC_WRITER", 

60 b"\x00\x00\x02\xc7": "ENTITYID_SEDP_BUILTIN_TOPIC_READER", 

61 b"\x00\x00\x03\xc2": "ENTITYID_SEDP_BUILTIN_PUBLICATIONS_WRITER", 

62 b"\x00\x00\x03\xc7": "ENTITYID_SEDP_BUILTIN_PUBLICATIONS_READER", 

63 b"\x00\x00\x04\xc2": "ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_WRITER", 

64 b"\x00\x00\x04\xc7": "ENTITYID_SEDP_BUILTIN_SUBSCRIPTIONS_READER", 

65 b"\x00\x01\x00\xc2": "ENTITYID_SPDP_BUILTIN_PARTICIPANT_WRITER", 

66 b"\x00\x01\x00\xc7": "ENTITYID_SPDP_BUILTIN_PARTICIPANT_READER", 

67 b"\x00\x02\x00\xc2": "ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_WRITER", 

68 b"\x00\x02\x00\xc7": "ENTITYID_P2P_BUILTIN_PARTICIPANT_MESSAGE_READER", 

69} 

70 

71 

72class GUIDPrefixPacket(Packet): 

73 name = "RTPS GUID Prefix" 

74 fields_desc = [ 

75 XIntField("hostId", 0), 

76 XIntField("appId", 0), 

77 XIntField("instanceId", 0), 

78 ] 

79 

80 def extract_padding(self, p): 

81 return b"", p 

82 

83 

84class RTPS(Packet): 

85 """ 

86 RTPS package, overall structure as per DDSI-RTPS v2.3, section 9.4.1 

87 The structure is also discussed at 8.3.3. 

88 

89 The wire representation (bits) is as follows: 

90 

91 0...2...........7...............15.............23.............. 31 

92 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

93 | Header (RTPSHeader) | 

94 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

95 | Submessage (RTPSSubmessage) | 

96 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

97 ................................................................. 

98 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

99 | Submessage | 

100 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

101 

102 For representation purposes, this package will only contain the header 

103 and other submessages will be bound as layers (bind_layers): 

104 

105 RTPS Header structure as per DDSI-RTPS v2.3, section 9.4.4 

106 The wire representation (bits) is as follows: 

107 

108 0...2...........7...............15.............23...............31 

109 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

110 | 'R' | 'T' | 'P' | 'S' | 

111 +---------------+---------------+---------------+---------------+ 

112 | ProtocolVersion version | VendorId vendorId | 

113 +---------------+---------------+---------------+---------------+ 

114 | | 

115 + + 

116 | GuidPrefix guidPrefix | 

117 + + 

118 | | 

119 +---------------+---------------+---------------+---------------+ 

120 

121 References: 

122 

123 * https://community.rti.com/static/documentation/wireshark/current/doc/understanding_rtps.html # noqa E501 

124 * https://www.omg.org/spec/DDSI-RTPS/2.3/PDF 

125 * https://www.wireshark.org/docs/dfref/r/rtps.html 

126 """ 

127 

128 name = "RTPS Header" 

129 fields_desc = [ 

130 StrFixedLenField("magic", b"", 4), 

131 PacketField( 

132 "protocolVersion", ProtocolVersionPacket(), ProtocolVersionPacket), 

133 PacketField( 

134 "vendorId", VendorIdPacket(), VendorIdPacket), 

135 PacketField( 

136 "guidPrefix", GUIDPrefixPacket(), GUIDPrefixPacket), 

137 ] 

138 

139 

140class InlineQoSPacket(EPacket): 

141 name = "Inline QoS" 

142 

143 fields_desc = [ 

144 PacketListField("parameters", [], next_cls_cb=get_pid_class), 

145 PacketField("sentinel", "", PID_SENTINEL), 

146 ] 

147 

148 

149class ParticipantMessageDataPacket(EPacket): 

150 name = "Participant Message Data" 

151 fields_desc = [ 

152 PacketField("guidPrefix", "", GUIDPrefixPacket), 

153 XIntField("kind", 0), 

154 EField(XIntField("sequenceSize", 0), 

155 endianness_from=e_flags), # octets 

156 StrLenField( 

157 "serializedData", 

158 "", 

159 length_from=lambda x: x.sequenceSize * 4, 

160 max_length=STR_MAX_LEN, 

161 ), 

162 ] 

163 

164 

165class DataPacket(EPacket): 

166 name = "Data Packet" 

167 _pl_type = None 

168 _pl_len = 0 

169 

170 fields_desc = [ 

171 XShortField("encapsulationKind", 0), 

172 XShortField("encapsulationOptions", 0), 

173 # if payload encoding == PL_CDR_{LE,BE} then parameter list 

174 ConditionalField( 

175 EPacketField("parameterList", "", ParameterListPacket), 

176 lambda pkt: pkt.encapsulationKind == 0x0003, 

177 ), 

178 # if writer entity id == 0x200c2: then participant message data 

179 ConditionalField( 

180 EPacketField( 

181 "participantMessageData", "", ParticipantMessageDataPacket), 

182 lambda pkt: pkt._pl_type == "ParticipantMessageData", 

183 ), 

184 # else (neither the cases) 

185 ConditionalField( 

186 SerializedDataField( 

187 "serializedData", "", length_from=lambda pkt: pkt._pl_len 

188 ), 

189 lambda pkt: ( 

190 pkt.encapsulationKind != 0x0003 \ 

191 and pkt._pl_type != "ParticipantMessageData" 

192 ), 

193 ), 

194 ] 

195 

196 def __init__(self, *args, **kwargs): 

197 writer_entity_id_key = kwargs.pop("writer_entity_id_key", None) 

198 writer_entity_id_kind = kwargs.pop("writer_entity_id_kind", None) 

199 pl_len = kwargs.pop("pl_len", 0) 

200 if writer_entity_id_key == 0x200 and writer_entity_id_kind == 0xC2: 

201 DataPacket._pl_type = "ParticipantMessageData" 

202 else: 

203 DataPacket._pl_type = "SerializedData" 

204 

205 DataPacket._pl_len = pl_len 

206 

207 super(DataPacket, self).__init__(*args, **kwargs) 

208 

209 

210class RTPSSubMessage_DATA(EPacket): 

211 """ 

212 0...2...........7...............15.............23...............31 

213 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

214 | RTPS_DATA | flags | octetsToNextHeader | 

215 +---------------+---------------+---------------+---------------+ 

216 | Flags extraFlags | octetsToInlineQos | 

217 +---------------+---------------+---------------+---------------+ 

218 | EntityId readerEntityId | 

219 +---------------+---------------+---------------+---------------+ 

220 | EntityId writerEntityId | 

221 +---------------+---------------+---------------+---------------+ 

222 | | 

223 + SequenceNumber writerSeqNum + 

224 | | 

225 +---------------+---------------+---------------+---------------+ 

226 | | 

227 ~ ParameterList inlineQos [only if Q==1] ~ 

228 | | 

229 +---------------+---------------+---------------+---------------+ 

230 | | 

231 ~ SerializedData serializedData [only if D==1 || K==1] ~ 

232 | | 

233 +---------------+---------------+---------------+---------------+ 

234 """ 

235 

236 name = "RTPS DATA (0x15)" 

237 fields_desc = [ 

238 XByteField("submessageId", 0x15), 

239 XByteField("submessageFlags", 0x00), 

240 EField(ShortField("octetsToNextHeader", 0), 

241 endianness_from=e_flags), 

242 XNBytesField("extraFlags", 0x0000, 2), 

243 EField(ShortField("octetsToInlineQoS", 0), 

244 endianness_from=e_flags), 

245 X3BytesField("readerEntityIdKey", 0), 

246 XByteField("readerEntityIdKind", 0), 

247 X3BytesField("writerEntityIdKey", 0), 

248 XByteField("writerEntityIdKind", 0), 

249 # EnumField( 

250 # "reader_id", 

251 # default=b"\x00\x00\x00\x00", 

252 # fmt="4s", 

253 # enum=_rtps_reserved_entity_ids, 

254 # ), 

255 # EnumField( 

256 # "writer_id", 

257 # default=b"\x00\x00\x00\x00", 

258 # fmt="4s", 

259 # enum=_rtps_reserved_entity_ids, 

260 # ), 

261 EField(IntField("writerSeqNumHi", 0), 

262 endianness_from=e_flags), 

263 EField(IntField("writerSeqNumLow", 0), 

264 endianness_from=e_flags), 

265 # ------------------------------------- 

266 ConditionalField( 

267 InlineQoSPacketField("inlineQoS", "", InlineQoSPacket), 

268 lambda pkt: pkt.submessageFlags & 0b00000010 == 0b00000010, 

269 ), 

270 ConditionalField( 

271 DataPacketField("key", "", DataPacket, 

272 endianness_from=e_flags), 

273 lambda pkt: pkt.submessageFlags & 0b00001000 == 0b00001000, 

274 ), 

275 ConditionalField( 

276 DataPacketField("data", "", DataPacket, 

277 endianness_from=e_flags), 

278 lambda pkt: pkt.submessageFlags & 0b00000100 == 0b00000100, 

279 ), 

280 ] 

281 

282 

283class RTPSSubMessage_INFO_TS(EPacket): 

284 """ 

285 0...2...........7...............15.............23...............31 

286 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

287 | INFO_TS | flags | octetsToNextHeader | 

288 +---------------+---------------+---------------+---------------+ 

289 | | 

290 + Timestamp timestamp [only if T==1] + 

291 | | 

292 +---------------+---------------+---------------+---------------+ 

293 """ 

294 

295 name = "RTPS INFO_TS (0x09)" 

296 fields_desc = [ 

297 XByteField("submessageId", 0x09), 

298 FlagsField( 

299 "submessageFlags", 0, 8, 

300 ["E", "I", "?", "?", "?", "?", "?", "?"]), 

301 EField(ShortField("octetsToNextHeader", 0), 

302 endianness_from=e_flags), 

303 ConditionalField( 

304 Field("ts_seconds", default=0, fmt="<l"), 

305 lambda pkt: str(pkt.submessageFlags).find("I"), 

306 ), 

307 ConditionalField( 

308 Field("ts_fraction", default=0, fmt="<L"), 

309 lambda pkt: str(pkt.submessageFlags).find("I"), 

310 ), 

311 ] 

312 

313 

314class RTPSSubMessage_ACKNACK(EPacket): 

315 """ 

316 0...2...........7...............15.............23...............31 

317 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

318 | ACKNACK | flags | octetsToNextHeader | 

319 +---------------+---------------+---------------+---------------+ 

320 | EntityId readerEntityId | 

321 +---------------+---------------+---------------+---------------+ 

322 | EntityId writerEntityId | 

323 +---------------+---------------+---------------+---------------+ 

324 | | 

325 + SequenceNumberSet readerSNState + 

326 | | 

327 +---------------+---------------+---------------+---------------+ 

328 | Counter count | 

329 +---------------+---------------+---------------+---------------+ 

330 """ 

331 

332 name = "RTPS ACKNACK (0x06)" 

333 fields_desc = [ 

334 XByteField("submessageId", 0x06), 

335 XByteField("submessageFlags", 0x00), 

336 EField(ShortField("octetsToNextHeader", 0), 

337 endianness_from=e_flags), 

338 EnumField( 

339 "reader_id", 

340 default=b"\x00\x00\x00\x00", 

341 fmt="4s", 

342 enum=_rtps_reserved_entity_ids, 

343 ), 

344 EnumField( 

345 "writer_id", 

346 default=b"\x00\x00\x00\x00", 

347 fmt="4s", 

348 enum=_rtps_reserved_entity_ids, 

349 ), 

350 XStrLenField( 

351 "readerSNState", 

352 0, length_from=lambda pkt: pkt.octetsToNextHeader - 8 - 4 

353 ), 

354 EField(IntField("count", 0), 

355 endianness_from=e_flags), 

356 ] 

357 

358 

359class RTPSSubMessage_HEARTBEAT(EPacket): 

360 """ 

361 0...2...........7...............15.............23...............31 

362 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

363 | HEARTBEAT | flags | octetsToNextHeader | 

364 +---------------+---------------+---------------+---------------+ 

365 | EntityId readerEntityId | 

366 +---------------+---------------+---------------+---------------+ 

367 | EntityId writerEntityId | 

368 +---------------+---------------+---------------+---------------+ 

369 | | 

370 + SequenceNumber firstAvailableSeqNumber + 

371 | | 

372 +---------------+---------------+---------------+---------------+ 

373 | | 

374 + SequenceNumber lastSeqNumber + 

375 | | 

376 +---------------+---------------+---------------+---------------+ 

377 | Counter count | 

378 +---------------+---------------+---------------+---------------+ 

379 """ 

380 

381 name = "RTPS HEARTBEAT (0x07)" 

382 fields_desc = [ 

383 XByteField("submessageId", 0x07), 

384 XByteField("submessageFlags", 0), 

385 EField(ShortField("octetsToNextHeader", 0), 

386 endianness_from=e_flags), 

387 EnumField( 

388 "reader_id", 

389 default=b"\x00\x00\x00\x00", 

390 fmt="4s", 

391 enum=_rtps_reserved_entity_ids, 

392 ), 

393 EnumField( 

394 "writer_id", 

395 default=b"\x00\x00\x00\x00", 

396 fmt="4s", 

397 enum=_rtps_reserved_entity_ids, 

398 ), 

399 EField(IntField("firstAvailableSeqNumHi", 0), 

400 endianness_from=e_flags), 

401 EField(IntField("firstAvailableSeqNumLow", 0), 

402 endianness_from=e_flags), 

403 EField(IntField("lastSeqNumHi", 0), 

404 endianness_from=e_flags), 

405 EField(IntField("lastSeqNumLow", 0), 

406 endianness_from=e_flags), 

407 EField(IntField("count", 0), 

408 endianness_from=e_flags), 

409 ] 

410 

411 

412class RTPSSubMessage_INFO_DST(EPacket): 

413 """ 

414 0...2...........7...............15.............23...............31 

415 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

416 | INFO_DST | flags | octetsToNextHeader | 

417 +---------------+---------------+---------------+---------------+ 

418 | | 

419 + GuidPrefix guidPrefix + 

420 | | 

421 +---------------+---------------+---------------+---------------+ 

422 """ 

423 

424 name = "RTPS INFO_DTS (0x0e)" 

425 endianness = ">" 

426 

427 fields_desc = [ 

428 XByteField("submessageId", 0x0E), 

429 XByteField("submessageFlags", 0), 

430 EField(ShortField("octetsToNextHeader", 0), 

431 endianness_from=e_flags), 

432 PacketField("guidPrefix", "", GUIDPrefixPacket), 

433 ] 

434 

435 

436class RTPSSubMessage_PAD(EPacket): 

437 """ 

438 0...2...........7...............15.............23...............31 

439 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

440 | PAD | flags | octetsToNextHeader | 

441 +---------------+---------------+---------------+---------------+ 

442 """ 

443 

444 name = "RTPS PAD (0x01)" 

445 fields_desc = [ 

446 XByteField("submessageId", 0x01), 

447 XByteField("submessageFlags", 0), 

448 EField(ShortField("octetsToNextHeader", 0), 

449 endianness_from=e_flags), 

450 ] 

451 

452 

453class RTPSSubMessage_DATA_FRAG(EPacket): 

454 name = "RTPS DATA_FRAG (0x16)" 

455 fields_desc = [StrField("uninterpreted_data", 0)] 

456 

457 

458class RTPSSubMessage_SEC_PREFIX(EPacket): 

459 name = "RTPS SEC_PREFIX (0x31)" 

460 fields_desc = [StrField("uninterpreted_data", 0)] 

461 

462 

463class RTPSSubMessage_SEC_POSTFIX(EPacket): 

464 name = "RTPS SEC_POSTFIX (0x32)" 

465 fields_desc = [StrField("uninterpreted_data", 0)] 

466 

467 

468class RTPSSubMessage_SEC_BODY(EPacket): 

469 name = "RTPS SEC_BODY (0x30)" 

470 fields_desc = [StrField("uninterpreted_data", 0)] 

471 

472 

473class RTPSSubMessage_SRTPS_PREFIX(EPacket): 

474 name = "RTPS SRPTS_PREFIX (0x33)" 

475 fields_desc = [StrField("uninterpreted_data", 0)] 

476 

477 

478class RTPSSubMessage_SRTPS_POSTFIX(EPacket): 

479 name = "RTPS SRPTS_POSTFIX (0x34)" 

480 fields_desc = [StrField("uninterpreted_data", 0)] 

481 

482 

483class RTPSSubMessage_GAP(EPacket): 

484 name = "RTPS GAP (0x08)" 

485 fields_desc = [StrField("uninterpreted_data", 0)] 

486 

487 

488_RTPSSubMessageTypes = { 

489 0x01: RTPSSubMessage_PAD, 

490 0x06: RTPSSubMessage_ACKNACK, 

491 0x07: RTPSSubMessage_HEARTBEAT, 

492 0x09: RTPSSubMessage_INFO_TS, 

493 0x0E: RTPSSubMessage_INFO_DST, 

494 0x15: RTPSSubMessage_DATA, 

495 # ---------------------------- 

496 0x16: RTPSSubMessage_DATA_FRAG, 

497 0x31: RTPSSubMessage_SEC_PREFIX, 

498 0x32: RTPSSubMessage_SEC_POSTFIX, 

499 0x30: RTPSSubMessage_SEC_BODY, 

500 0x33: RTPSSubMessage_SRTPS_PREFIX, 

501 0x34: RTPSSubMessage_SRTPS_POSTFIX, 

502 0x08: RTPSSubMessage_GAP, 

503} 

504 

505 

506def _next_cls_cb(pkt, lst, p, remain): 

507 sm_id = struct.unpack("!b", remain[0:1])[0] 

508 next_cls = _RTPSSubMessageTypes.get(sm_id, None) 

509 

510 return next_cls 

511 

512 

513class RTPSMessage(Packet): 

514 name = "RTPS Message" 

515 fields_desc = [ 

516 PacketListField("submessages", [], next_cls_cb=_next_cls_cb) 

517 ] 

518 

519 

520bind_layers(RTPS, RTPSMessage, magic=b"RTPS") 

521bind_layers(RTPS, RTPSMessage, magic=b"RTPX")