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 or writer_entity_id_key == 0x100) and \ 

201 writer_entity_id_kind == 0xC2: 

202 DataPacket._pl_type = "ParticipantMessageData" 

203 else: 

204 DataPacket._pl_type = "SerializedData" 

205 

206 DataPacket._pl_len = pl_len 

207 

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

209 

210 

211class RTPSSubMessage_DATA(EPacket): 

212 """ 

213 0...2...........7...............15.............23...............31 

214 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

215 | RTPS_DATA | flags | octetsToNextHeader | 

216 +---------------+---------------+---------------+---------------+ 

217 | Flags extraFlags | octetsToInlineQos | 

218 +---------------+---------------+---------------+---------------+ 

219 | EntityId readerEntityId | 

220 +---------------+---------------+---------------+---------------+ 

221 | EntityId writerEntityId | 

222 +---------------+---------------+---------------+---------------+ 

223 | | 

224 + SequenceNumber writerSeqNum + 

225 | | 

226 +---------------+---------------+---------------+---------------+ 

227 | | 

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

229 | | 

230 +---------------+---------------+---------------+---------------+ 

231 | | 

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

233 | | 

234 +---------------+---------------+---------------+---------------+ 

235 """ 

236 

237 name = "RTPS DATA (0x15)" 

238 fields_desc = [ 

239 XByteField("submessageId", 0x15), 

240 XByteField("submessageFlags", 0x00), 

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

242 endianness_from=e_flags), 

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

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

245 endianness_from=e_flags), 

246 X3BytesField("readerEntityIdKey", 0), 

247 XByteField("readerEntityIdKind", 0), 

248 X3BytesField("writerEntityIdKey", 0), 

249 XByteField("writerEntityIdKind", 0), 

250 # EnumField( 

251 # "reader_id", 

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

253 # fmt="4s", 

254 # enum=_rtps_reserved_entity_ids, 

255 # ), 

256 # EnumField( 

257 # "writer_id", 

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

259 # fmt="4s", 

260 # enum=_rtps_reserved_entity_ids, 

261 # ), 

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

263 endianness_from=e_flags), 

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

265 endianness_from=e_flags), 

266 # ------------------------------------- 

267 ConditionalField( 

268 InlineQoSPacketField("inlineQoS", "", InlineQoSPacket), 

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

270 ), 

271 ConditionalField( 

272 DataPacketField("key", "", DataPacket, 

273 endianness_from=e_flags), 

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

275 ), 

276 ConditionalField( 

277 DataPacketField("data", "", DataPacket, 

278 endianness_from=e_flags), 

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

280 ), 

281 ] 

282 

283 

284class RTPSSubMessage_INFO_TS(EPacket): 

285 """ 

286 0...2...........7...............15.............23...............31 

287 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

288 | INFO_TS | flags | octetsToNextHeader | 

289 +---------------+---------------+---------------+---------------+ 

290 | | 

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

292 | | 

293 +---------------+---------------+---------------+---------------+ 

294 """ 

295 

296 name = "RTPS INFO_TS (0x09)" 

297 fields_desc = [ 

298 XByteField("submessageId", 0x09), 

299 FlagsField( 

300 "submessageFlags", 0, 8, 

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

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

303 endianness_from=e_flags), 

304 ConditionalField( 

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

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

307 ), 

308 ConditionalField( 

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

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

311 ), 

312 ] 

313 

314 

315class RTPSSubMessage_ACKNACK(EPacket): 

316 """ 

317 0...2...........7...............15.............23...............31 

318 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

319 | ACKNACK | flags | octetsToNextHeader | 

320 +---------------+---------------+---------------+---------------+ 

321 | EntityId readerEntityId | 

322 +---------------+---------------+---------------+---------------+ 

323 | EntityId writerEntityId | 

324 +---------------+---------------+---------------+---------------+ 

325 | | 

326 + SequenceNumberSet readerSNState + 

327 | | 

328 +---------------+---------------+---------------+---------------+ 

329 | Counter count | 

330 +---------------+---------------+---------------+---------------+ 

331 """ 

332 

333 name = "RTPS ACKNACK (0x06)" 

334 fields_desc = [ 

335 XByteField("submessageId", 0x06), 

336 XByteField("submessageFlags", 0x00), 

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

338 endianness_from=e_flags), 

339 EnumField( 

340 "reader_id", 

341 default=b"\x00\x00\x00\x00", 

342 fmt="4s", 

343 enum=_rtps_reserved_entity_ids, 

344 ), 

345 EnumField( 

346 "writer_id", 

347 default=b"\x00\x00\x00\x00", 

348 fmt="4s", 

349 enum=_rtps_reserved_entity_ids, 

350 ), 

351 XStrLenField( 

352 "readerSNState", 

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

354 ), 

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

356 endianness_from=e_flags), 

357 ] 

358 

359 

360class RTPSSubMessage_HEARTBEAT(EPacket): 

361 """ 

362 0...2...........7...............15.............23...............31 

363 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

364 | HEARTBEAT | flags | octetsToNextHeader | 

365 +---------------+---------------+---------------+---------------+ 

366 | EntityId readerEntityId | 

367 +---------------+---------------+---------------+---------------+ 

368 | EntityId writerEntityId | 

369 +---------------+---------------+---------------+---------------+ 

370 | | 

371 + SequenceNumber firstAvailableSeqNumber + 

372 | | 

373 +---------------+---------------+---------------+---------------+ 

374 | | 

375 + SequenceNumber lastSeqNumber + 

376 | | 

377 +---------------+---------------+---------------+---------------+ 

378 | Counter count | 

379 +---------------+---------------+---------------+---------------+ 

380 """ 

381 

382 name = "RTPS HEARTBEAT (0x07)" 

383 fields_desc = [ 

384 XByteField("submessageId", 0x07), 

385 XByteField("submessageFlags", 0), 

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

387 endianness_from=e_flags), 

388 EnumField( 

389 "reader_id", 

390 default=b"\x00\x00\x00\x00", 

391 fmt="4s", 

392 enum=_rtps_reserved_entity_ids, 

393 ), 

394 EnumField( 

395 "writer_id", 

396 default=b"\x00\x00\x00\x00", 

397 fmt="4s", 

398 enum=_rtps_reserved_entity_ids, 

399 ), 

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

401 endianness_from=e_flags), 

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

403 endianness_from=e_flags), 

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

405 endianness_from=e_flags), 

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

407 endianness_from=e_flags), 

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

409 endianness_from=e_flags), 

410 ] 

411 

412 

413class RTPSSubMessage_INFO_DST(EPacket): 

414 """ 

415 0...2...........7...............15.............23...............31 

416 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

417 | INFO_DST | flags | octetsToNextHeader | 

418 +---------------+---------------+---------------+---------------+ 

419 | | 

420 + GuidPrefix guidPrefix + 

421 | | 

422 +---------------+---------------+---------------+---------------+ 

423 """ 

424 

425 name = "RTPS INFO_DTS (0x0e)" 

426 endianness = ">" 

427 

428 fields_desc = [ 

429 XByteField("submessageId", 0x0E), 

430 XByteField("submessageFlags", 0), 

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

432 endianness_from=e_flags), 

433 PacketField("guidPrefix", "", GUIDPrefixPacket), 

434 ] 

435 

436 

437class RTPSSubMessage_PAD(EPacket): 

438 """ 

439 0...2...........7...............15.............23...............31 

440 +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+ 

441 | PAD | flags | octetsToNextHeader | 

442 +---------------+---------------+---------------+---------------+ 

443 """ 

444 

445 name = "RTPS PAD (0x01)" 

446 fields_desc = [ 

447 XByteField("submessageId", 0x01), 

448 XByteField("submessageFlags", 0), 

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

450 endianness_from=e_flags), 

451 ] 

452 

453 

454class RTPSSubMessage_DATA_FRAG(EPacket): 

455 name = "RTPS DATA_FRAG (0x16)" 

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

457 

458 

459class RTPSSubMessage_SEC_PREFIX(EPacket): 

460 name = "RTPS SEC_PREFIX (0x31)" 

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

462 

463 

464class RTPSSubMessage_SEC_POSTFIX(EPacket): 

465 name = "RTPS SEC_POSTFIX (0x32)" 

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

467 

468 

469class RTPSSubMessage_SEC_BODY(EPacket): 

470 name = "RTPS SEC_BODY (0x30)" 

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

472 

473 

474class RTPSSubMessage_SRTPS_PREFIX(EPacket): 

475 name = "RTPS SRPTS_PREFIX (0x33)" 

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

477 

478 

479class RTPSSubMessage_SRTPS_POSTFIX(EPacket): 

480 name = "RTPS SRPTS_POSTFIX (0x34)" 

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

482 

483 

484class RTPSSubMessage_GAP(EPacket): 

485 name = "RTPS GAP (0x08)" 

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

487 

488 

489_RTPSSubMessageTypes = { 

490 0x01: RTPSSubMessage_PAD, 

491 0x06: RTPSSubMessage_ACKNACK, 

492 0x07: RTPSSubMessage_HEARTBEAT, 

493 0x09: RTPSSubMessage_INFO_TS, 

494 0x0E: RTPSSubMessage_INFO_DST, 

495 0x15: RTPSSubMessage_DATA, 

496 # ---------------------------- 

497 0x16: RTPSSubMessage_DATA_FRAG, 

498 0x31: RTPSSubMessage_SEC_PREFIX, 

499 0x32: RTPSSubMessage_SEC_POSTFIX, 

500 0x30: RTPSSubMessage_SEC_BODY, 

501 0x33: RTPSSubMessage_SRTPS_PREFIX, 

502 0x34: RTPSSubMessage_SRTPS_POSTFIX, 

503 0x08: RTPSSubMessage_GAP, 

504} 

505 

506 

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

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

509 next_cls = _RTPSSubMessageTypes.get(sm_id, None) 

510 

511 return next_cls 

512 

513 

514class RTPSMessage(Packet): 

515 name = "RTPS Message" 

516 fields_desc = [ 

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

518 ] 

519 

520 

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

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