活动介绍

constexpr uint8_t k_max_id = (constexpr)max_id;

时间: 2023-07-01 16:14:56 浏览: 157
在这个代码中,你试图将一个非`constexpr`类型的变量`max_id`强制转换为`constexpr`类型,并将其赋值给`constexpr`变量`k_max_id`。这是错误的,因为一个非`constexpr`类型的变量不能被强制转换为`constexpr`类型。 要将一个变量声明为`constexpr`类型,必须在声明时使用常量表达式进行初始化,例如: ```cpp constexpr uint8_t k_max_id = 255; // 正确:使用常量表达式初始化 k_max_id ``` 如果你想将一个非`constexpr`类型的变量转换为`constexpr`类型,你可以考虑使用编译时计算的方式,例如使用模板元编程或`constexpr`函数来计算值,例如: ```cpp template <typename T> constexpr T to_constexpr(T value) { return value; } constexpr uint8_t k_max_id = to_constexpr(max_id); // 可行:使用编译时计算来初始化 k_max_id ``` 或者: ```cpp constexpr uint8_t to_constexpr(uint8_t value) { return value; } constexpr uint8_t k_max_id = to_constexpr(max_id); // 可行:使用 constexpr 函数来初始化 k_max_id ``` 在这两个例子中,我们使用了一个`constexpr`函数或模板函数来将一个运行时值转换为`constexpr`值,从而能够在编译时进行初始化。
阅读全文

相关推荐

写出下面代码用到的AT命令,讲解每个AT命令的具体的含义: #include <base/functional/bind.h> 27 #include <base/functional/callback_forward.h> 28 #include <string.h> 29 30 #include "device/include/esco_parameters.h" 31 #include "hcidefs.h" 32 #include "hcimsgs.h" 33 #include "internal_include/bt_target.h" 34 #include "main/shim/acl_api.h" 35 #include "osi/include/allocator.h" 36 #include "stack/include/bt_dev_class.h" 37 #include "stack/include/bt_hdr.h" 38 #include "stack/include/bt_lap.h" 39 #include "stack/include/bt_octets.h" 40 #include "stack/include/bt_types.h" 41 #include "stack/include/btu_hcif.h" 42 #include "types/raw_address.h" 43 44 /* Message by message.... */ 45 46 #define HCIC_PARAM_SIZE_INQUIRY 5 47 48 #define HCIC_INQ_INQ_LAP_OFF 0 49 #define HCIC_INQ_DUR_OFF 3 50 #define HCIC_INQ_RSP_CNT_OFF 4 51 52 /* Periodic Inquiry Mode */ 53 #define HCIC_PARAM_SIZE_PER_INQ_MODE 9 54 55 #define HCI_PER_INQ_MAX_INTRVL_OFF 0 56 #define HCI_PER_INQ_MIN_INTRVL_OFF 2 57 #define HCI_PER_INQ_INQ_LAP_OFF 4 58 #define HCI_PER_INQ_DURATION_OFF 7 59 #define HCI_PER_INQ_RSP_CNT_OFF 8 60 /* Periodic Inquiry Mode */ 61 62 /* Exit Periodic Inquiry Mode */ 63 #define HCIC_PARAM_SIZE_EXIT_PER_INQ 0 64 65 /* Create Connection */ 66 #define HCIC_PARAM_SIZE_CREATE_CONN 13 67 68 #define HCIC_CR_CONN_BD_ADDR_OFF 0 69 #define HCIC_CR_CONN_PKT_TYPES_OFF 6 70 #define HCIC_CR_CONN_REP_MODE_OFF 8 71 #define HCIC_CR_CONN_PAGE_SCAN_MODE_OFF 9 72 #define HCIC_CR_CONN_CLK_OFF_OFF 10 73 #define HCIC_CR_CONN_ALLOW_SWITCH_OFF 12 74 /* Create Connection */ 75 76 /* Disconnect */ 77 #define HCIC_PARAM_SIZE_DISCONNECT 3 78 79 #define HCI_DISC_HANDLE_OFF 0 80 #define HCI_DISC_REASON_OFF 2 81 /* Disconnect */ 82 83 /* Add SCO Connection */ 84 #define HCIC_PARAM_SIZE_ADD_SCO_CONN 4 85 86 #define HCI_ADD_SCO_HANDLE_OFF 0 87 #define HCI_ADD_SCO_PACKET_TYPES_OFF 2 88 /* Add SCO Connection */ 89 90 /* Create Connection Cancel */ 91 #define HCIC_PARAM_SIZE_CREATE_CONN_CANCEL 6 92 93 #define HCIC_CR_CONN_CANCEL_BD_ADDR_OFF 0 94 /* Create Connection Cancel */ 95 96 /* Accept Connection Request */ 97 #define HCIC_PARAM_SIZE_ACCEPT_CONN 7 98 99 #define HCI_ACC_CONN_BD_ADDR_OFF 0 100 #define HCI_ACC_CONN_ROLE_OFF 6 101 /* Accept Connection Request */ 102 103 /* Reject Connection Request */ 104 #define HCIC_PARAM_SIZE_REJECT_CONN 7 105 106 #define HCI_REJ_CONN_BD_ADDR_OFF 0 107 #define HCI_REJ_CONN_REASON_OFF 6 108 /* Reject Connection Request */ 109 110 /* Link Key Request Reply */ 111 #define HCIC_PARAM_SIZE_LINK_KEY_REQ_REPLY 22 112 113 #define HCI_LINK_KEY_REPLY_BD_ADDR_OFF 0 114 #define HCI_LINK_KEY_REPLY_LINK_KEY_OFF 6 115 /* Link Key Request Reply */ 116 117 /* Link Key Request Neg Reply */ 118 #define HCIC_PARAM_SIZE_LINK_KEY_NEG_REPLY 6 119 120 #define HCI_LINK_KEY_NEG_REP_BD_ADR_OFF 0 121 /* Link Key Request Neg Reply */ 122 123 /* PIN Code Request Reply */ 124 #define HCIC_PARAM_SIZE_PIN_CODE_REQ_REPLY 23 125 126 #define HCI_PIN_CODE_REPLY_BD_ADDR_OFF 0 127 #define HCI_PIN_CODE_REPLY_PIN_LEN_OFF 6 128 #define HCI_PIN_CODE_REPLY_PIN_CODE_OFF 7 129 /* PIN Code Request Reply */ 130 131 /* Link Key Request Neg Reply */ 132 #define HCIC_PARAM_SIZE_PIN_CODE_NEG_REPLY 6 133 134 #define HCI_PIN_CODE_NEG_REP_BD_ADR_OFF 0 135 /* Link Key Request Neg Reply */ 136 137 /* Change Connection Type */ 138 #define HCIC_PARAM_SIZE_CHANGE_CONN_TYPE 4 139 140 #define HCI_CHNG_PKT_TYPE_HANDLE_OFF 0 141 #define HCI_CHNG_PKT_TYPE_PKT_TYPE_OFF 2 142 /* Change Connection Type */ 143 144 #define HCIC_PARAM_SIZE_CMD_HANDLE 2 145 146 #define HCI_CMD_HANDLE_HANDLE_OFF 0 147 148 /* Set Connection Encryption */ 149 #define HCIC_PARAM_SIZE_SET_CONN_ENCRYPT 3 150 151 #define HCI_SET_ENCRYPT_HANDLE_OFF 0 152 #define HCI_SET_ENCRYPT_ENABLE_OFF 2 153 /* Set Connection Encryption */ 154 155 /* Remote Name Request */ 156 #define HCIC_PARAM_SIZE_RMT_NAME_REQ 10 157 158 #define HCI_RMT_NAME_BD_ADDR_OFF 0 159 #define HCI_RMT_NAME_REP_MODE_OFF 6 160 #define HCI_RMT_NAME_PAGE_SCAN_MODE_OFF 7 161 #define HCI_RMT_NAME_CLK_OFF_OFF 8 162 /* Remote Name Request */ 163 164 /* Remote Name Request Cancel */ 165 #define HCIC_PARAM_SIZE_RMT_NAME_REQ_CANCEL 6 166 167 #define HCI_RMT_NAME_CANCEL_BD_ADDR_OFF 0 168 /* Remote Name Request Cancel */ 169 170 /* Remote Extended Features */ 171 #define HCIC_PARAM_SIZE_RMT_EXT_FEATURES 3 172 173 #define HCI_RMT_EXT_FEATURES_HANDLE_OFF 0 174 #define HCI_RMT_EXT_FEATURES_PAGE_NUM_OFF 2 175 /* Remote Extended Features */ 176 177 #define HCIC_PARAM_SIZE_SETUP_ESCO 17 178 179 #define HCI_SETUP_ESCO_HANDLE_OFF 0 180 #define HCI_SETUP_ESCO_TX_BW_OFF 2 181 #define HCI_SETUP_ESCO_RX_BW_OFF 6 182 #define HCI_SETUP_ESCO_MAX_LAT_OFF 10 183 #define HCI_SETUP_ESCO_VOICE_OFF 12 184 #define HCI_SETUP_ESCO_RETRAN_EFF_OFF 14 185 #define HCI_SETUP_ESCO_PKT_TYPES_OFF 15 186 187 #define HCIC_PARAM_SIZE_ACCEPT_ESCO 21 188 189 #define HCI_ACCEPT_ESCO_BDADDR_OFF 0 190 #define HCI_ACCEPT_ESCO_TX_BW_OFF 6 191 #define HCI_ACCEPT_ESCO_RX_BW_OFF 10 192 #define HCI_ACCEPT_ESCO_MAX_LAT_OFF 14 193 #define HCI_ACCEPT_ESCO_VOICE_OFF 16 194 #define HCI_ACCEPT_ESCO_RETRAN_EFF_OFF 18 195 #define HCI_ACCEPT_ESCO_PKT_TYPES_OFF 19 196 197 #define HCIC_PARAM_SIZE_REJECT_ESCO 7 198 199 #define HCI_REJECT_ESCO_BDADDR_OFF 0 200 #define HCI_REJECT_ESCO_REASON_OFF 6 201 202 /* Hold Mode */ 203 #define HCIC_PARAM_SIZE_HOLD_MODE 6 204 205 #define HCI_HOLD_MODE_HANDLE_OFF 0 206 #define HCI_HOLD_MODE_MAX_PER_OFF 2 207 #define HCI_HOLD_MODE_MIN_PER_OFF 4 208 /* Hold Mode */ 209 210 /* Sniff Mode */ 211 #define HCIC_PARAM_SIZE_SNIFF_MODE 10 212 213 #define HCI_SNIFF_MODE_HANDLE_OFF 0 214 #define HCI_SNIFF_MODE_MAX_PER_OFF 2 215 #define HCI_SNIFF_MODE_MIN_PER_OFF 4 216 #define HCI_SNIFF_MODE_ATTEMPT_OFF 6 217 #define HCI_SNIFF_MODE_TIMEOUT_OFF 8 218 /* Sniff Mode */ 219 220 /* Park Mode */ 221 #define HCIC_PARAM_SIZE_PARK_MODE 6 222 223 #define HCI_PARK_MODE_HANDLE_OFF 0 224 #define HCI_PARK_MODE_MAX_PER_OFF 2 225 #define HCI_PARK_MODE_MIN_PER_OFF 4 226 /* Park Mode */ 227 228 /* QoS Setup */ 229 #define HCIC_PARAM_SIZE_QOS_SETUP 20 230 231 #define HCI_QOS_HANDLE_OFF 0 232 #define HCI_QOS_FLAGS_OFF 2 233 #define HCI_QOS_SERVICE_TYPE_OFF 3 234 #define HCI_QOS_TOKEN_RATE_OFF 4 235 #define HCI_QOS_PEAK_BANDWIDTH_OFF 8 236 #define HCI_QOS_LATENCY_OFF 12 237 #define HCI_QOS_DELAY_VAR_OFF 16 238 /* QoS Setup */ 239 240 #define HCIC_PARAM_SIZE_SWITCH_ROLE 7 241 242 #define HCI_SWITCH_BD_ADDR_OFF 0 243 #define HCI_SWITCH_ROLE_OFF 6 244 /* Switch Role Request */ 245 246 /* Write Policy Settings */ 247 #define HCIC_PARAM_SIZE_WRITE_POLICY_SET 4 248 249 #define HCI_WRITE_POLICY_HANDLE_OFF 0 250 #define HCI_WRITE_POLICY_SETTINGS_OFF 2 251 /* Write Policy Settings */ 252 253 /* Write Default Policy Settings */ 254 #define HCIC_PARAM_SIZE_WRITE_DEF_POLICY_SET 2 255 256 #define HCI_WRITE_DEF_POLICY_SETTINGS_OFF 0 257 /* Write Default Policy Settings */ 258 259 #define HCIC_PARAM_SIZE_SNIFF_SUB_RATE 8 260 261 #define HCI_SNIFF_SUB_RATE_HANDLE_OFF 0 262 #define HCI_SNIFF_SUB_RATE_MAX_LAT_OFF 2 263 #define HCI_SNIFF_SUB_RATE_MIN_REM_LAT_OFF 4 264 #define HCI_SNIFF_SUB_RATE_MIN_LOC_LAT_OFF 6 265 /* Sniff Subrating */ 266 267 /* Extended Inquiry Response */ 268 #define HCIC_PARAM_SIZE_EXT_INQ_RESP 241 269 270 #define HCIC_EXT_INQ_RESP_FEC_OFF 0 271 #define HCIC_EXT_INQ_RESP_RESPONSE 1 272 /* IO Capabilities Response */ 273 #define HCIC_PARAM_SIZE_IO_CAP_RESP 9 274 275 #define HCI_IO_CAP_BD_ADDR_OFF 0 276 #define HCI_IO_CAPABILITY_OFF 6 277 #define HCI_IO_CAP_OOB_DATA_OFF 7 278 #define HCI_IO_CAP_AUTH_REQ_OFF 8 279 280 /* IO Capabilities Req Neg Reply */ 281 #define HCIC_PARAM_SIZE_IO_CAP_NEG_REPLY 7 282 283 #define HCI_IO_CAP_NR_BD_ADDR_OFF 0 284 #define HCI_IO_CAP_NR_ERR_CODE 6 285 286 /* Read Local OOB Data */ 287 #define HCIC_PARAM_SIZE_R_LOCAL_OOB 0 288 289 /* Read Local OOB Extended Data */ 290 #define HCIC_PARAM_SIZE_R_LOCAL_OOB_EXTENDED 0 291 292 #define HCIC_PARAM_SIZE_UCONF_REPLY 6 293 294 #define HCI_USER_CONF_BD_ADDR_OFF 0 295 296 #define HCIC_PARAM_SIZE_U_PKEY_REPLY 10 297 298 #define HCI_USER_PASSKEY_BD_ADDR_OFF 0 299 #define HCI_USER_PASSKEY_VALUE_OFF 6 300 301 #define HCIC_PARAM_SIZE_U_PKEY_NEG_REPLY 6 302 303 #define HCI_USER_PASSKEY_NEG_BD_ADDR_OFF 0 304 305 /* Remote OOB Data Request Reply */ 306 #define HCIC_PARAM_SIZE_REM_OOB_REPLY 38 307 308 #define HCI_REM_OOB_DATA_BD_ADDR_OFF 0 309 #define HCI_REM_OOB_DATA_C_OFF 6 310 #define HCI_REM_OOB_DATA_R_OFF 22 311 312 /* Remote OOB Data Request Negative Reply */ 313 #define HCIC_PARAM_SIZE_REM_OOB_NEG_REPLY 6 314 315 #define HCI_REM_OOB_DATA_NEG_BD_ADDR_OFF 0 316 317 /* Read Tx Power Level */ 318 #define HCIC_PARAM_SIZE_R_TX_POWER 0 319 320 /* Read Default Erroneous Data Reporting */ 321 #define HCIC_PARAM_SIZE_R_ERR_DATA_RPT 0 322 323 #define HCIC_PARAM_SIZE_SEND_KEYPRESS_NOTIF 7 324 325 #define HCI_SEND_KEYPRESS_NOTIF_BD_ADDR_OFF 0 326 #define HCI_SEND_KEYPRESS_NOTIF_NOTIF_OFF 6 327 328 /**** end of Simple Pairing Commands ****/ 329 330 #define HCIC_PARAM_SIZE_SET_EVT_FILTER 9 331 332 #define HCI_FILT_COND_FILT_TYPE_OFF 0 333 #define HCI_FILT_COND_COND_TYPE_OFF 1 334 #define HCI_FILT_COND_FILT_OFF 2 335 /* Set Event Filter */ 336 337 /* Delete Stored Key */ 338 #define HCIC_PARAM_SIZE_DELETE_STORED_KEY 7 339 340 #define HCI_DELETE_KEY_BD_ADDR_OFF 0 341 #define HCI_DELETE_KEY_ALL_FLAG_OFF 6 342 /* Delete Stored Key */ 343 344 /* Change Local Name */ 345 #define HCIC_PARAM_SIZE_CHANGE_NAME BD_NAME_LEN 346 347 #define HCI_CHANGE_NAME_NAME_OFF 0 348 /* Change Local Name */ 349 350 #define HCIC_PARAM_SIZE_READ_CMD 0 351 352 #define HCIC_PARAM_SIZE_WRITE_PARAM1 1 353 354 #define HCIC_WRITE_PARAM1_PARAM_OFF 0 355 356 #define HCIC_PARAM_SIZE_WRITE_PARAM2 2 357 358 #define HCIC_WRITE_PARAM2_PARAM_OFF 0 359 360 #define HCIC_PARAM_SIZE_WRITE_PARAM3 3 361 362 #define HCIC_WRITE_PARAM3_PARAM_OFF 0 363 364 #define HCIC_PARAM_SIZE_SET_AFH_CHANNELS 10 365 366 #define HCIC_PARAM_SIZE_ENH_SET_ESCO_CONN 59 367 #define HCIC_PARAM_SIZE_ENH_ACC_ESCO_CONN 63 368 369 #define HCIC_PARAM_SIZE_WRITE_PAGESCAN_CFG 4 370 371 #define HCI_SCAN_CFG_INTERVAL_OFF 0 372 #define HCI_SCAN_CFG_WINDOW_OFF 2 373 /* Write Page Scan Activity */ 374 375 /* Write Inquiry Scan Activity */ 376 #define HCIC_PARAM_SIZE_WRITE_INQSCAN_CFG 4 377 378 #define HCI_SCAN_CFG_INTERVAL_OFF 0 379 #define HCI_SCAN_CFG_WINDOW_OFF 2 380 /* Write Inquiry Scan Activity */ 381 382 /* Host Controller to Host flow control */ 383 #define HCI_HOST_FLOW_CTRL_OFF 0 384 #define HCI_HOST_FLOW_CTRL_ACL_ON 1 385 #define HCI_HOST_FLOW_CTRL_SCO_ON 2 386 #define HCI_HOST_FLOW_CTRL_BOTH_ON 3 387 388 #define HCIC_PARAM_SIZE_WRITE_AUTOMATIC_FLUSH_TIMEOUT 4 389 390 #define HCI_FLUSH_TOUT_HANDLE_OFF 0 391 #define HCI_FLUSH_TOUT_TOUT_OFF 2 392 393 #define HCIC_PARAM_SIZE_READ_TX_POWER 3 394 395 #define HCI_READ_TX_POWER_HANDLE_OFF 0 396 #define HCI_READ_TX_POWER_TYPE_OFF 2 397 398 /* Read transmit power level parameter */ 399 #define HCI_READ_CURRENT 0x00 400 #define HCI_READ_MAXIMUM 0x01 401 402 #define HCIC_PARAM_SIZE_NUM_PKTS_DONE_SIZE sizeof(btmsg_hcic_num_pkts_done_t) 403 404 #define MAX_DATA_HANDLES 10 405 406 #define HCI_PKTS_DONE_NUM_HANDLES_OFF 0 407 #define HCI_PKTS_DONE_HANDLE_OFF 1 408 #define HCI_PKTS_DONE_NUM_PKTS_OFF 3 409 410 #define HCIC_PARAM_SIZE_WRITE_LINK_SUPER_TOUT 4 411 412 #define HCI_LINK_SUPER_TOUT_HANDLE_OFF 0 413 #define HCI_LINK_SUPER_TOUT_TOUT_OFF 2 414 /* Write Link Supervision Timeout */ 415 416 #define MAX_IAC_LAPS 0x40 417 418 #define HCI_WRITE_IAC_LAP_NUM_OFF 0 419 #define HCI_WRITE_IAC_LAP_LAP_OFF 1 420 /* Write Current IAC LAP */ 421 422 #define HCIC_PARAM_SIZE_CONFIGURE_DATA_PATH 3 423 424 /******************************************************************************* 425 * BLE Commands 426 * Note: "local_controller_id" is for transport, not counted in HCI 427 * message size 428 ******************************************************************************/ 429 #define HCIC_BLE_RAND_DI_SIZE 8 430 #define HCIC_BLE_IRK_SIZE 16 431 432 #define HCIC_PARAM_SIZE_SET_USED_FEAT_CMD 8 433 #define HCIC_PARAM_SIZE_WRITE_RANDOM_ADDR_CMD 6 434 #define HCIC_PARAM_SIZE_BLE_WRITE_ADV_PARAMS 15 435 #define HCIC_PARAM_SIZE_BLE_WRITE_SCAN_RSP 31 436 #define HCIC_PARAM_SIZE_WRITE_ADV_ENABLE 1 437 #define HCIC_PARAM_SIZE_BLE_WRITE_SCAN_PARAM 7 438 #define HCIC_PARAM_SIZE_BLE_WRITE_SCAN_ENABLE 2 439 #define HCIC_PARAM_SIZE_BLE_CREATE_LL_CONN 25 440 #define HCIC_PARAM_SIZE_BLE_CREATE_CONN_CANCEL 0 441 #define HCIC_PARAM_SIZE_CLEAR_ACCEPTLIST 0 442 #define HCIC_PARAM_SIZE_ADD_ACCEPTLIST 7 443 #define HCIC_PARAM_SIZE_REMOVE_ACCEPTLIST 7 444 #define HCIC_PARAM_SIZE_BLE_UPD_LL_CONN_PARAMS 14 445 #define HCIC_PARAM_SIZE_SET_HOST_CHNL_CLASS 5 446 #define HCIC_PARAM_SIZE_READ_CHNL_MAP 2 447 #define HCIC_PARAM_SIZE_BLE_READ_REMOTE_FEAT 2 448 #define HCIC_PARAM_SIZE_BLE_ENCRYPT 32 449 #define HCIC_PARAM_SIZE_WRITE_LE_HOST_SUPPORTED 2 450 451 #define HCIC_BLE_RAND_DI_SIZE 8 452 #define HCIC_BLE_ENCRYPT_KEY_SIZE 16 453 #define HCIC_PARAM_SIZE_BLE_START_ENC \ 454 (4 + HCIC_BLE_RAND_DI_SIZE + HCIC_BLE_ENCRYPT_KEY_SIZE) 455 #define HCIC_PARAM_SIZE_LTK_REQ_REPLY (2 + HCIC_BLE_ENCRYPT_KEY_SIZE) 456 #define HCIC_PARAM_SIZE_LTK_REQ_NEG_REPLY 2 457 #define HCIC_BLE_CHNL_MAP_SIZE 5 458 #define HCIC_PARAM_SIZE_BLE_WRITE_ADV_DATA 31 459 460 #define HCIC_PARAM_SIZE_BLE_ADD_DEV_RESOLVING_LIST (7 + HCIC_BLE_IRK_SIZE * 2) 461 #define HCIC_PARAM_SIZE_BLE_RM_DEV_RESOLVING_LIST 7 462 #define HCIC_PARAM_SIZE_BLE_SET_PRIVACY_MODE 8 463 #define HCIC_PARAM_SIZE_BLE_CLEAR_RESOLVING_LIST 0 464 #define HCIC_PARAM_SIZE_BLE_READ_RESOLVING_LIST_SIZE 0 465 #define HCIC_PARAM_SIZE_BLE_READ_RESOLVABLE_ADDR_PEER 7 466 #define HCIC_PARAM_SIZE_BLE_READ_RESOLVABLE_ADDR_LOCAL 7 467 #define HCIC_PARAM_SIZE_BLE_SET_ADDR_RESOLUTION_ENABLE 1 468 #define HCIC_PARAM_SIZE_BLE_SET_RAND_PRIV_ADDR_TIMOUT 2 469 470 #define HCIC_PARAM_SIZE_BLE_READ_PHY 2 471 #define HCIC_PARAM_SIZE_BLE_SET_DEFAULT_PHY 3 472 #define HCIC_PARAM_SIZE_BLE_SET_PHY 7 473 #define HCIC_PARAM_SIZE_BLE_ENH_RX_TEST 3 474 #define HCIC_PARAM_SIZE_BLE_ENH_TX_TEST 4 475 476 #define HCIC_PARAM_SIZE_BLE_SET_DATA_LENGTH 6 477 #define HCIC_PARAM_SIZE_BLE_WRITE_EXTENDED_SCAN_PARAM 11 478 479 #define HCIC_PARAM_SIZE_BLE_RC_PARAM_REQ_REPLY 14 480 #define HCIC_PARAM_SIZE_BLE_RC_PARAM_REQ_NEG_REPLY 3 481 482 static void btsnd_hcic_disconnect(uint16_t handle, uint8_t reason) { 483 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 484 uint8_t* pp = (uint8_t*)(p + 1); 485 486 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_DISCONNECT; 487 p->offset = 0; 488 489 UINT16_TO_STREAM(pp, HCI_DISCONNECT); 490 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_DISCONNECT); 491 UINT16_TO_STREAM(pp, handle); 492 UINT8_TO_STREAM(pp, reason); 493 494 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 495 } 496 497 void btsnd_hcic_add_SCO_conn(uint16_t handle, uint16_t packet_types) { 498 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 499 uint8_t* pp = (uint8_t*)(p + 1); 500 501 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_ADD_SCO_CONN; 502 p->offset = 0; 503 504 UINT16_TO_STREAM(pp, HCI_ADD_SCO_CONNECTION); 505 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_ADD_SCO_CONN); 506 507 UINT16_TO_STREAM(pp, handle); 508 UINT16_TO_STREAM(pp, packet_types); 509 510 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 511 } 512 513 void btsnd_hcic_create_conn_cancel(const RawAddress& dest) { 514 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 515 uint8_t* pp = (uint8_t*)(p + 1); 516 517 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CREATE_CONN_CANCEL; 518 p->offset = 0; 519 520 UINT16_TO_STREAM(pp, HCI_CREATE_CONNECTION_CANCEL); 521 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CREATE_CONN_CANCEL); 522 523 BDADDR_TO_STREAM(pp, dest); 524 525 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 526 } 527 528 void btsnd_hcic_accept_conn(const RawAddress& dest, uint8_t role) { 529 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 530 uint8_t* pp = (uint8_t*)(p + 1); 531 532 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_ACCEPT_CONN; 533 p->offset = 0; 534 535 UINT16_TO_STREAM(pp, HCI_ACCEPT_CONNECTION_REQUEST); 536 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_ACCEPT_CONN); 537 BDADDR_TO_STREAM(pp, dest); 538 UINT8_TO_STREAM(pp, role); 539 540 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 541 } 542 543 void btsnd_hcic_reject_conn(const RawAddress& dest, uint8_t reason) { 544 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 545 uint8_t* pp = (uint8_t*)(p + 1); 546 547 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_REJECT_CONN; 548 p->offset = 0; 549 550 UINT16_TO_STREAM(pp, HCI_REJECT_CONNECTION_REQUEST); 551 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_REJECT_CONN); 552 553 BDADDR_TO_STREAM(pp, dest); 554 UINT8_TO_STREAM(pp, reason); 555 556 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 557 } 558 559 void btsnd_hcic_link_key_req_reply(const RawAddress& bd_addr, 560 const LinkKey& link_key) { 561 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 562 uint8_t* pp = (uint8_t*)(p + 1); 563 564 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_LINK_KEY_REQ_REPLY; 565 p->offset = 0; 566 567 UINT16_TO_STREAM(pp, HCI_LINK_KEY_REQUEST_REPLY); 568 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_LINK_KEY_REQ_REPLY); 569 570 BDADDR_TO_STREAM(pp, bd_addr); 571 ARRAY16_TO_STREAM(pp, link_key.data()); 572 573 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 574 } 575 576 void btsnd_hcic_link_key_neg_reply(const RawAddress& bd_addr) { 577 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 578 uint8_t* pp = (uint8_t*)(p + 1); 579 580 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_LINK_KEY_NEG_REPLY; 581 p->offset = 0; 582 583 UINT16_TO_STREAM(pp, HCI_LINK_KEY_REQUEST_NEG_REPLY); 584 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_LINK_KEY_NEG_REPLY); 585 586 BDADDR_TO_STREAM(pp, bd_addr); 587 588 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 589 } 590 591 void btsnd_hcic_pin_code_req_reply(const RawAddress& bd_addr, 592 uint8_t pin_code_len, PIN_CODE pin_code) { 593 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 594 uint8_t* pp = (uint8_t*)(p + 1); 595 int i; 596 597 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_PIN_CODE_REQ_REPLY; 598 p->offset = 0; 599 600 UINT16_TO_STREAM(pp, HCI_PIN_CODE_REQUEST_REPLY); 601 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_PIN_CODE_REQ_REPLY); 602 603 BDADDR_TO_STREAM(pp, bd_addr); 604 UINT8_TO_STREAM(pp, pin_code_len); 605 606 for (i = 0; i < pin_code_len; i++) *pp++ = *pin_code++; 607 608 for (; i < PIN_CODE_LEN; i++) *pp++ = 0; 609 610 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 611 } 612 613 void btsnd_hcic_pin_code_neg_reply(const RawAddress& bd_addr) { 614 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 615 uint8_t* pp = (uint8_t*)(p + 1); 616 617 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_PIN_CODE_NEG_REPLY; 618 p->offset = 0; 619 620 UINT16_TO_STREAM(pp, HCI_PIN_CODE_REQUEST_NEG_REPLY); 621 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_PIN_CODE_NEG_REPLY); 622 623 BDADDR_TO_STREAM(pp, bd_addr); 624 625 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 626 } 627 628 void btsnd_hcic_change_conn_type(uint16_t handle, uint16_t packet_types) { 629 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 630 uint8_t* pp = (uint8_t*)(p + 1); 631 632 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CHANGE_CONN_TYPE; 633 p->offset = 0; 634 635 UINT16_TO_STREAM(pp, HCI_CHANGE_CONN_PACKET_TYPE); 636 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CHANGE_CONN_TYPE); 637 638 UINT16_TO_STREAM(pp, handle); 639 UINT16_TO_STREAM(pp, packet_types); 640 641 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 642 } 643 644 void btsnd_hcic_auth_request(uint16_t handle) { 645 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 646 uint8_t* pp = (uint8_t*)(p + 1); 647 648 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CMD_HANDLE; 649 p->offset = 0; 650 651 UINT16_TO_STREAM(pp, HCI_AUTHENTICATION_REQUESTED); 652 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CMD_HANDLE); 653 654 UINT16_TO_STREAM(pp, handle); 655 656 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 657 } 658 659 void btsnd_hcic_set_conn_encrypt(uint16_t handle, bool enable) { 660 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 661 uint8_t* pp = (uint8_t*)(p + 1); 662 663 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_SET_CONN_ENCRYPT; 664 p->offset = 0; 665 666 UINT16_TO_STREAM(pp, HCI_SET_CONN_ENCRYPTION); 667 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_SET_CONN_ENCRYPT); 668 669 UINT16_TO_STREAM(pp, handle); 670 UINT8_TO_STREAM(pp, enable); 671 672 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 673 } 674 675 void btsnd_hcic_rmt_ext_features(uint16_t handle, uint8_t page_num) { 676 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 677 uint8_t* pp = (uint8_t*)(p + 1); 678 679 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_RMT_EXT_FEATURES; 680 p->offset = 0; 681 682 UINT16_TO_STREAM(pp, HCI_READ_RMT_EXT_FEATURES); 683 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_RMT_EXT_FEATURES); 684 685 UINT16_TO_STREAM(pp, handle); 686 UINT8_TO_STREAM(pp, page_num); 687 688 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 689 } 690 691 void btsnd_hcic_rmt_ver_req(uint16_t handle) { 692 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 693 uint8_t* pp = (uint8_t*)(p + 1); 694 695 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CMD_HANDLE; 696 p->offset = 0; 697 698 UINT16_TO_STREAM(pp, HCI_READ_RMT_VERSION_INFO); 699 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CMD_HANDLE); 700 701 UINT16_TO_STREAM(pp, handle); 702 703 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 704 } 705 706 void btsnd_hcic_read_rmt_clk_offset(uint16_t handle) { 707 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 708 uint8_t* pp = (uint8_t*)(p + 1); 709 710 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CMD_HANDLE; 711 p->offset = 0; 712 713 UINT16_TO_STREAM(pp, HCI_READ_RMT_CLOCK_OFFSET); 714 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CMD_HANDLE); 715 716 UINT16_TO_STREAM(pp, handle); 717 718 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 719 } 720 721 void btsnd_hcic_setup_esco_conn(uint16_t handle, uint32_t transmit_bandwidth, 722 uint32_t receive_bandwidth, 723 uint16_t max_latency, uint16_t voice, 724 uint8_t retrans_effort, uint16_t packet_types) { 725 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 726 uint8_t* pp = (uint8_t*)(p + 1); 727 728 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_SETUP_ESCO; 729 p->offset = 0; 730 731 UINT16_TO_STREAM(pp, HCI_SETUP_ESCO_CONNECTION); 732 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_SETUP_ESCO); 733 734 UINT16_TO_STREAM(pp, handle); 735 UINT32_TO_STREAM(pp, transmit_bandwidth); 736 UINT32_TO_STREAM(pp, receive_bandwidth); 737 UINT16_TO_STREAM(pp, max_latency); 738 UINT16_TO_STREAM(pp, voice); 739 UINT8_TO_STREAM(pp, retrans_effort); 740 UINT16_TO_STREAM(pp, packet_types); 741 742 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 743 } 744 745 void btsnd_hcic_accept_esco_conn(const RawAddress& bd_addr, 746 uint32_t transmit_bandwidth, 747 uint32_t receive_bandwidth, 748 uint16_t max_latency, uint16_t content_fmt, 749 uint8_t retrans_effort, 750 uint16_t packet_types) { 751 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 752 uint8_t* pp = (uint8_t*)(p + 1); 753 754 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_ACCEPT_ESCO; 755 p->offset = 0; 756 757 UINT16_TO_STREAM(pp, HCI_ACCEPT_ESCO_CONNECTION); 758 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_ACCEPT_ESCO); 759 760 BDADDR_TO_STREAM(pp, bd_addr); 761 UINT32_TO_STREAM(pp, transmit_bandwidth); 762 UINT32_TO_STREAM(pp, receive_bandwidth); 763 UINT16_TO_STREAM(pp, max_latency); 764 UINT16_TO_STREAM(pp, content_fmt); 765 UINT8_TO_STREAM(pp, retrans_effort); 766 UINT16_TO_STREAM(pp, packet_types); 767 768 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 769 } 770 771 void btsnd_hcic_reject_esco_conn(const RawAddress& bd_addr, uint8_t reason) { 772 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 773 uint8_t* pp = (uint8_t*)(p + 1); 774 775 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_REJECT_ESCO; 776 p->offset = 0; 777 778 UINT16_TO_STREAM(pp, HCI_REJECT_ESCO_CONNECTION); 779 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_REJECT_ESCO); 780 781 BDADDR_TO_STREAM(pp, bd_addr); 782 UINT8_TO_STREAM(pp, reason); 783 784 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 785 } 786 787 void btsnd_hcic_hold_mode(uint16_t handle, uint16_t max_hold_period, 788 uint16_t min_hold_period) { 789 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 790 uint8_t* pp = (uint8_t*)(p + 1); 791 792 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_HOLD_MODE; 793 p->offset = 0; 794 795 UINT16_TO_STREAM(pp, HCI_HOLD_MODE); 796 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_HOLD_MODE); 797 798 UINT16_TO_STREAM(pp, handle); 799 UINT16_TO_STREAM(pp, max_hold_period); 800 UINT16_TO_STREAM(pp, min_hold_period); 801 802 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 803 } 804 805 void btsnd_hcic_sniff_mode(uint16_t handle, uint16_t max_sniff_period, 806 uint16_t min_sniff_period, uint16_t sniff_attempt, 807 uint16_t sniff_timeout) { 808 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 809 uint8_t* pp = (uint8_t*)(p + 1); 810 811 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_SNIFF_MODE; 812 p->offset = 0; 813 814 UINT16_TO_STREAM(pp, HCI_SNIFF_MODE); 815 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_SNIFF_MODE); 816 817 UINT16_TO_STREAM(pp, handle); 818 UINT16_TO_STREAM(pp, max_sniff_period); 819 UINT16_TO_STREAM(pp, min_sniff_period); 820 UINT16_TO_STREAM(pp, sniff_attempt); 821 UINT16_TO_STREAM(pp, sniff_timeout); 822 823 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 824 } 825 826 void btsnd_hcic_exit_sniff_mode(uint16_t handle) { 827 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 828 uint8_t* pp = (uint8_t*)(p + 1); 829 830 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CMD_HANDLE; 831 p->offset = 0; 832 833 UINT16_TO_STREAM(pp, HCI_EXIT_SNIFF_MODE); 834 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CMD_HANDLE); 835 836 UINT16_TO_STREAM(pp, handle); 837 838 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 839 } 840 841 void btsnd_hcic_park_mode(uint16_t handle, uint16_t beacon_max_interval, 842 uint16_t beacon_min_interval) { 843 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 844 uint8_t* pp = (uint8_t*)(p + 1); 845 846 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_PARK_MODE; 847 p->offset = 0; 848 849 UINT16_TO_STREAM(pp, HCI_PARK_MODE); 850 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_PARK_MODE); 851 852 UINT16_TO_STREAM(pp, handle); 853 UINT16_TO_STREAM(pp, beacon_max_interval); 854 UINT16_TO_STREAM(pp, beacon_min_interval); 855 856 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 857 } 858 859 void btsnd_hcic_exit_park_mode(uint16_t handle) { 860 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 861 uint8_t* pp = (uint8_t*)(p + 1); 862 863 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CMD_HANDLE; 864 p->offset = 0; 865 866 UINT16_TO_STREAM(pp, HCI_EXIT_PARK_MODE); 867 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CMD_HANDLE); 868 869 UINT16_TO_STREAM(pp, handle); 870 871 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 872 } 873 874 static void btsnd_hcic_switch_role(const RawAddress& bd_addr, uint8_t role) { 875 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 876 uint8_t* pp = (uint8_t*)(p + 1); 877 878 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_SWITCH_ROLE; 879 p->offset = 0; 880 881 UINT16_TO_STREAM(pp, HCI_SWITCH_ROLE); 882 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_SWITCH_ROLE); 883 884 BDADDR_TO_STREAM(pp, bd_addr); 885 UINT8_TO_STREAM(pp, role); 886 887 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 888 } 889 890 void btsnd_hcic_write_policy_set(uint16_t handle, uint16_t settings) { 891 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 892 uint8_t* pp = (uint8_t*)(p + 1); 893 894 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_POLICY_SET; 895 p->offset = 0; 896 UINT16_TO_STREAM(pp, HCI_WRITE_POLICY_SETTINGS); 897 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_POLICY_SET); 898 899 UINT16_TO_STREAM(pp, handle); 900 UINT16_TO_STREAM(pp, settings); 901 902 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 903 } 904 905 void btsnd_hcic_write_def_policy_set(uint16_t settings) { 906 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 907 uint8_t* pp = (uint8_t*)(p + 1); 908 909 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_DEF_POLICY_SET; 910 p->offset = 0; 911 UINT16_TO_STREAM(pp, HCI_WRITE_DEF_POLICY_SETTINGS); 912 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_DEF_POLICY_SET); 913 914 UINT16_TO_STREAM(pp, settings); 915 916 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 917 } 918 919 void btsnd_hcic_set_event_filter(uint8_t filt_type, uint8_t filt_cond_type, 920 uint8_t* filt_cond, uint8_t filt_cond_len) { 921 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 922 uint8_t* pp = (uint8_t*)(p + 1); 923 924 p->offset = 0; 925 926 UINT16_TO_STREAM(pp, HCI_SET_EVENT_FILTER); 927 928 if (filt_type) { 929 p->len = (uint16_t)(HCIC_PREAMBLE_SIZE + 2 + filt_cond_len); 930 UINT8_TO_STREAM(pp, (uint8_t)(2 + filt_cond_len)); 931 932 UINT8_TO_STREAM(pp, filt_type); 933 UINT8_TO_STREAM(pp, filt_cond_type); 934 935 if (filt_cond_type == HCI_FILTER_COND_DEVICE_CLASS) { 936 DEVCLASS_TO_STREAM(pp, filt_cond); 937 filt_cond += kDevClassLength; 938 DEVCLASS_TO_STREAM(pp, filt_cond); 939 filt_cond += kDevClassLength; 940 941 filt_cond_len -= (2 * kDevClassLength); 942 } else if (filt_cond_type == HCI_FILTER_COND_BD_ADDR) { 943 BDADDR_TO_STREAM(pp, *((RawAddress*)filt_cond)); 944 filt_cond += BD_ADDR_LEN; 945 946 filt_cond_len -= BD_ADDR_LEN; 947 } 948 949 if (filt_cond_len) ARRAY_TO_STREAM(pp, filt_cond, filt_cond_len); 950 } else { 951 p->len = (uint16_t)(HCIC_PREAMBLE_SIZE + 1); 952 UINT8_TO_STREAM(pp, 1); 953 954 UINT8_TO_STREAM(pp, filt_type); 955 } 956 957 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 958 } 959 960 void btsnd_hcic_write_pin_type(uint8_t type) { 961 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 962 uint8_t* pp = (uint8_t*)(p + 1); 963 964 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM1; 965 p->offset = 0; 966 967 UINT16_TO_STREAM(pp, HCI_WRITE_PIN_TYPE); 968 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM1); 969 970 UINT8_TO_STREAM(pp, type); 971 972 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 973 } 974 975 void btsnd_hcic_delete_stored_key(const RawAddress& bd_addr, 976 bool delete_all_flag) { 977 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 978 uint8_t* pp = (uint8_t*)(p + 1); 979 980 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_DELETE_STORED_KEY; 981 p->offset = 0; 982 983 UINT16_TO_STREAM(pp, HCI_DELETE_STORED_LINK_KEY); 984 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_DELETE_STORED_KEY); 985 986 BDADDR_TO_STREAM(pp, bd_addr); 987 UINT8_TO_STREAM(pp, delete_all_flag); 988 989 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 990 } 991 992 void btsnd_hcic_change_name(BD_NAME name) { 993 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 994 uint8_t* pp = (uint8_t*)(p + 1); 995 uint16_t len = strlen((char*)name) + 1; 996 997 memset(pp, 0, HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CHANGE_NAME); 998 999 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CHANGE_NAME; 1000 p->offset = 0; 1001 1002 UINT16_TO_STREAM(pp, HCI_CHANGE_LOCAL_NAME); 1003 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CHANGE_NAME); 1004 1005 if (len > HCIC_PARAM_SIZE_CHANGE_NAME) len = HCIC_PARAM_SIZE_CHANGE_NAME; 1006 1007 ARRAY_TO_STREAM(pp, name, len); 1008 1009 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1010 } 1011 1012 void btsnd_hcic_read_name(void) { 1013 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1014 uint8_t* pp = (uint8_t*)(p + 1); 1015 1016 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_READ_CMD; 1017 p->offset = 0; 1018 1019 UINT16_TO_STREAM(pp, HCI_READ_LOCAL_NAME); 1020 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_READ_CMD); 1021 1022 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1023 } 1024 1025 void btsnd_hcic_write_page_tout(uint16_t timeout) { 1026 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1027 uint8_t* pp = (uint8_t*)(p + 1); 1028 1029 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM2; 1030 p->offset = 0; 1031 1032 UINT16_TO_STREAM(pp, HCI_WRITE_PAGE_TOUT); 1033 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM2); 1034 1035 UINT16_TO_STREAM(pp, timeout); 1036 1037 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1038 } 1039 1040 void btsnd_hcic_write_scan_enable(uint8_t flag) { 1041 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1042 uint8_t* pp = (uint8_t*)(p + 1); 1043 1044 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM1; 1045 p->offset = 0; 1046 1047 UINT16_TO_STREAM(pp, HCI_WRITE_SCAN_ENABLE); 1048 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM1); 1049 1050 UINT8_TO_STREAM(pp, flag); 1051 1052 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1053 } 1054 1055 void btsnd_hcic_write_pagescan_cfg(uint16_t interval, uint16_t window) { 1056 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1057 uint8_t* pp = (uint8_t*)(p + 1); 1058 1059 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PAGESCAN_CFG; 1060 p->offset = 0; 1061 1062 UINT16_TO_STREAM(pp, HCI_WRITE_PAGESCAN_CFG); 1063 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PAGESCAN_CFG); 1064 1065 UINT16_TO_STREAM(pp, interval); 1066 UINT16_TO_STREAM(pp, window); 1067 1068 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1069 } 1070 1071 void btsnd_hcic_write_inqscan_cfg(uint16_t interval, uint16_t window) { 1072 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1073 uint8_t* pp = (uint8_t*)(p + 1); 1074 1075 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_INQSCAN_CFG; 1076 p->offset = 0; 1077 1078 UINT16_TO_STREAM(pp, HCI_WRITE_INQUIRYSCAN_CFG); 1079 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_INQSCAN_CFG); 1080 1081 UINT16_TO_STREAM(pp, interval); 1082 UINT16_TO_STREAM(pp, window); 1083 1084 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1085 } 1086 1087 void btsnd_hcic_write_auth_enable(uint8_t flag) { 1088 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1089 uint8_t* pp = (uint8_t*)(p + 1); 1090 1091 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM1; 1092 p->offset = 0; 1093 1094 UINT16_TO_STREAM(pp, HCI_WRITE_AUTHENTICATION_ENABLE); 1095 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM1); 1096 1097 UINT8_TO_STREAM(pp, flag); 1098 1099 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1100 } 1101 1102 void btsnd_hcic_write_dev_class(DEV_CLASS dev_class) { 1103 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1104 uint8_t* pp = (uint8_t*)(p + 1); 1105 1106 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM3; 1107 p->offset = 0; 1108 1109 UINT16_TO_STREAM(pp, HCI_WRITE_CLASS_OF_DEVICE); 1110 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM3); 1111 1112 DEVCLASS_TO_STREAM(pp, dev_class); 1113 1114 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1115 } 1116 1117 void btsnd_hcic_write_voice_settings(uint16_t flags) { 1118 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1119 uint8_t* pp = (uint8_t*)(p + 1); 1120 1121 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM2; 1122 p->offset = 0; 1123 1124 UINT16_TO_STREAM(pp, HCI_WRITE_VOICE_SETTINGS); 1125 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM2); 1126 1127 UINT16_TO_STREAM(pp, flags); 1128 1129 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1130 } 1131 1132 void btsnd_hcic_write_auto_flush_tout(uint16_t handle, uint16_t tout) { 1133 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1134 uint8_t* pp = (uint8_t*)(p + 1); 1135 1136 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_AUTOMATIC_FLUSH_TIMEOUT; 1137 p->offset = 0; 1138 1139 UINT16_TO_STREAM(pp, HCI_WRITE_AUTOMATIC_FLUSH_TIMEOUT); 1140 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_AUTOMATIC_FLUSH_TIMEOUT); 1141 1142 UINT16_TO_STREAM(pp, handle); 1143 UINT16_TO_STREAM(pp, tout); 1144 1145 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1146 } 1147 1148 void btsnd_hcic_read_tx_power(uint16_t handle, uint8_t type) { 1149 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1150 uint8_t* pp = (uint8_t*)(p + 1); 1151 1152 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_READ_TX_POWER; 1153 p->offset = 0; 1154 1155 UINT16_TO_STREAM(pp, HCI_READ_TRANSMIT_POWER_LEVEL); 1156 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_READ_TX_POWER); 1157 1158 UINT16_TO_STREAM(pp, handle); 1159 UINT8_TO_STREAM(pp, type); 1160 1161 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1162 } 1163 1164 void btsnd_hcic_write_link_super_tout(uint16_t handle, uint16_t timeout) { 1165 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1166 uint8_t* pp = (uint8_t*)(p + 1); 1167 1168 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_LINK_SUPER_TOUT; 1169 p->offset = 0; 1170 1171 UINT16_TO_STREAM(pp, HCI_WRITE_LINK_SUPER_TOUT); 1172 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_LINK_SUPER_TOUT); 1173 1174 UINT16_TO_STREAM(pp, handle); 1175 UINT16_TO_STREAM(pp, timeout); 1176 1177 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1178 } 1179 1180 void btsnd_hcic_write_cur_iac_lap(uint8_t num_cur_iac, LAP* const iac_lap) { 1181 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1182 uint8_t* pp = (uint8_t*)(p + 1); 1183 1184 p->len = HCIC_PREAMBLE_SIZE + 1 + (LAP_LEN * num_cur_iac); 1185 p->offset = 0; 1186 1187 UINT16_TO_STREAM(pp, HCI_WRITE_CURRENT_IAC_LAP); 1188 UINT8_TO_STREAM(pp, p->len - HCIC_PREAMBLE_SIZE); 1189 1190 UINT8_TO_STREAM(pp, num_cur_iac); 1191 1192 for (int i = 0; i < num_cur_iac; i++) LAP_TO_STREAM(pp, iac_lap[i]); 1193 1194 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1195 } 1196 1197 /****************************************** 1198 * Lisbon Features 1199 ******************************************/ 1200 void btsnd_hcic_sniff_sub_rate(uint16_t handle, uint16_t max_lat, 1201 uint16_t min_remote_lat, 1202 uint16_t min_local_lat) { 1203 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1204 uint8_t* pp = (uint8_t*)(p + 1); 1205 1206 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_SNIFF_SUB_RATE; 1207 p->offset = 0; 1208 1209 UINT16_TO_STREAM(pp, HCI_SNIFF_SUB_RATE); 1210 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_SNIFF_SUB_RATE); 1211 1212 UINT16_TO_STREAM(pp, handle); 1213 UINT16_TO_STREAM(pp, max_lat); 1214 UINT16_TO_STREAM(pp, min_remote_lat); 1215 UINT16_TO_STREAM(pp, min_local_lat); 1216 1217 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1218 } 1219 1220 /**** Extended Inquiry Response Commands ****/ 1221 void btsnd_hcic_write_ext_inquiry_response(void* buffer, uint8_t fec_req) { 1222 BT_HDR* p = (BT_HDR*)buffer; 1223 uint8_t* pp = (uint8_t*)(p + 1); 1224 1225 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_EXT_INQ_RESP; 1226 p->offset = 0; 1227 1228 UINT16_TO_STREAM(pp, HCI_WRITE_EXT_INQ_RESPONSE); 1229 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_EXT_INQ_RESP); 1230 1231 UINT8_TO_STREAM(pp, fec_req); 1232 1233 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1234 } 1235 1236 void btsnd_hcic_io_cap_req_reply(const RawAddress& bd_addr, uint8_t capability, 1237 uint8_t oob_present, uint8_t auth_req) { 1238 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1239 uint8_t* pp = (uint8_t*)(p + 1); 1240 1241 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_IO_CAP_RESP; 1242 p->offset = 0; 1243 1244 UINT16_TO_STREAM(pp, HCI_IO_CAPABILITY_REQUEST_REPLY); 1245 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_IO_CAP_RESP); 1246 1247 BDADDR_TO_STREAM(pp, bd_addr); 1248 UINT8_TO_STREAM(pp, capability); 1249 UINT8_TO_STREAM(pp, oob_present); 1250 UINT8_TO_STREAM(pp, auth_req); 1251 1252 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1253 } 1254 1255 void btsnd_hcic_enhanced_set_up_synchronous_connection( 1256 uint16_t conn_handle, enh_esco_params_t* p_params) { 1257 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1258 uint8_t* pp = (uint8_t*)(p + 1); 1259 1260 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_ENH_SET_ESCO_CONN; 1261 p->offset = 0; 1262 1263 UINT16_TO_STREAM(pp, HCI_ENH_SETUP_ESCO_CONNECTION); 1264 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_ENH_SET_ESCO_CONN); 1265 1266 UINT16_TO_STREAM(pp, conn_handle); 1267 UINT32_TO_STREAM(pp, p_params->transmit_bandwidth); 1268 UINT32_TO_STREAM(pp, p_params->receive_bandwidth); 1269 UINT8_TO_STREAM(pp, p_params->transmit_coding_format.coding_format); 1270 UINT16_TO_STREAM(pp, p_params->transmit_coding_format.company_id); 1271 UINT16_TO_STREAM(pp, 1272 p_params->transmit_coding_format.vendor_specific_codec_id); 1273 UINT8_TO_STREAM(pp, p_params->receive_coding_format.coding_format); 1274 UINT16_TO_STREAM(pp, p_params->receive_coding_format.company_id); 1275 UINT16_TO_STREAM(pp, 1276 p_params->receive_coding_format.vendor_specific_codec_id); 1277 UINT16_TO_STREAM(pp, p_params->transmit_codec_frame_size); 1278 UINT16_TO_STREAM(pp, p_params->receive_codec_frame_size); 1279 UINT32_TO_STREAM(pp, p_params->input_bandwidth); 1280 UINT32_TO_STREAM(pp, p_params->output_bandwidth); 1281 UINT8_TO_STREAM(pp, p_params->input_coding_format.coding_format); 1282 UINT16_TO_STREAM(pp, p_params->input_coding_format.company_id); 1283 UINT16_TO_STREAM(pp, p_params->input_coding_format.vendor_specific_codec_id); 1284 UINT8_TO_STREAM(pp, p_params->output_coding_format.coding_format); 1285 UINT16_TO_STREAM(pp, p_params->output_coding_format.company_id); 1286 UINT16_TO_STREAM(pp, p_params->output_coding_format.vendor_specific_codec_id); 1287 UINT16_TO_STREAM(pp, p_params->input_coded_data_size); 1288 UINT16_TO_STREAM(pp, p_params->output_coded_data_size); 1289 UINT8_TO_STREAM(pp, p_params->input_pcm_data_format); 1290 UINT8_TO_STREAM(pp, p_params->output_pcm_data_format); 1291 UINT8_TO_STREAM(pp, p_params->input_pcm_payload_msb_position); 1292 UINT8_TO_STREAM(pp, p_params->output_pcm_payload_msb_position); 1293 UINT8_TO_STREAM(pp, p_params->input_data_path); 1294 UINT8_TO_STREAM(pp, p_params->output_data_path); 1295 UINT8_TO_STREAM(pp, p_params->input_transport_unit_size); 1296 UINT8_TO_STREAM(pp, p_params->output_transport_unit_size); 1297 UINT16_TO_STREAM(pp, p_params->max_latency_ms); 1298 UINT16_TO_STREAM(pp, p_params->packet_types); 1299 UINT8_TO_STREAM(pp, p_params->retransmission_effort); 1300 1301 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1302 } 1303 1304 void btsnd_hcic_enhanced_accept_synchronous_connection( 1305 const RawAddress& bd_addr, enh_esco_params_t* p_params) { 1306 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1307 uint8_t* pp = (uint8_t*)(p + 1); 1308 1309 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_ENH_ACC_ESCO_CONN; 1310 p->offset = 0; 1311 1312 UINT16_TO_STREAM(pp, HCI_ENH_ACCEPT_ESCO_CONNECTION); 1313 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_ENH_ACC_ESCO_CONN); 1314 1315 BDADDR_TO_STREAM(pp, bd_addr); 1316 UINT32_TO_STREAM(pp, p_params->transmit_bandwidth); 1317 UINT32_TO_STREAM(pp, p_params->receive_bandwidth); 1318 UINT8_TO_STREAM(pp, p_params->transmit_coding_format.coding_format); 1319 UINT16_TO_STREAM(pp, p_params->transmit_coding_format.company_id); 1320 UINT16_TO_STREAM(pp, 1321 p_params->transmit_coding_format.vendor_specific_codec_id); 1322 UINT8_TO_STREAM(pp, p_params->receive_coding_format.coding_format); 1323 UINT16_TO_STREAM(pp, p_params->receive_coding_format.company_id); 1324 UINT16_TO_STREAM(pp, 1325 p_params->receive_coding_format.vendor_specific_codec_id); 1326 UINT16_TO_STREAM(pp, p_params->transmit_codec_frame_size); 1327 UINT16_TO_STREAM(pp, p_params->receive_codec_frame_size); 1328 UINT32_TO_STREAM(pp, p_params->input_bandwidth); 1329 UINT32_TO_STREAM(pp, p_params->output_bandwidth); 1330 UINT8_TO_STREAM(pp, p_params->input_coding_format.coding_format); 1331 UINT16_TO_STREAM(pp, p_params->input_coding_format.company_id); 1332 UINT16_TO_STREAM(pp, p_params->input_coding_format.vendor_specific_codec_id); 1333 UINT8_TO_STREAM(pp, p_params->output_coding_format.coding_format); 1334 UINT16_TO_STREAM(pp, p_params->output_coding_format.company_id); 1335 UINT16_TO_STREAM(pp, p_params->output_coding_format.vendor_specific_codec_id); 1336 UINT16_TO_STREAM(pp, p_params->input_coded_data_size); 1337 UINT16_TO_STREAM(pp, p_params->output_coded_data_size); 1338 UINT8_TO_STREAM(pp, p_params->input_pcm_data_format); 1339 UINT8_TO_STREAM(pp, p_params->output_pcm_data_format); 1340 UINT8_TO_STREAM(pp, p_params->input_pcm_payload_msb_position); 1341 UINT8_TO_STREAM(pp, p_params->output_pcm_payload_msb_position); 1342 UINT8_TO_STREAM(pp, p_params->input_data_path); 1343 UINT8_TO_STREAM(pp, p_params->output_data_path); 1344 UINT8_TO_STREAM(pp, p_params->input_transport_unit_size); 1345 UINT8_TO_STREAM(pp, p_params->output_transport_unit_size); 1346 UINT16_TO_STREAM(pp, p_params->max_latency_ms); 1347 UINT16_TO_STREAM(pp, p_params->packet_types); 1348 UINT8_TO_STREAM(pp, p_params->retransmission_effort); 1349 1350 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1351 } 1352 1353 void btsnd_hcic_io_cap_req_neg_reply(const RawAddress& bd_addr, 1354 uint8_t err_code) { 1355 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1356 uint8_t* pp = (uint8_t*)(p + 1); 1357 1358 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_IO_CAP_NEG_REPLY; 1359 p->offset = 0; 1360 1361 UINT16_TO_STREAM(pp, HCI_IO_CAP_REQ_NEG_REPLY); 1362 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_IO_CAP_NEG_REPLY); 1363 1364 BDADDR_TO_STREAM(pp, bd_addr); 1365 UINT8_TO_STREAM(pp, err_code); 1366 1367 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1368 } 1369 1370 void btsnd_hcic_read_local_oob_data(void) { 1371 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1372 uint8_t* pp = (uint8_t*)(p + 1); 1373 1374 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_R_LOCAL_OOB; 1375 p->offset = 0; 1376 1377 UINT16_TO_STREAM(pp, HCI_READ_LOCAL_OOB_DATA); 1378 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_R_LOCAL_OOB); 1379 1380 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1381 } 1382 1383 void btsnd_hcic_read_local_oob_extended_data(void) { 1384 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1385 uint8_t* pp = (uint8_t*)(p + 1); 1386 1387 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_R_LOCAL_OOB_EXTENDED; 1388 p->offset = 0; 1389 1390 UINT16_TO_STREAM(pp, HCI_READ_LOCAL_OOB_EXTENDED_DATA); 1391 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_R_LOCAL_OOB_EXTENDED); 1392 1393 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1394 } 1395 1396 void btsnd_hcic_user_conf_reply(const RawAddress& bd_addr, bool is_yes) { 1397 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1398 uint8_t* pp = (uint8_t*)(p + 1); 1399 1400 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_UCONF_REPLY; 1401 p->offset = 0; 1402 1403 if (!is_yes) { 1404 /* Negative reply */ 1405 UINT16_TO_STREAM(pp, HCI_USER_CONF_VALUE_NEG_REPLY); 1406 } else { 1407 /* Confirmation */ 1408 UINT16_TO_STREAM(pp, HCI_USER_CONF_REQUEST_REPLY); 1409 } 1410 1411 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_UCONF_REPLY); 1412 1413 BDADDR_TO_STREAM(pp, bd_addr); 1414 1415 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1416 } 1417 1418 void btsnd_hcic_user_passkey_reply(const RawAddress& bd_addr, uint32_t value) { 1419 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1420 uint8_t* pp = (uint8_t*)(p + 1); 1421 1422 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_U_PKEY_REPLY; 1423 p->offset = 0; 1424 1425 UINT16_TO_STREAM(pp, HCI_USER_PASSKEY_REQ_REPLY); 1426 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_U_PKEY_REPLY); 1427 1428 BDADDR_TO_STREAM(pp, bd_addr); 1429 UINT32_TO_STREAM(pp, value); 1430 1431 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1432 } 1433 1434 void btsnd_hcic_user_passkey_neg_reply(const RawAddress& bd_addr) { 1435 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1436 uint8_t* pp = (uint8_t*)(p + 1); 1437 1438 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_U_PKEY_NEG_REPLY; 1439 p->offset = 0; 1440 1441 UINT16_TO_STREAM(pp, HCI_USER_PASSKEY_REQ_NEG_REPLY); 1442 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_U_PKEY_NEG_REPLY); 1443 1444 BDADDR_TO_STREAM(pp, bd_addr); 1445 1446 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1447 } 1448 1449 void btsnd_hcic_rem_oob_reply(const RawAddress& bd_addr, const Octet16& c, 1450 const Octet16& r) { 1451 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1452 uint8_t* pp = (uint8_t*)(p + 1); 1453 1454 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_REM_OOB_REPLY; 1455 p->offset = 0; 1456 1457 UINT16_TO_STREAM(pp, HCI_REM_OOB_DATA_REQ_REPLY); 1458 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_REM_OOB_REPLY); 1459 1460 BDADDR_TO_STREAM(pp, bd_addr); 1461 ARRAY16_TO_STREAM(pp, c.data()); 1462 ARRAY16_TO_STREAM(pp, r.data()); 1463 1464 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1465 } 1466 1467 void btsnd_hcic_rem_oob_neg_reply(const RawAddress& bd_addr) { 1468 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1469 uint8_t* pp = (uint8_t*)(p + 1); 1470 1471 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_REM_OOB_NEG_REPLY; 1472 p->offset = 0; 1473 1474 UINT16_TO_STREAM(pp, HCI_REM_OOB_DATA_REQ_NEG_REPLY); 1475 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_REM_OOB_NEG_REPLY); 1476 1477 BDADDR_TO_STREAM(pp, bd_addr); 1478 1479 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1480 } 1481 1482 /**** end of Simple Pairing Commands ****/ 1483 1484 /************************* 1485 * End of Lisbon Commands 1486 *************************/ 1487 1488 void btsnd_hcic_read_rssi(uint16_t handle) { 1489 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1490 uint8_t* pp = (uint8_t*)(p + 1); 1491 1492 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CMD_HANDLE; 1493 p->offset = 0; 1494 1495 UINT16_TO_STREAM(pp, HCI_READ_RSSI); 1496 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CMD_HANDLE); 1497 1498 UINT16_TO_STREAM(pp, handle); 1499 1500 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1501 } 1502 1503 static void read_encryption_key_size_complete( 1504 ReadEncKeySizeCb cb, uint8_t* return_parameters, 1505 uint16_t /* return_parameters_length */) { 1506 uint8_t status; 1507 uint16_t handle; 1508 uint8_t key_size; 1509 STREAM_TO_UINT8(status, return_parameters); 1510 STREAM_TO_UINT16(handle, return_parameters); 1511 STREAM_TO_UINT8(key_size, return_parameters); 1512 1513 std::move(cb).Run(status, handle, key_size); 1514 } 1515 1516 void btsnd_hcic_read_encryption_key_size(uint16_t handle, ReadEncKeySizeCb cb) { 1517 constexpr uint8_t len = 2; 1518 uint8_t param[len]; 1519 memset(param, 0, len); 1520 1521 uint8_t* p = param; 1522 UINT16_TO_STREAM(p, handle); 1523 1524 btu_hcif_send_cmd_with_cb(FROM_HERE, HCI_READ_ENCR_KEY_SIZE, param, len, 1525 base::Bind(&read_encryption_key_size_complete, base::Passed(&cb))); 1526 } 1527 1528 void btsnd_hcic_read_failed_contact_counter(uint16_t handle) { 1529 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1530 uint8_t* pp = (uint8_t*)(p + 1); 1531 1532 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CMD_HANDLE; 1533 p->offset = 0; 1534 1535 UINT16_TO_STREAM(pp, HCI_READ_FAILED_CONTACT_COUNTER); 1536 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CMD_HANDLE); 1537 1538 UINT16_TO_STREAM(pp, handle); 1539 1540 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1541 } 1542 1543 void btsnd_hcic_enable_test_mode(void) { 1544 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1545 uint8_t* pp = (uint8_t*)(p + 1); 1546 1547 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_READ_CMD; 1548 p->offset = 0; 1549 1550 UINT16_TO_STREAM(pp, HCI_ENABLE_DEV_UNDER_TEST_MODE); 1551 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_READ_CMD); 1552 1553 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1554 } 1555 1556 void btsnd_hcic_write_inqscan_type(uint8_t type) { 1557 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1558 uint8_t* pp = (uint8_t*)(p + 1); 1559 1560 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM1; 1561 p->offset = 0; 1562 1563 UINT16_TO_STREAM(pp, HCI_WRITE_INQSCAN_TYPE); 1564 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM1); 1565 1566 UINT8_TO_STREAM(pp, type); 1567 1568 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1569 } 1570 1571 void btsnd_hcic_write_inquiry_mode(uint8_t mode) { 1572 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1573 uint8_t* pp = (uint8_t*)(p + 1); 1574 1575 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM1; 1576 p->offset = 0; 1577 1578 UINT16_TO_STREAM(pp, HCI_WRITE_INQUIRY_MODE); 1579 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM1); 1580 1581 UINT8_TO_STREAM(pp, mode); 1582 1583 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1584 } 1585 1586 void btsnd_hcic_write_pagescan_type(uint8_t type) { 1587 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1588 uint8_t* pp = (uint8_t*)(p + 1); 1589 1590 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_WRITE_PARAM1; 1591 p->offset = 0; 1592 1593 UINT16_TO_STREAM(pp, HCI_WRITE_PAGESCAN_TYPE); 1594 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_WRITE_PARAM1); 1595 1596 UINT8_TO_STREAM(pp, type); 1597 1598 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1599 } 1600 1601 static void btsnd_hcic_vendor_spec_complete(tBTM_VSC_CMPL_CB* p_vsc_cplt_cback, 1602 uint16_t opcode, uint8_t* data, 1603 uint16_t len) { 1604 /* If there was a callback address for vcs complete, call it */ 1605 if (p_vsc_cplt_cback) { 1606 tBTM_VSC_CMPL vcs_cplt_params; 1607 vcs_cplt_params.opcode = opcode; 1608 vcs_cplt_params.param_len = len; 1609 vcs_cplt_params.p_param_buf = data; 1610 /* Call the VSC complete callback function */ 1611 (*p_vsc_cplt_cback)(&vcs_cplt_params); 1612 } 1613 } 1614 1615 void btsnd_hcic_vendor_spec_cmd(uint16_t opcode, uint8_t len, uint8_t* p_data, 1616 tBTM_VSC_CMPL_CB* p_cmd_cplt_cback) { 1617 uint16_t v_opcode = HCI_GRP_VENDOR_SPECIFIC | opcode; 1618 1619 btu_hcif_send_cmd_with_cb( 1620 FROM_HERE, v_opcode, p_data, len, 1621 base::BindOnce(&btsnd_hcic_vendor_spec_complete, 1622 base::Unretained(p_cmd_cplt_cback), v_opcode)); 1623 } 1624 1625 void btsnd_hcic_configure_data_path(hci_data_direction_t data_path_direction, 1626 uint8_t data_path_id, 1627 std::vector<uint8_t> vendor_config) { 1628 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1629 uint8_t* pp = (uint8_t*)(p + 1); 1630 uint8_t size = static_cast<uint8_t>(vendor_config.size()); 1631 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_CONFIGURE_DATA_PATH + size; 1632 p->offset = 0; 1633 1634 UINT16_TO_STREAM(pp, HCI_CONFIGURE_DATA_PATH); 1635 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_CONFIGURE_DATA_PATH + size); 1636 UINT8_TO_STREAM(pp, data_path_direction); 1637 UINT8_TO_STREAM(pp, data_path_id); 1638 UINT8_TO_STREAM(pp, vendor_config.size()); 1639 if (size != 0) { 1640 ARRAY_TO_STREAM(pp, vendor_config.data(), size); 1641 } 1642 1643 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1644 } 1645 1646 namespace bluetooth::legacy::hci { 1647 class InterfaceImpl : public Interface { 1648 void Disconnect(uint16_t handle, uint8_t reason) const override { 1649 btsnd_hcic_disconnect(handle, reason); 1650 } 1651 void ChangeConnectionPacketType(uint16_t handle, 1652 uint16_t packet_types) const override { 1653 btsnd_hcic_change_conn_type(handle, packet_types); 1654 } 1655 void StartRoleSwitch(const RawAddress& bd_addr, uint8_t role) const override { 1656 btsnd_hcic_switch_role(bd_addr, role); 1657 } 1658 void ConfigureDataPath(hci_data_direction_t data_path_direction, 1659 uint8_t data_path_id, 1660 std::vector<uint8_t> vendor_config) const override { 1661 btsnd_hcic_configure_data_path(data_path_direction, data_path_id, 1662 vendor_config); 1663 } 1664 void SendVendorSpecificCmd(unsigned short opcode, 1665 unsigned char len, unsigned char* param, void (*cb)(tBTM_VSC_CMPL*)) const override { 1666 btsnd_hcic_vendor_spec_cmd(opcode, len, param, cb); 1667 } 1668 }; 1669 1670 namespace { 1671 const InterfaceImpl interface_; 1672 } 1673 1674 const Interface& GetInterface() { return interface_; } 1675 } // namespace bluetooth::legacy::hci 1676 1677 void btsnd_hcic_flow_spec(uint16_t handle, uint8_t unused, uint8_t direction, 1678 uint8_t service_type, uint32_t token_rate, 1679 uint32_t token_size, uint32_t peak, uint32_t latency){ 1680 1681 BT_HDR* p = (BT_HDR*)osi_malloc(HCI_CMD_BUF_SIZE); 1682 uint8_t* pp = (uint8_t*)(p + 1); 1683 1684 p->len = HCIC_PREAMBLE_SIZE + HCIC_PARAM_SIZE_FLOW_SPECIFICATION; 1685 p->offset = 0; 1686 1687 UINT16_TO_STREAM(pp, HCI_FLOW_SPECIFICATION); 1688 UINT8_TO_STREAM(pp, HCIC_PARAM_SIZE_FLOW_SPECIFICATION); 1689 1690 UINT16_TO_STREAM(pp, handle); 1691 UINT8_TO_STREAM(pp, unused); 1692 UINT8_TO_STREAM(pp, direction); 1693 UINT8_TO_STREAM(pp, service_type); 1694 UINT32_TO_STREAM(pp, token_rate); 1695 UINT32_TO_STREAM(pp, token_size); 1696 UINT32_TO_STREAM(pp, peak); 1697 UINT32_TO_STREAM(pp, latency); 1698 1699 btu_hcif_send_cmd(LOCAL_BR_EDR_CONTROLLER_ID, p); 1700 }

#include "master.h" #include <log_custom.h> #include <cstddef> #include <cstdint> #include <map> #include <string> #include <base/params_manager/params_manager.h> #include #include "fmt/core.h" #include "lib/sd_navigation/SDMapElementExtract.h" #include "message/env_model/routing_map/routing_map.h" #include "message/env_model/occ/occ.h" #include "message/sensor/camera/bev_lane/bev_lane.h" #include "cyber/event/trace.h" #include "cyber/time/time.h" #include "modules/perception/env/src/lib/base/localmap_conf.h" #include "modules/simulator/deterministic_scheduler/deterministic_scheduler.h" using byd::simulator::deterministic_scheduler::SchedulerUnitManager; using byd::simulator::deterministic_scheduler::TaskType; using namespace cem::fusion; using namespace cem::fusion::extendLane; namespace cem { namespace fusion { std::shared_ptr<cem::fusion::Master> MasterPtr = nullptr; std::shared_ptr<MasterInterface> GetMasterInstance() { if (!MasterPtr) { MasterPtr = std::shared_ptr<Master>(new Master("modules/perception/env", "local_map")); } return MasterPtr; } Master::Master(const std::string &confdir, const std::string &name) { conf_abs_path_ = confdir + std::string("/"); VLOG(1) << "conf_abs_path_ = " << conf_abs_path_; Init(); } void Master::Init() { LoadConfig(); InitMember(); } void Master::LoadConfig() { ParamsManager::Instance()->ReadJsonParams(conf_abs_path_ + "conf/TrafficRoadFusion/E2_Conf_TrafficRoadFusion.json", conf_abs_path_ + "conf/TrafficRoadFusion/E2_Conf_MeasurementEvaluation.json"); } void Master::InitMember() { routing_map_.reset(new RoutingMap()); pre_processor_.reset(new PreProcessor()); post_processor_.reset(new PostProcessor()); messenger_.reset(new Messenger()); stopline_mapping_.reset(new StoplineMapping); traffic_light_mapping_.reset(new TrafficLightMapping); bev_map_processor_.reset(new BevMapPreProcessor()); fusion_manager_.reset(new FusionManager()); fusion_manager_->Init(); map_fusion_.reset(new MapFusion); lane_guidance_.reset(new LaneGuidance); lane_guidance_city_.reset(new LaneGuidanceCity); env_info_processor_.reset(new navigation::EnvInfoProcessor()); traffic_flow_mapping_.reset(new TrafficFlowMapping); toggle_to_bev_info_.push(MapType::BEV_MAP); cross_road_construction_.reset(new CrossRoadConstruction); cross_road_construction_->Init(); extend_lane_.reset(new ExtendLane); debug_infos_.reset(new DebugInfos); sd_navigation_base_ = std::make_shared(); sd_navigation_highway_ = std::make_shared(); sd_navigation_city_ = std::make_shared(); if (FLAGS_init_map_source == "ld" || FLAGS_init_map_source == "LD") { toggle_to_bev_info_.push(MapType::HD_MAP); } else { toggle_to_bev_info_.push(MapType::BEV_MAP); } } #define MapLessFlag void Master::Proc() { env_status_ = std::make_shared<byd::msg::orin::routing_map::EnvStatus>(); //get previous cross_road status and cross road distance pre_processor_->is_cross_road_status = cross_road_construction_->IsCrossRoad(); pre_processor_->cross_road_distance = cross_road_construction_->CrossRoadDis(); pre_processor_->Process(); //get previous cross_road status pre_processor_->is_cross_road_status = cross_road_construction_->IsCrossRoad(); RoutingMapPtr routing_map_raw{nullptr}; SensorDataManager::Instance()->GetLatestSensorFrame(routing_map_raw); auto raw_bev_map_ptr = INTERNAL_PARAMS.raw_bev_data.GetRawBevMapPtr(); ///高速和城区的切换 bool IS_ON_HIGHWAY_FLAG = false; { IS_ON_HIGHWAY_FLAG = GetIsOnHighwayFlag(routing_map_raw); if (IS_ON_HIGHWAY_FLAG) { sd_navigation_base_ = sd_navigation_highway_; } else { sd_navigation_base_ = sd_navigation_city_; } } env_status_->set_is_on_freeway(IS_ON_HIGHWAY_FLAG == true); DetectBevMap bev_map = pre_processor_->GetGlobalBevMapPtr(); ///停止线处理 stopline_mapping_->Process(); const auto &bev_stopline_objs = stopline_mapping_->GetStopLines(); ///导航推荐 std::vector<std::pair<uint64_t, std::pair<int, int>>> guide_lane_result = {}; std::vector<uint64_t> sd_guide_lane_ids_ref = {}; sd_navigation_base_->Proc(routing_map_raw, raw_bev_map_ptr, bev_map.bev_map_ptr, guide_lane_result, sd_guide_lane_ids_ref); auto junctions_ptr = INTERNAL_PARAMS.navigation_info_data.GetJunctionInfoCityPtr(); if ((!IS_ON_HIGHWAY_FLAG) && (routing_map_raw != nullptr)) { auto trajectory = sd_navigation_city_->GetNextRightPoints(routing_map_raw); pre_processor_->SetRightTurnTrajectory(routing_map_raw->header.timestamp, trajectory); } ///env_info env_info_processor_->SetIsOnHighway(IS_ON_HIGHWAY_FLAG); env_info_processor_->Process(bev_stopline_objs); const EnvInfo *env_info = env_info_processor_->GetEnvInfo(); if (bev_map.bev_map_ptr != nullptr) { bev_map.bev_map_ptr->env_info = *env_info; } bev_map_processor_->Process(bev_map, sd_guide_lane_ids_ref); fusion_manager_->Process(); const auto routing_map = pre_processor_->routing_map_pre_processor_.GetRoutingMap(); if (routing_map != nullptr) { routing_map->env_info = *env_info; } extend_lane_->Process(bev_map); bev_ld_map_geometry_match_info_.CalculateMapGeometryMatchInfo(fusion_manager_->GetBevMapInfo(), fusion_manager_->GetLdMapInfo()); // cross road part cross_road_construction_->SetGeometryMatchInfo(&bev_ld_map_geometry_match_info_); cross_road_construction_->Process(bev_map.bev_map_ptr, bev_map.Twb, raw_bev_map_ptr, junctions_ptr); // Post-processing of the bev map and intergration the guide lane, virtual // lane, traffic light status, etc. lane_guidance_city_->Process(routing_map_raw, bev_map, bev_stopline_objs, guide_lane_result); traffic_light_mapping_->Process(routing_map_raw, routing_map, lane_guidance_city_->GetMapInfo(), sd_navigation_city_); GeometryMatchResult match_detail; auto map_convert_type = MapToggleMode(match_detail); messenger_->PulishDiagno(); messenger_->PulishTrafficLightE2E(traffic_light_mapping_->GetTrafficLightsE2E()); bool used_ld_map = toggle_to_bev_info_.back() == MapType::HD_MAP; uint32_t map_counter = 0; // std::string map_type_str = used_ld_map ? "ld" : "bev"; map_counter = used_ld_map ? (routing_map ? routing_map->header.cycle_counter : 2) : (lane_guidance_city_->GetMapInfo() ? lane_guidance_city_->GetMapInfo()->header.cycle_counter : 1); const auto &map_convert_params = ParamsManager::Instance()->GetMapConvertParams(); if (IS_ON_HIGHWAY_FLAG) { used_ld_map = false; env_status_->clear_hd_map_suppression_reason(); env_status_->clear_perception_map_suppression_reason(); if (env_info->is_switched_to_LD_ && match_detail.fail_reason != kLostHdMapLane) { env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_THE_RAMP); used_ld_map = true; } else { if (match_detail.fail_reason == kLostHdMapLane) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_OUTSIDE_MAP); } else { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_NOT_IN_THE_RAMP); } used_ld_map = false; } env_status_->clear_distance_to_next_junction(); env_status_->clear_distance_to_passed_junction(); env_status_->clear_left_laneline_match_info(); env_status_->clear_right_laneline_match_info(); // todo: add env status here } else { if (map_convert_params.map_stay_ld) { used_ld_map = true; } else if (map_convert_params.map_stay_bev) { used_ld_map = false; } } std::string map_type_str = used_ld_map ? "ld" : "bev"; if (!map_convert_params.map_stay_ld && used_ld_map) { const bool ld_has{false}; // const bool ld_has = bev_ld_map_geometry_match_info_.LDHasEnoughPoints(); // const bool bev_has = bev_ld_map_geometry_match_info_.BEVHasEnoughPoints(); if (!routing_map || routing_map->route.navi_start.section_id == 0U) { used_ld_map = false; if (!toggle_to_bev_info_.empty()) { toggle_to_bev_info_.pop(); } toggle_to_bev_info_.emplace(MapType::BEV_MAP); if (!routing_map) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_NO_HD_MAP_FRAME); } else { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_NO_NAVIGATION_START_SECTION); } env_status_->clear_perception_map_suppression_reason(); map_type_str = fmt::format("bev ld_less_points.[{:d}-{}-{:d}]", static_cast<bool>(routing_map), routing_map ? routing_map->route.navi_start.section_id : 0, ld_has); } } RoutingMapPtr routing_map_copy_ptr = nullptr; if (nullptr != routing_map) { bool use_ld_tmp = used_ld_map; if (true == used_ld_map) { routing_map->sensor_status_info.sensor_status = 2; } post_processor_->run(routing_map, used_ld_map); if (use_ld_tmp && !used_ld_map) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_CLOSE_TO_HIGH_LEVEL_ROAD); env_status_->clear_perception_map_suppression_reason(); env_status_->clear_distance_to_next_junction(); env_status_->clear_distance_to_passed_junction(); env_status_->clear_left_laneline_match_info(); env_status_->clear_right_laneline_match_info(); } map_fusion_->SetGeometryMatchInfo(&bev_ld_map_geometry_match_info_); routing_map_copy_ptr = std::make_shared<RoutingMap>(*routing_map); map_fusion_->Process(bev_map, routing_map_copy_ptr); } if (used_ld_map) { env_status_->set_map_source(byd::msg::orin::routing_map::EnvStatus::EnvSource::EnvStatus_EnvSource_HD_MAP); } else { env_status_->set_map_source(byd::msg::orin::routing_map::EnvStatus::EnvSource::EnvStatus_EnvSource_PERCEPTION_MAP); } #ifdef MapLessFlag used_ld_map = false; #endif debug_infos_->SetDebugInfo(used_ld_map, lane_guidance_city_->GetMapInfo(), lane_guidance_city_->GetTwb(), routing_map_copy_ptr); messenger_->PulishFusionMap(!used_ld_map, lane_guidance_city_->GetMapInfo(), routing_map_copy_ptr, bev_stopline_objs, IS_ON_HIGHWAY_FLAG, sd_navigation_city_->GetInfo()); if (auto map_output = messenger_->GetFusionMapInfo(); map_output) { double adc_to_junction = traffic_light_mapping_->DistanceToJunction(); uint64_t ld_counter = routing_map == nullptr ? 0 : routing_map->header.cycle_counter; uint64_t bev_counter = lane_guidance_city_->GetMapInfo() == nullptr ? 0 : lane_guidance_city_->GetMapInfo()->header.cycle_counter; map_output->mutable_map_info()->mutable_env_status()->CopyFrom(*env_status_); map_output->mutable_map_info()->set_map_version( fmt::format("map_type:{} ld_counter:{} bev_counter:{} vision_counter:{} " "adc_to_junction:{:.2f} map_change_type:{} " "stay_ld:{:d} stay_bev:{:d} IS_ON_HIGHWAY_FLAG:{:d} " "used_ld_map:{:d} is_switched:{:d}", map_type_str, ld_counter, bev_counter, traffic_light_mapping_->GetPerceptionCyberCounter(), adc_to_junction, to_string(map_convert_type), map_convert_params.map_stay_ld, map_convert_params.map_stay_bev, IS_ON_HIGHWAY_FLAG, used_ld_map, env_info->is_switched_to_LD_) + sd_navigation_city_->GetInfo()); map_output->mutable_map_info()->set_engine_version( bev_ld_map_geometry_match_info_.GetDebugInfo() + fmt::format(" is_keep_lane:{:d} is_right:{:d} ", is_lane_keeping_, sd_navigation_city_->IsEgoInDedicatedRightLane()) + traffic_light_mapping_->TrafficLightLaneDebug()); GEOMETRY_LOG << map_output->map_info().map_version(); TRAFFIC_LOG << map_output->map_info().engine_version(); } } bool Master::GetIsOnHighwayFlag(const RoutingMapPtr &routing_map_raw) { auto is_on_highway_flag = [](const SDSectionInfo &sd_section_info) { auto ¤t_road_class = sd_section_info.road_class; uint32_t current_section_type = sd_section_info.link_type; uint64_t sd_section_id = sd_section_info.id / 10; if ((sd_section_id == 1678534592) || (sd_section_id == 1699361336) || (sd_section_id == 1699374097) || (sd_section_id == 1691246158) || (sd_section_id == 1607693456) || (sd_section_id == 1607693601) || (sd_section_id == 1648672436) || (sd_section_id == 1648672824) || (sd_section_id == 1678890528) || (sd_section_id == 1705771743) || (sd_section_id == 1705771555) || (sd_section_id == 1699402002) || (sd_section_id == 1699402004) || (sd_section_id == 1613352349) || (sd_section_id == 1678533134) || (sd_section_id == 1699372997) || (sd_section_id == 1699374094) || (sd_section_id == 1607690036) || (sd_section_id == 1607690332) || (sd_section_id == 1664789284) || (sd_section_id == 1664789271) || (sd_section_id == 1648672814) || (sd_section_id == 1678890452) || (sd_section_id == 1699443498) || (sd_section_id == 1705771744) || (sd_section_id == 1705771880) || (sd_section_id == 1705771278) || (sd_section_id == 1705771745) || (sd_section_id == 1699402006) || (sd_section_id == 1699402007) || (sd_section_id == 1705771745) || (sd_section_id == 1663998523) || (sd_section_id == 1582251492)) { return false; } if (current_road_class == SDRoadClass::SD_HIGHWAY || current_road_class == SDRoadClass::SD_CITY_FAST_WAY || (current_section_type & (uint32_t)SDLinkTypeMask::SDLT_IC) == (uint32_t)SDLinkTypeMask::SDLT_IC || (current_section_type & (uint32_t)SDLinkTypeMask::SDLT_JCT) == (uint32_t)SDLinkTypeMask::SDLT_JCT || (current_section_type & (uint32_t)SDLinkTypeMask::SDLT_SERVICE) == (uint32_t)SDLinkTypeMask::SDLT_SERVICE) { return true; } return false; }; auto is_highway_ramp = [](const SDSectionInfo &sd_section_info) { uint32_t section_type = sd_section_info.link_type; if ((section_type & (uint32_t)SDLinkTypeMask::SDLT_JCT) == (uint32_t)SDLinkTypeMask::SDLT_JCT || (section_type & (uint32_t)SDLinkTypeMask::SDLT_IC) == (uint32_t)SDLinkTypeMask::SDLT_IC || (section_type & (uint32_t)SDLinkTypeMask::SDLT_SERVICE) == (uint32_t)SDLinkTypeMask::SDLT_SERVICE || (section_type & (uint32_t)SDLinkTypeMask::SDLT_PARK) == (uint32_t)SDLinkTypeMask::SDLT_PARK || (section_type & (uint32_t)SDLinkTypeMask::SDLT_RAMP) == (uint32_t)SDLinkTypeMask::SDLT_RAMP || (sd_section_info.road_class == SDRoadClass::SD_OTHER_ROAD && (section_type & (uint32_t)SDLinkTypeMask::SDLT_POICONNECTION) == (uint32_t)SDLinkTypeMask::SDLT_POICONNECTION)) { return true; } else { return false; } }; auto is_highway_city_connection = [is_on_highway_flag, is_highway_ramp]( const SDSectionInfo &pre_sd_section_info, const SDSectionInfo &cur_sd_section_info, const SDSectionInfo &suc_sd_section_info) { uint32_t cur_section_type = cur_sd_section_info.link_type; if ((pre_sd_section_info.successor_section_id_list.size() == 1) && (is_on_highway_flag(pre_sd_section_info)) && ((cur_section_type & (uint32_t)SDLinkTypeMask::SDLT_IC) == (uint32_t)SDLinkTypeMask::SDLT_IC) && (!is_on_highway_flag(suc_sd_section_info) && (!is_highway_ramp(suc_sd_section_info)))) { return true; } else { return false; } }; if (routing_map_raw == nullptr) { return false; } auto &sd_mpp_sections = routing_map_raw->sd_route.mpp_sections; uint64_t sd_ego_sectionid = routing_map_raw->sd_route.navi_start.section_id; int ego_section_index = -1; for (unsigned int i = 0; i < sd_mpp_sections.size(); i++) { if (sd_ego_sectionid == sd_mpp_sections[i].id) { ego_section_index = i; break; } } if (ego_section_index == -1) { return routing_map_raw->is_on_highway; } auto ¤t_section = sd_mpp_sections[ego_section_index]; bool IS_ON_HIGHWAY_FLAG = routing_map_raw->is_on_highway; if (IS_ON_HIGHWAY_FLAG) { if (!is_on_highway_flag(current_section)) { return false; } if (ego_section_index >= 1 && ego_section_index < sd_mpp_sections.size() - 1) { if (is_highway_city_connection(sd_mpp_sections[ego_section_index - 1], current_section, sd_mpp_sections[ego_section_index + 1])) { return false; } } if (is_highway_ramp(current_section)) { return true; } double length_tmp = fabs(current_section.length - routing_map_raw->sd_route.navi_start.s_offset); double max_range = 1500; for (unsigned int i = ego_section_index + 1; i < sd_mpp_sections.size(); i++) { if (max_range < length_tmp) { break; } if (i >= 1 && i < sd_mpp_sections.size() - 1) { if (is_highway_city_connection(sd_mpp_sections[i - 1], sd_mpp_sections[i], sd_mpp_sections[i + 1])) { IS_ON_HIGHWAY_FLAG = false; break; } } if (is_on_highway_flag(sd_mpp_sections[i]) && is_highway_ramp(sd_mpp_sections[i])) { break; } if (!is_on_highway_flag(sd_mpp_sections[i])) { IS_ON_HIGHWAY_FLAG = false; break; } length_tmp += sd_mpp_sections[i].length; } return IS_ON_HIGHWAY_FLAG; } else { return false; } } MapConvertType Master::MapToggleMode(GeometryMatchResult &match_detail) { static constexpr size_t queue_max_size = 30; if (bev_ld_matched_info_.size() > queue_max_size) { bev_ld_matched_info_.pop(); } if (toggle_to_bev_info_.size() > queue_max_size) { toggle_to_bev_info_.pop(); } constexpr double distance_bev_to_ld_max = 150; constexpr double distance_bev_to_ld_min = 0.5; constexpr double distance_ld_to_bev_max = 150; constexpr double distance_threshold = 100; const bool bev_ld_boundary_matched = bev_ld_map_geometry_match_info_.IsLaneBoundaryMatch(match_detail); double adc_to_junction = traffic_light_mapping_->DistanceToJunction(); bool is_drive_into_unreliable_road = traffic_light_mapping_->IsUnreliableRoadCrossJunction(); is_drive_into_unreliable_road = false; bool distance_bev_to_ld = adc_to_junction < distance_bev_to_ld_max && adc_to_junction > distance_bev_to_ld_min; bool bev_ld_matched_current_period = distance_bev_to_ld && bev_ld_boundary_matched; double dis_ego_to_passed_junction = traffic_light_mapping_->GetPreviousJunctionEgoPassed(); LaneInfo ego_lane_ld = bev_ld_map_geometry_match_info_.GetLdBevLane(); bool ego_in_junction = ego_lane_ld.id != 0 && ego_lane_ld.type == cem::message::env_model::LaneType::LANE_VIRTUAL_JUNCTION; PLanningResultPtr planning_result{nullptr}; SensorDataManager::Instance()->GetLatestSensorFrame(planning_result); if (planning_result) { if (planning_result->lc_state == PLanningResultData::Keeping) { is_lane_keeping_ = true; } else { is_lane_keeping_ = false; } } if (!env_status_->is_on_freeway() && ((match_detail.fail_reason == MatchFailReason::kNotFailed) || (match_detail.fail_reason == MatchFailReason::kMatchFailed))) { env_status_->set_distance_to_next_junction(adc_to_junction); env_status_->set_distance_to_passed_junction(dis_ego_to_passed_junction); env_status_->mutable_left_laneline_match_info()->set_average_offset(match_detail.left_offset_average); env_status_->mutable_left_laneline_match_info()->set_max_offset(match_detail.left_offset_max); env_status_->mutable_right_laneline_match_info()->set_average_offset(match_detail.right_offset_average); env_status_->mutable_right_laneline_match_info()->set_max_offset(match_detail.right_offset_max); } if (env_status_->is_on_freeway() && match_detail.fail_reason == MatchFailReason::kLostHdMapLane && (!is_lane_keeping_)) { match_detail.fail_reason = MatchFailReason::kNotFailed; } GEOMETRY_LOG << fmt::format( " bev_ld_boundary_matched:{:d} distance_to_junction:{:.2f} ego_in_junction:{:d} vector_type:{:d} size:{:d} " "ego_dis_to_last_junction:{:.2f} is_lane_keeping:{:d} bev_ld_matched_current_period:{:d}", bev_ld_boundary_matched, adc_to_junction, ego_in_junction, (toggle_to_bev_info_.empty() ? MapType::UNKOWN_MAP : toggle_to_bev_info_.back()), toggle_to_bev_info_.size(), dis_ego_to_passed_junction, is_lane_keeping_, bev_ld_matched_current_period); switch (toggle_to_bev_info_.back()) { case MapType::HD_MAP: { if (adc_to_junction > distance_ld_to_bev_max && bev_ld_boundary_matched && dis_ego_to_passed_junction > distance_threshold && is_lane_keeping_) { toggle_to_bev_info_.emplace(MapType::BEV_MAP); env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_PASSED_JUNCTION); return MapConvertType::LD_2_BEV; } if (!is_lane_keeping_) { env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IS_CHANGING_LANE); } else if (adc_to_junction <= distance_ld_to_bev_max || dis_ego_to_passed_junction <= distance_threshold) { env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_JUNCTION); } else { env_status_->set_perception_map_suppression_reason(byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason:: EnvStatus_MapSuppressionReason_UNABLE_TO_MATCH_PERCEPTION_WITH_HD_MAP); } toggle_to_bev_info_.emplace(MapType::HD_MAP); return MapConvertType::LD_2_LD; } case MapType::BEV_MAP: { if (bev_ld_matched_current_period && is_lane_keeping_ && (!is_drive_into_unreliable_road)) { toggle_to_bev_info_.emplace(MapType::HD_MAP); env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_JUNCTION); return MapConvertType::BEV_2_LD; } if (adc_to_junction < distance_bev_to_ld_min) { toggle_to_bev_info_.emplace(MapType::UNKOWN_MAP); env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_UNKNOWN_REASON); return MapConvertType::BEV_2_UNKNOWN; } toggle_to_bev_info_.emplace(MapType::BEV_MAP); if (!is_lane_keeping_) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IS_CHANGING_LANE); } else if (is_drive_into_unreliable_road) { env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_CLOSE_TO_HIGH_LEVEL_ROAD); } else { if (adc_to_junction > distance_bev_to_ld_max) { env_status_->set_hd_map_suppression_reason(byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason:: EnvStatus_MapSuppressionReason_EGO_NOT_CLOSE_ENOUGH_TO_JUNCTION); } else { env_status_->set_hd_map_suppression_reason(byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason:: EnvStatus_MapSuppressionReason_UNABLE_TO_MATCH_PERCEPTION_WITH_HD_MAP); } } return MapConvertType::BEV_2_BEV; } case MapType::UNKOWN_MAP: { if (bev_ld_matched_current_period && is_lane_keeping_) { toggle_to_bev_info_.emplace(MapType::HD_MAP); env_status_->set_perception_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_EGO_IN_JUNCTION); return MapConvertType::UNKNOWN_2_LD; } if (adc_to_junction > distance_bev_to_ld_min * 2 && is_lane_keeping_) { toggle_to_bev_info_.emplace(MapType::BEV_MAP); env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_UNKNOWN_REASON); return MapConvertType::UNKNOWN_2_BEV; } env_status_->set_hd_map_suppression_reason( byd::msg::orin::routing_map::EnvStatus::MapSuppressionReason::EnvStatus_MapSuppressionReason_UNKNOWN_REASON); toggle_to_bev_info_.emplace(MapType::UNKOWN_MAP); return MapConvertType::UNKNOWN_2_UNKNOWN; } default: { toggle_to_bev_info_.emplace(MapType::BEV_MAP); return MapConvertType::DEFAULT_BEV; } } toggle_to_bev_info_.emplace(MapType::BEV_MAP); return MapConvertType::DEFAULT_BEV; } /**********************************发送消息的数据结构接口************************************/ std::shared_ptr<byd::msg::basic::ModuleStatus> Master::GetModuleStatusPtr() { return this->messenger_->GetDiagnoInfo(); } std::shared_ptr<byd::msg::orin::routing_map::RoutingMap> Master::GetENVRoutingMapPtr() { return this->messenger_->GetRoutingMapInfo(); } std::shared_ptr<byd::msg::orin::routing_map::TrafficLightsE2EInfo> Master::GetTrafficLightsE2EPtr() { return this->messenger_->GetTrafficLightsE2EInfo(); } /***************************接收消息callback函数 * 在此处实现************************************/ void Master::OnVehInfoCallback(const std::shared_ptr<byd::msg::drivers::VehInfo> &msg) { this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::VehicleSignal>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Bcm50msPdu02>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Ibs20msPdu08>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Imcu20msPdu05>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Eps010msPdu00>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Ibs20msPdu15>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Sas10msPdu00>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Mpd100msPdu04>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::Icm100msPdu29>(msg); this->messenger_->Callback<byd::msg::drivers::VehInfo, cem::message::sensor::AdasIpd100msPdu12>(msg); } void Master::OnEnvModelLocalMapInfoCallback(const std::shared_ptr<byd::msg::env_model::LocalMapInfo> &msg) { this->messenger_->Callback<byd::msg::env_model::LocalMapInfo, cem::message::sensor::BevMapInfo>(msg); } void Master::OnRoutingMapCallBack(const std::shared_ptr<byd::msg::orin::routing_map::RoutingMap> &msg) { this->messenger_->Callback<byd::msg::orin::routing_map::RoutingMap, cem::message::env_model::RoutingMap>(msg); } void Master::OnMsgMapEventCallback(const std::shared_ptr<byd::msg::orin::routing_map::MapEvent> &msg) { this->messenger_->Callback<byd::msg::orin::routing_map::MapEvent, cem::message::env_model::MapEvent>(msg); } /* OnMsgMapEventCallback */ void Master::OnPercepTrfInfoCallback(const std::shared_ptr<byd::msg::perception::PerceptionTrafficInfo> &msg) { this->messenger_->Callback<byd::msg::perception::PerceptionTrafficInfo, cem::message::sensor::PercepTrfInfo>(msg); } void Master::OnPerceptionObstaclesInfoCallback(const std::shared_ptr<byd::msg::perception::PerceptionObstacles> &msg) { this->messenger_->Callback<byd::msg::perception::PerceptionObstacles, cem::message::sensor::FusObjInfo>(msg); } void Master::OnMsgDriverInsCallback(const std::shared_ptr<byd::msg::drivers::Ins> &msg) { this->messenger_->Callback<byd::msg::drivers::Ins, cem::message::sensor::LocalizationData>(msg); } void Master::OnMsgLocationDrCallback(const std::shared_ptr<byd::msg::localization::LocalizationEstimate> &msg) { this->messenger_->Callback<byd::msg::localization::LocalizationEstimate, cem::message::sensor::LocalizationData>(msg); } // void Master::OncanoutCallback( // const std::shared_ptr<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output> &msg) // { // this->messenger_->Callback<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output, // cem::message::sensor::CAN1>(msg); // } void Master::OnEnvModelLidaRoadedgeCallback(const std::shared_ptr<byd::msg::env_model::LocalMapInfo> &msg) { this->messenger_->Callback<byd::msg::env_model::LocalMapInfo, cem::message::sensor::LidarRoadEdgeInfo>(msg); } void Master::OnPlanResultCallback(const std::shared_ptr<byd::msg::planning::PLanningResultProto> &msg) { this->messenger_->Callback<byd::msg::planning::PLanningResultProto, cem::message::env_model::PLanningResultData>(msg); } void Master::OnPlanFuncStateCallback(const std::shared_ptr<byd::msg::pnc::PlanFuncState> &msg) { this->messenger_->Callback<byd::msg::pnc::PlanFuncState, cem::message::env_model::PLanFuncState>(msg); } void Master::OncanoutCallback(const std::shared_ptr<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output> &msg) { this->messenger_->Callback<byd::msg::orin::sm_msgs::MsgSM_BYD_CAN_Output, cem::message::env_model::CAN1>(msg); } void Master::OnOccCallback(const std::shared_ptr<byd::msg::perception::OCCInfo>& msg){ this->messenger_->Callback<byd::msg::perception::OCCInfo,cem::env_model::occ::OCCInfo>(msg); return; } /***************************************************************************************************/ } // namespace fusion } // namespace cem 解读下代码

#include <ntifs.h> #include <ntddk.h> #include <intrin.h> #include "ptehook.h" #define CR0_WP (1 << 16) // 在类定义前添加以下常量定义 #define DIRECT_MAPPING_BASE 0xFFFF000000000000ULL // 页表标志位 #define PTE_GLOBAL_FLAG (1ULL << 8) #define PDE_GLOBAL_FLAG (1ULL << 8) #define PDPTE_GLOBAL_FLAG (1ULL << 8) #define PTE_WRITABLE_FLAG (1ULL << 1) #define PTE_NOEXECUTE_FLAG (1ULL << 63) #define PDE_2MB_PAGE_FLAG (1ULL << 7) #define PDPTE_1GB_PAGE_FLAG (1ULL << 7) HANDLE targetProcessId = NULL; typedef INT(*LDE_DISASM)(PVOID address, INT bits); typedef unsigned long DWORD; typedef unsigned __int64 ULONG64; // 使用WDK标准类型 typedef unsigned char BYTE; typedef LONG NTSTATUS; // 修正后的跳转指令结构 #pragma pack(push, 1) typedef struct _JMP_ABS { BYTE opcode[6]; // FF 25 00 00 00 00 ULONG64 address; // 8字节绝对地址 } JMP_ABS, * PJMP_ABS; #pragma pack(pop) LDE_DISASM lde_disasm; // 初始化引擎 VOID lde_init() { lde_disasm = (LDE_DISASM)ExAllocatePool(NonPagedPool, 12800); memcpy(lde_disasm, szShellCode, 12800); } // 得到完整指令长度,避免截断 ULONG GetFullPatchSize(PUCHAR Address) { ULONG LenCount = 0, Len = 0; // 至少需要14字节 while (LenCount <= 14) { Len = lde_disasm(Address, 64); Address = Address + Len; LenCount = LenCount + Len; } return LenCount; } #define PROCESS_NAME_LENGTH 16 #define DRIVER_TAG 'HKOB' EXTERN_C char* PsGetProcessImageFileName(PEPROCESS process); char target_process_name[] = "oxygen.exe"; typedef NTSTATUS(*fn_ObReferenceObjectByHandleWithTag)( HANDLE Handle, ACCESS_MASK DesiredAccess, POBJECT_TYPE ObjectType, KPROCESSOR_MODE AccessMode, ULONG Tag, PVOID* Object, POBJECT_HANDLE_INFORMATION HandleInformation ); fn_ObReferenceObjectByHandleWithTag g_OriginalObReferenceObjectByHandleWithTag = NULL; // PTE Hook Framework #define MAX_G_BIT_RECORDS 128 #define MAX_HOOK_COUNT 64 #define PAGE_ALIGN(va) ((PVOID)((ULONG_PTR)(va) & ~0xFFF)) #define PDPTE_PS_BIT (1 << 7) #define PDE_PS_BIT (1 << 7) #define PTE_NX_BIT (1ULL << 63) #define CACHE_WB (6ULL << 3) // 页表结构定义 typedef struct _PAGE_TABLE { UINT64 LineAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 pat : 1; UINT64 global : 1; UINT64 ignored_1 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_2 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PteAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 large_page : 1; UINT64 global : 1; UINT64 ignored_2 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PdeAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 ignored_1 : 1; UINT64 page_size : 1; UINT64 ignored_2 : 4; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PdpteAddress; UINT64* Pml4Address; BOOLEAN IsLargePage; BOOLEAN Is1GBPage; UINT64 OriginalPte; UINT64 OriginalPde; UINT64 OriginalPdpte; UINT64 OriginalPml4e; HANDLE ProcessId; } PAGE_TABLE, * PPAGE_TABLE; // G位信息记录结构体 typedef struct _G_BIT_INFO { void* AlignAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 large_page : 1; UINT64 global : 1; UINT64 ignored_2 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PdeAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 pat : 1; UINT64 global : 1; UINT64 ignored_1 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_2 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PteAddress; BOOLEAN IsLargePage; } G_BIT_INFO, * PG_BIT_INFO; typedef struct _HOOK_INFO { void* OriginalAddress; void* HookAddress; UINT8 OriginalBytes[20]; UINT8 HookBytes[20]; UINT32 HookLength; BOOLEAN IsHooked; HANDLE ProcessId; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 pat : 1; UINT64 global : 1; UINT64 ignored_1 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_2 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*HookedPte; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 large_page : 1; UINT64 global : 1; UINT64 ignored_2 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*HookedPde; } HOOK_INFO; class PteHookManager { public: bool fn_pte_inline_hook_bp_pg(HANDLE process_id, _Inout_ void** ori_addr, void* hk_addr); bool fn_remove_hook(HANDLE process_id, void* hook_addr); static PteHookManager* GetInstance(); HOOK_INFO* GetHookInfo() { return m_HookInfo; } char* GetTrampLinePool() { return m_TrampLinePool; } UINT32 GetHookCount() { return m_HookCount; } bool fn_resume_global_bits(void* align_addr); ~PteHookManager(); // 添加析构函数声明 bool fn_modify_pte_permission(HANDLE process_id, void* addr, bool make_writable); UINT64 get_process_cr3(PEPROCESS process); private: bool WriteTrampolineInstruction(void* trampoline, const JMP_ABS& jmpCmd); void fn_add_g_bit_info(void* align_addr, void* pde_address, void* pte_address); bool fn_isolation_pagetable(UINT64 cr3_val, void* replace_align_addr, void* split_pde); bool fn_isolation_pages(HANDLE process_id, void* ori_addr); bool fn_split_large_pages(void* in_pde, void* out_pde); NTSTATUS get_page_table(UINT64 cr3, PAGE_TABLE& table); void* fn_pa_to_va(UINT64 pa); UINT64 fn_va_to_pa(void* va); __forceinline KIRQL DisableWriteProtection(); __forceinline void EnableWriteProtection(KIRQL oldIrql); void logger(const char* info, bool is_err, LONG err_code = 0); void PrintPageTableInfo(const PAGE_TABLE& table); void PrintHookInfo(const HOOK_INFO& hookInfo); void PrintGBitInfo(const G_BIT_INFO& gbitInfo); UINT64 m_OriginalPermissions; struct MdlEntry { PVOID va; PMDL mdl; }; static constexpr UINT32 MAX_MDL_CACHE = 16; // 添加常量定义 MdlEntry m_MdlCache[MAX_MDL_CACHE]; // MDL 缓存数组 UINT32 m_MdlCount = 0; // 当前缓存数量 // 添加 fn_unmap_pa 声明 void fn_unmap_pa(PVOID va); static constexpr SIZE_T MAX_HOOKS = 256; // 根据需求调整 G_BIT_INFO m_GbitRecords[MAX_G_BIT_RECORDS]; UINT32 m_GbitCount = 0; void* m_PteBase = 0; HOOK_INFO m_HookInfo[MAX_HOOK_COUNT] = { 0 }; DWORD m_HookCount = 0; char* m_TrampLinePool = nullptr; // 合并为一个声明 UINT32 m_PoolUsed = 0; static PteHookManager* m_Instance; UINT64 m_OriginalPermissions; }; PteHookManager* PteHookManager::m_Instance = nullptr; void PteHookManager::fn_unmap_pa(PVOID va) { if (!va) return; // 查找 MDL 映射 for (UINT32 i = 0; i < m_MdlCount; i++) { if (m_MdlCache[i].va == va) { MmUnmapLockedPages(va, m_MdlCache[i].mdl); IoFreeMdl(m_MdlCache[i].mdl); // 从缓存中移除 if (i < m_MdlCount - 1) { RtlMoveMemory(&m_MdlCache[i], &m_MdlCache[i + 1], (m_MdlCount - i - 1) * sizeof(MdlEntry)); } m_MdlCount--; return; } } UINT64 PteHookManager::get_process_cr3(PEPROCESS process) { #if defined(_AMD64_) // Windows 10/11 的CR3偏移通常是0x28 return *(UINT64*)((PUCHAR)process + 0x28); #elif defined(_X86_) return *(UINT64*)((PUCHAR)process + 0x18); #endif } // 尝试释放 MmMapIoSpace 映射 PHYSICAL_ADDRESS physAddr = MmGetPhysicalAddress(va); if (physAddr.QuadPart != 0) { MmUnmapIoSpace(va, PAGE_SIZE); } } // 修改析构函数释放所有 MDL 资源 PteHookManager::~PteHookManager() { // 清理 MDL 缓存 for (UINT32 i = 0; i < m_MdlCount; i++) { if (m_MdlCache[i].va) { MmUnmapLockedPages(m_MdlCache[i].va, m_MdlCache[i].mdl); IoFreeMdl(m_MdlCache[i].mdl); } } m_MdlCount = 0; // 清理其他资源 if (m_TrampLinePool) { MmFreeContiguousMemory(m_TrampLinePool); m_TrampLinePool = nullptr; } } // 实现部分 __forceinline KIRQL PteHookManager::DisableWriteProtection() { KIRQL oldIrql = KeRaiseIrqlToDpcLevel(); UINT64 cr0 = __readcr0(); __writecr0(cr0 & ~0x10000); // 清除CR0.WP位 _mm_mfence(); return oldIrql; } __forceinline void PteHookManager::EnableWriteProtection(KIRQL oldIrql) { _mm_mfence(); UINT64 cr0 = __readcr0(); __writecr0(cr0 | 0x10000); // 设置CR0.WP位 KeLowerIrql(oldIrql); } void PteHookManager::logger(const char* info, bool is_err, LONG err_code) { if (is_err) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] ERROR: %s (0x%X)\n", info, err_code); } else { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] INFO: %s\n", info); } } void PteHookManager::PrintPageTableInfo(const PAGE_TABLE& table) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] Page Table Info for VA: 0x%p\n", (void*)table.LineAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PML4E: 0x%llx (Address: 0x%p)\n", table.OriginalPml4e, table.Pml4Address); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PDPTE: 0x%llx (Address: 0x%p), Is1GBPage: %d\n", table.OriginalPdpte, table.PdpteAddress, table.Is1GBPage); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PDE: 0x%llx (Address: 0x%p), IsLargePage: %d\n", table.OriginalPde, table.PdeAddress, table.IsLargePage); if (!table.IsLargePage && !table.Is1GBPage) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PTE: 0x%llx (Address: 0x%p)\n", table.OriginalPte, table.PteAddress); } } void PteHookManager::PrintHookInfo(const HOOK_INFO& hookInfo) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] Hook Info:\n"); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Original Address: 0x%p\n", hookInfo.OriginalAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Hook Address: 0x%p\n", hookInfo.HookAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Hook Length: %d\n", hookInfo.HookLength); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Is Hooked: %d\n", hookInfo.IsHooked); // 打印原始字节 DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Original Bytes: "); for (UINT32 i = 0; i < sizeof(hookInfo.OriginalBytes); i++) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "%02X ", hookInfo.OriginalBytes[i]); } DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "\n"); // 打印Hook字节 DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Hook Bytes: "); for (UINT32 i = 0; i < sizeof(hookInfo.HookBytes); i++) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "%02X ", hookInfo.HookBytes[i]); } DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "\n"); } void PteHookManager::PrintGBitInfo(const G_BIT_INFO& gbitInfo) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] G-Bit Info:\n"); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Align Address: 0x%p\n", gbitInfo.AlignAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " IsLargePage: %d\n", gbitInfo.IsLargePage); if (gbitInfo.PdeAddress) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PDE: 0x%llx (Address: 0x%p)\n", gbitInfo.PdeAddress->value, gbitInfo.PdeAddress); } if (gbitInfo.PteAddress) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PTE: 0x%llx (Address: 0x%p)\n", gbitInfo.PteAddress->value, gbitInfo.PteAddress); } } void* PteHookManager::fn_pa_to_va(UINT64 pa) { PHYSICAL_ADDRESS physAddr; physAddr.QuadPart = pa; // 1. 验证物理地址有效性 if (!MmIsPhysicalAddressValid(physAddr)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PA] 物理地址无效: 0x%llx\n", pa); return nullptr; } // 2. 尝试使用MmMapIoSpace PVOID mappedVa = MmMapIoSpace(physAddr, PAGE_SIZE, MmNonCached); if (mappedVa) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PA] MmMapIoSpace映射成功: PA=0x%llx -> VA=0x%p\n", pa, mappedVa); return mappedVa; } // 3. 低4GB直接映射 if (physAddr.QuadPart < 0x100000000) { PVOID directVa = (PVOID)(physAddr.QuadPart + DIRECT_MAPPING_BASE); if (MmIsAddressValid(directVa)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PA] 低4GB直接映射成功: PA=0x%llx -> VA=0x%p\n", pa, directVa); return directVa; } } // 4. 使用MDL映射 PMDL pMdl = IoAllocateMdl(nullptr, PAGE_SIZE, FALSE, FALSE, NULL); if (!pMdl) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PA] MDL分配失败: PA=0x%llx\n", pa); return nullptr; } PVOID va = nullptr; __try { MmBuildMdlForNonPagedPool(pMdl); MmProbeAndLockPages(pMdl, KernelMode, IoReadAccess); va = MmGetSystemAddressForMdlSafe(pMdl, NormalPagePriority); if (va) { // 添加到MDL缓存 if (m_MdlCount < MAX_MDL_CACHE) { m_MdlCache[m_MdlCount].va = va; m_MdlCache[m_MdlCount].mdl = pMdl; m_MdlCount++; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PA] MDL映射缓存成功: PA=0x%llx -> VA=0x%p (缓存索引: %d)\n", pa, va, m_MdlCount - 1); } else { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_WARNING_LEVEL, "[PA] MDL缓存已满,临时映射: PA=0x%llx -> VA=0x%p\n", pa, va); return va; // 临时映射,调用者需负责释放 } } } __except (EXCEPTION_EXECUTE_HANDLER) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PA] 物理地址转换异常: PA=0x%llx, 代码: 0x%X\n", pa, GetExceptionCode()); IoFreeMdl(pMdl); return nullptr; } if (!va) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PA] MDL映射失败: PA=0x%llx\n", pa); IoFreeMdl(pMdl); } return va; } UINT64 PteHookManager::fn_va_to_pa(void* va) { PHYSICAL_ADDRESS physAddr = MmGetPhysicalAddress(va); return physAddr.QuadPart; } NTSTATUS PteHookManager::get_page_table(UINT64 cr3_val, PAGE_TABLE& table) { UINT64 va = table.LineAddress; UINT64 pml4e_index = (va >> 39) & 0x1FF; UINT64 pdpte_index = (va >> 30) & 0x1FF; UINT64 pde_index = (va >> 21) & 0x1FF; UINT64 pte_index = (va >> 12) & 0x1FF; // PML4 UINT64 pml4_pa = cr3_val & ~0xFFF; UINT64* pml4_va = (UINT64*)fn_pa_to_va(pml4_pa); if (!pml4_va) return STATUS_INVALID_ADDRESS; table.Pml4Address = &pml4_va[pml4e_index]; table.OriginalPml4e = *table.Pml4Address; if (!(table.OriginalPml4e & 1)) return STATUS_ACCESS_VIOLATION; // PDPTE UINT64 pdpte_pa = table.OriginalPml4e & ~0xFFF; UINT64* pdpte_va = (UINT64*)fn_pa_to_va(pdpte_pa); if (!pdpte_va) return STATUS_INVALID_ADDRESS; table.PdpteAddress = (decltype(table.PdpteAddress))&pdpte_va[pdpte_index]; table.OriginalPdpte = table.PdpteAddress->value; table.Is1GBPage = (table.OriginalPdpte & PDPTE_1GB_PAGE_FLAG) ? TRUE : FALSE; if (!(table.OriginalPdpte & 1)) return STATUS_ACCESS_VIOLATION; if (table.Is1GBPage) return STATUS_SUCCESS; // PDE UINT64 pde_pa = table.OriginalPdpte & ~0xFFF; UINT64* pde_va = (UINT64*)fn_pa_to_va(pde_pa); if (!pde_va) return STATUS_INVALID_ADDRESS; table.PdeAddress = (decltype(table.PdeAddress))&pde_va[pde_index]; table.OriginalPde = table.PdeAddress->value; table.IsLargePage = (table.OriginalPde & PDE_2MB_PAGE_FLAG) ? TRUE : FALSE; if (!(table.OriginalPde & 1)) return STATUS_ACCESS_VIOLATION; if (table.IsLargePage) return STATUS_SUCCESS; // PTE UINT64 pte_pa = table.OriginalPde & ~0xFFF; UINT64* pte_va = (UINT64*)fn_pa_to_va(pte_pa); if (!pte_va) return STATUS_INVALID_ADDRESS; table.PteAddress = (decltype(table.PteAddress))&pte_va[pte_index]; table.OriginalPte = table.PteAddress->value; if (!(table.OriginalPte & 1)) return STATUS_ACCESS_VIOLATION; // 打印页表信息 PrintPageTableInfo(table); return STATUS_SUCCESS; } bool PteHookManager::fn_split_large_pages(void* in_pde_ptr, void* out_pde_ptr) { auto in_pde = (decltype(PAGE_TABLE::PdeAddress))in_pde_ptr; auto out_pde = (decltype(PAGE_TABLE::PdeAddress))out_pde_ptr; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 正在拆分大页: 输入PDE=0x%llx, 输出PDE=0x%p\n", in_pde->value, out_pde); PHYSICAL_ADDRESS LowAddr = { 0 }, HighAddr = { 0 }; HighAddr.QuadPart = MAXULONG64; auto pt = (decltype(PAGE_TABLE::PteAddress))MmAllocateContiguousMemorySpecifyCache( PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); if (!pt) { logger("分配连续内存失败 (用于拆分大页)", true); return false; } UINT64 start_pfn = in_pde->flags.page_frame_number; for (int i = 0; i < 512; i++) { pt[i].value = 0; pt[i].flags.present = 1; pt[i].flags.write = in_pde->flags.write; pt[i].flags.user = in_pde->flags.user; pt[i].flags.write_through = in_pde->flags.write_through; pt[i].flags.cache_disable = in_pde->flags.cache_disable; pt[i].flags.accessed = in_pde->flags.accessed; pt[i].flags.dirty = in_pde->flags.dirty; pt[i].flags.global = 0; pt[i].flags.page_frame_number = start_pfn + i; } out_pde->value = in_pde->value; out_pde->flags.large_page = 0; out_pde->flags.page_frame_number = fn_va_to_pa(pt) >> 12; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 大页拆分完成: 新PTE表物理地址=0x%llx\n", fn_va_to_pa(pt)); return true; } bool PteHookManager::fn_isolation_pagetable(UINT64 cr3_val, void* replace_align_addr, void* split_pde_ptr) { PHYSICAL_ADDRESS LowAddr = { 0 }, HighAddr = { 0 }; HighAddr.QuadPart = MAXULONG64; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 开始隔离页表: CR3=0x%llx, 地址=0x%p\n", cr3_val, replace_align_addr); // 分配连续内存用于新页表 auto Va4kb = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); auto VaPt = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); auto VaPdt = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); auto VaPdpt = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); if (!VaPt || !Va4kb || !VaPdt || !VaPdpt) { if (VaPt) MmFreeContiguousMemory(VaPt); if (Va4kb) MmFreeContiguousMemory(Va4kb); if (VaPdt) MmFreeContiguousMemory(VaPdt); if (VaPdpt) MmFreeContiguousMemory(VaPdpt); logger("分配连续内存失败 (用于隔离页表)", true); return false; } // 获取原始页表信息(只保存值) PAGE_TABLE Table = { 0 }; Table.LineAddress = (UINT64)replace_align_addr; NTSTATUS status = get_page_table(cr3_val, Table); if (!NT_SUCCESS(status)) { MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); logger("获取页表信息失败", true, status); return false; } UINT64 pte_index = (Table.LineAddress >> 12) & 0x1FF; UINT64 pde_index = (Table.LineAddress >> 21) & 0x1FF; UINT64 pdpte_index = (Table.LineAddress >> 30) & 0x1FF; UINT64 pml4e_index = (Table.LineAddress >> 39) & 0x1FF; // 复制原始页面内容 memcpy(Va4kb, replace_align_addr, PAGE_SIZE); // 物理地址转虚拟地址的辅助函数(增强版) auto get_va_from_pfn = [this](UINT64 pfn) -> void* { if (pfn == 0) return nullptr; PHYSICAL_ADDRESS physAddr; physAddr.QuadPart = pfn << 12; // 优先使用 MmMapIoSpace PVOID mappedVa = MmMapIoSpace(physAddr, PAGE_SIZE, MmNonCached); if (mappedVa) { return mappedVa; } // 低4GB直接映射 if (physAddr.QuadPart < 0x100000000) { return (PVOID)(physAddr.QuadPart + 0xFFFF000000000000); } // 回退到MDL方式 PMDL pMdl = IoAllocateMdl(nullptr, PAGE_SIZE, FALSE, FALSE, NULL); if (!pMdl) return nullptr; MmBuildMdlForNonPagedPool(pMdl); PVOID va = MmMapLockedPagesSpecifyCache(pMdl, KernelMode, MmNonCached, NULL, FALSE, NormalPagePriority); if (va) { // 添加到MDL缓存 if (m_MdlCount < MAX_MDL_CACHE) { m_MdlCache[m_MdlCount++] = { va, pMdl }; } return va; } IoFreeMdl(pMdl); return nullptr; }; // 安全获取PT源地址 void* pt_source_va = nullptr; if (Table.IsLargePage && split_pde_ptr) { auto split_pde = (decltype(PAGE_TABLE::PdeAddress))split_pde_ptr; pt_source_va = get_va_from_pfn(split_pde->flags.page_frame_number); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 大页模式: 源PFN=0x%llx, 源VA=%p\n", split_pde->flags.page_frame_number, pt_source_va); } else { UINT64 pt_pfn = (Table.OriginalPde & ~0xFFF) >> 12; pt_source_va = get_va_from_pfn(pt_pfn); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 常规模式: 源PFN=0x%llx, 源VA=%p\n", pt_pfn, pt_source_va); } if (!pt_source_va) { logger("无法获取PT源虚拟地址", true); MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); return false; } // 复制PT数据(带异常处理) __try { memcpy(VaPt, pt_source_va, PAGE_SIZE); } __except (EXCEPTION_EXECUTE_HANDLER) { logger("复制PT数据时发生异常", true, GetExceptionCode()); MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); fn_unmap_pa(pt_source_va); return false; } fn_unmap_pa(pt_source_va); // 释放映射 // 安全获取PDT源地址 UINT64 pdt_pfn = (Table.OriginalPdpte & ~0xFFF) >> 12; void* pdt_source_va = get_va_from_pfn(pdt_pfn); if (!pdt_source_va) { logger("无法获取PDT源虚拟地址", true); MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); return false; } __try { memcpy(VaPdt, pdt_source_va, PAGE_SIZE); } __except (EXCEPTION_EXECUTE_HANDLER) { logger("复制PDT数据时发生异常", true, GetExceptionCode()); MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); fn_unmap_pa(pdt_source_va); return false; } fn_unmap_pa(pdt_source_va); // 安全获取PDPT源地址 UINT64 pdpt_pfn = (Table.OriginalPml4e & ~0xFFF) >> 12; void* pdpt_source_va = get_va_from_pfn(pdpt_pfn); if (!pdpt_source_va) { logger("无法获取PDPT源虚拟地址", true); MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); return false; } __try { memcpy(VaPdpt, pdpt_source_va, PAGE_SIZE); } __except (EXCEPTION_EXECUTE_HANDLER) { logger("复制PDPT数据时发生异常", true, GetExceptionCode()); MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); fn_unmap_pa(pdpt_source_va); return false; } fn_unmap_pa(pdpt_source_va); // 设置新PTE auto new_pte = (decltype(PAGE_TABLE::PteAddress))VaPt; new_pte[pte_index].flags.page_frame_number = fn_va_to_pa(Va4kb) >> 12; // 设置新PDE auto new_pde = (decltype(PAGE_TABLE::PdeAddress))VaPdt; new_pde[pde_index].value = Table.OriginalPde; new_pde[pde_index].flags.large_page = 0; new_pde[pde_index].flags.page_frame_number = fn_va_to_pa(VaPt) >> 12; // 设置新PDPTE auto new_pdpte = (decltype(PAGE_TABLE::PdpteAddress))VaPdpt; new_pdpte[pdpte_index].flags.page_frame_number = fn_va_to_pa(VaPdt) >> 12; // 设置新PML4E(安全方式) PHYSICAL_ADDRESS pml4_phys; pml4_phys.QuadPart = cr3_val & ~0xFFF; UINT64* new_pml4 = (UINT64*)fn_pa_to_va(pml4_phys.QuadPart); if (!new_pml4) { logger("无法获取PML4虚拟地址", true); MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); return false; } // 保存原始PML4E值 UINT64 original_pml4e = new_pml4[pml4e_index]; UINT64 new_pml4e = (original_pml4e & 0xFFF) | (fn_va_to_pa(VaPdpt) & ~0xFFF); // 原子更新PML4E KIRQL oldIrql = DisableWriteProtection(); new_pml4[pml4e_index] = new_pml4e; EnableWriteProtection(oldIrql); // 刷新TLB (多核安全) KeIpiGenericCall([](ULONG_PTR Context) -> ULONG_PTR { __invlpg((void*)Context); return 0; }, (ULONG_PTR)replace_align_addr); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 页表隔离完成: 新PFN=0x%llx\n", fn_va_to_pa(Va4kb) >> 12); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] PML4E更新: 0x%llx -> 0x%llx\n", original_pml4e, new_pml4e); return true; } bool PteHookManager::fn_isolation_pages(HANDLE process_id, void* ori_addr) { PEPROCESS Process; NTSTATUS status = PsLookupProcessByProcessId(process_id, &Process); if (!NT_SUCCESS(status)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[ISOL] 查找进程失败: PID=%d\n", HandleToULong(process_id)); return false; } KAPC_STATE ApcState; KeStackAttachProcess(Process, &ApcState); void* AlignAddr = PAGE_ALIGN(ori_addr); PAGE_TABLE Table = { 0 }; Table.LineAddress = (UINT64)AlignAddr; // 获取目标进程CR3 UINT64 target_cr3 = get_process_cr3(Process); if (target_cr3 == 0) { KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); return false; } // 获取页表项 status = get_page_table(target_cr3, Table); if (!NT_SUCCESS(status)) { KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[ISOL] 获取页表失败: VA=0x%p\n", AlignAddr); return false; } // 检查是否已隔离(G位为0) UINT64* pteEntry = nullptr; UINT64 globalFlag = 0; if (Table.PteAddress) { pteEntry = &Table.PteAddress->value; globalFlag = PTE_GLOBAL_FLAG; } else if (Table.PdeAddress && Table.IsLargePage) { pteEntry = &Table.PdeAddress->value; globalFlag = PDE_GLOBAL_FLAG; } else if (Table.PdpteAddress && Table.Is1GBPage) { pteEntry = &Table.PdpteAddress->value; globalFlag = PDPTE_GLOBAL_FLAG; } else { KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[ISOL] 无法确定页表级别\n"); return false; } UINT64 currentValue = *pteEntry; if ((currentValue & globalFlag) == 0) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[ISOL] 页面已隔离: VA=0x%p\n", AlignAddr); KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); return true; } // 清除G位(Global标志) UINT64 newValue = currentValue & ~globalFlag; InterlockedExchange64((LONG64*)pteEntry, newValue); // 刷新TLB __invlpg(AlignAddr); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[ISOL] 页面隔离成功: VA=0x%p, 原始值=0x%llx, 新值=0x%llx\n", AlignAddr, currentValue, newValue); KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); return true; } bool PteHookManager::WriteTrampolineInstruction(void* trampoline, const JMP_ABS& jmpCmd) { if (!MmIsAddressValid(trampoline)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 错误: 内存地址无效 (VA=%p)\n", trampoline); return false; } PHYSICAL_ADDRESS physAddr = MmGetPhysicalAddress(trampoline); if (physAddr.QuadPart == 0) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 错误: 无法获取物理地址 (VA=%p)\n", trampoline); return false; } KIRQL oldIrql = KeRaiseIrqlToDpcLevel(); BOOLEAN wpEnabled = (__readcr0() & CR0_WP); if (wpEnabled) { __writecr0(__readcr0() & ~CR0_WP); _mm_mfence(); } PMDL pMdl = IoAllocateMdl(trampoline, sizeof(JMP_ABS), FALSE, FALSE, NULL); if (!pMdl) { if (wpEnabled) __writecr0(__readcr0() | CR0_WP); KeLowerIrql(oldIrql); return false; } NTSTATUS status = STATUS_SUCCESS; __try { MmBuildMdlForNonPagedPool(pMdl); MmProtectMdlSystemAddress(pMdl, PAGE_READWRITE); // 正确写入 FF25 00000000 和 8字节地址 memcpy(trampoline, jmpCmd.opcode, 6); // FF25 00000000 *(ULONG64*)((BYTE*)trampoline + 6) = jmpCmd.address; // 地址写入 RIP+0 的位置 _mm_sfence(); _mm_clflush(trampoline); _mm_clflush((BYTE*)trampoline + 8); __invlpg(trampoline); _mm_mfence(); } __except (EXCEPTION_EXECUTE_HANDLER) { status = GetExceptionCode(); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 异常: 写入跳板失败 (代码: 0x%X)\n", status); } IoFreeMdl(pMdl); if (wpEnabled) { __writecr0(__readcr0() | CR0_WP); _mm_mfence(); } KeLowerIrql(oldIrql); if (!NT_SUCCESS(status)) return false; // 验证写入结果 if (*(USHORT*)trampoline != 0x25FF || *(ULONG64*)((BYTE*)trampoline + 6) != jmpCmd.address) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 验证失败: 跳板内容不匹配\n" " 预期: FF25 [%p]\n" " 实际: %02X%02X %02X%02X%02X%02X [%p]\n", jmpCmd.address, ((BYTE*)trampoline)[0], ((BYTE*)trampoline)[1], ((BYTE*)trampoline)[2], ((BYTE*)trampoline)[3], ((BYTE*)trampoline)[4], ((BYTE*)trampoline)[5], *(ULONG64*)((BYTE*)trampoline + 6)); return false; } return true; } bool PteHookManager::fn_pte_inline_hook_bp_pg(HANDLE process_id, _Inout_ void** ori_addr, void* hk_addr) { // [1] 页表隔离 if (!fn_isolation_pages(process_id, *ori_addr)) { return false; } // [2] 修改PTE权限为可写 if (!fn_modify_pte_permission(process_id, *ori_addr, true)) { return false; } // [3] 获取目标进程上下文 PEPROCESS targetProcess; if (!NT_SUCCESS(PsLookupProcessByProcessId(process_id, &targetProcess))) { return false; } KAPC_STATE apcState; KeStackAttachProcess(targetProcess, &apcState); // [4] 构造跳转指令 JMP_ABS jmpCmd = {}; memcpy(jmpCmd.opcode, "\xFF\x25\x00\x00\x00\x00", 6); // FF25 00000000 jmpCmd.address = reinterpret_cast(hk_addr); // [5] 直接写入被隔离页 void* targetFunc = *ori_addr; bool success = false; // 禁用写保护 KIRQL oldIrql = DisableWriteProtection(); __try { // 保存原始指令 (用于卸载) RtlCopyMemory(m_HookInfo[m_HookCount].OriginalBytes, targetFunc, sizeof(jmpCmd)); // 写入跳转指令到隔离页 memcpy(targetFunc, &jmpCmd, 6); *(ULONG64*)((BYTE*)targetFunc + 6) = jmpCmd.address; // 刷新缓存 _mm_sfence(); _mm_clflush(targetFunc); __invlpg(targetFunc); _mm_mfence(); success = true; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 直接写入隔离页成功: VA=%p -> Hook=%p\n", targetFunc, hk_addr); } __except (EXCEPTION_EXECUTE_HANDLER) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 写入隔离页异常: 0x%X\n", GetExceptionCode()); } // 恢复写保护 EnableWriteProtection(oldIrql); // [6] 恢复原始权限 fn_modify_pte_permission(process_id, *ori_addr, false); // [7] 记录Hook信息 if (success) { m_HookInfo[m_HookCount].OriginalAddress = targetFunc; m_HookInfo[m_HookCount].HookAddress = hk_addr; m_HookInfo[m_HookCount].ProcessId = process_id; m_HookInfo[m_HookCount].IsHooked = TRUE; m_HookCount++; } // [8] 清理 KeUnstackDetachProcess(&apcState); ObDereferenceObject(targetProcess); return success; } bool PteHookManager::fn_modify_pte_permission(HANDLE process_id, void* addr, bool make_writable) { PEPROCESS Process; NTSTATUS status = PsLookupProcessByProcessId(process_id, &Process); if (!NT_SUCCESS(status)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PERM] 查找进程失败: PID=%d\n", HandleToULong(process_id)); return false; } KAPC_STATE ApcState; KeStackAttachProcess(Process, &ApcState); void* AlignAddr = PAGE_ALIGN(addr); PAGE_TABLE Table = { 0 }; Table.LineAddress = (UINT64)AlignAddr; // 获取目标进程CR3 UINT64 target_cr3 = get_process_cr3(Process); if (target_cr3 == 0) { KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); return false; } // 获取页表项 status = get_page_table(target_cr3, Table); if (!NT_SUCCESS(status) || !Table.PteAddress) { KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PERM] 获取页表失败: VA=0x%p\n", AlignAddr); return false; } UINT64 pteValue = Table.PteAddress->value; UINT64 newPte; UINT64 permMask = PTE_WRITABLE_FLAG | PTE_NOEXECUTE_FLAG; if (make_writable) { // 保存原始权限 m_OriginalPermissions = pteValue & permMask; // 设置可写位并清除NX位 newPte = (pteValue & ~permMask) | PTE_WRITABLE_FLAG; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PERM] 修改权限为可写: VA=0x%p, 原始PTE=0x%llx, 新PTE=0x%llx\n", addr, pteValue, newPte); } else { // 恢复原始权限 newPte = (pteValue & ~permMask) | (m_OriginalPermissions & permMask); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PERM] 恢复原始权限: VA=0x%p, 原始PTE=0x%llx, 新PTE=0x%llx\n", addr, pteValue, newPte); } // 原子修改PTE InterlockedExchange64((LONG64*)&Table.PteAddress->value, newPte); // 刷新TLB __invlpg(addr); KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); return true; } bool PteHookManager::fn_remove_hook(HANDLE process_id, void* hook_addr) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 尝试移除Hook: Hook地址=0x%p\n", hook_addr); for (UINT32 i = 0; i < m_HookCount; i++) { if (m_HookInfo[i].HookAddress == hook_addr && m_HookInfo[i].IsHooked) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 找到匹配的Hook: 原始地址=0x%p\n", m_HookInfo[i].OriginalAddress); KIRQL oldIrql = DisableWriteProtection(); memcpy(m_HookInfo[i].OriginalAddress, m_HookInfo[i].OriginalBytes, sizeof(m_HookInfo[i].OriginalBytes)); EnableWriteProtection(oldIrql); m_HookInfo[i].IsHooked = FALSE; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] Hook已成功移除\n"); return true; } } logger("未找到匹配的Hook", true); return false; } void PteHookManager::fn_add_g_bit_info(void* align_addr, void* pde_address, void* pte_address) { if (m_GbitCount >= MAX_G_BIT_RECORDS) { logger("达到最大G位记录数量限制", true); return; } PG_BIT_INFO record = &m_GbitRecords[m_GbitCount++]; record->AlignAddress = align_addr; record->PdeAddress = (decltype(G_BIT_INFO::PdeAddress))pde_address; record->PteAddress = (decltype(G_BIT_INFO::PteAddress))pte_address; record->IsLargePage = (pde_address && ((decltype(PAGE_TABLE::PdeAddress))pde_address)->flags.large_page); // 打印G位信息 PrintGBitInfo(*record); } bool PteHookManager::fn_resume_global_bits(void* align_addr) { KIRQL oldIrql = DisableWriteProtection(); bool found = false; DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 开始恢复G位: 对齐地址=0x%p\n", align_addr); for (UINT32 i = 0; i < m_GbitCount; i++) { PG_BIT_INFO record = &m_GbitRecords[i]; if (align_addr && record->AlignAddress != align_addr) continue; if (record->PteAddress) { record->PteAddress->flags.global = 1; __invlpg(record->AlignAddress); DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, " 恢复PTE G位: PTE=0x%llx, 地址=0x%p\n", record->PteAddress->value, record->AlignAddress); } if (record->PdeAddress) { record->PdeAddress->flags.global = 1; if (record->IsLargePage) { __invlpg(record->AlignAddress); } DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, " 恢复PDE G位: PDE=0x%llx, 地址=0x%p, 大页=%d\n", record->PdeAddress->value, record->AlignAddress, record->IsLargePage); } found = true; if (align_addr) break; } EnableWriteProtection(oldIrql); if (found) { DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, "[PTE_HOOK] G位恢复完成\n"); } else { logger("未找到匹配的G位记录", true); } return found; } PteHookManager* PteHookManager::GetInstance() { if (!m_Instance) { m_Instance = static_cast( ExAllocatePoolWithTag(NonPagedPool, sizeof(PteHookManager), 'tpHk')); if (m_Instance) { RtlZeroMemory(m_Instance, sizeof(PteHookManager)); DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, "[PTE_HOOK] PTE Hook管理器实例已创建: 地址=0x%p\n", m_Instance); } else { DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 创建PTE Hook管理器实例失败\n"); } } return m_Instance; } // 全局PTE Hook管理器实例 PteHookManager* g_PteHookManager = nullptr; // 辅助函数:检查是否为目标进程 BOOLEAN IsTargetProcess(CHAR* imageName) { CHAR currentName[16]; // 复制到本地缓冲区并确保 NULL 终止 RtlCopyMemory(currentName, imageName, 16); currentName[15] = '\0'; // 确保终止 // 修剪尾部空格 for (int i = 15; i >= 0; i--) { if (currentName[i] == ' ') currentName[i] = '\0'; else if (currentName[i] != '\0') break; } return (strcmp(currentName, target_process_name) == 0); } // Hook 函数 NTSTATUS MyObReferenceObjectByHandleWithTag( HANDLE Handle, ACCESS_MASK DesiredAccess, POBJECT_TYPE ObjectType, KPROCESSOR_MODE AccessMode, ULONG Tag, PVOID* Object, POBJECT_HANDLE_INFORMATION HandleInformation ) { PEPROCESS currentProcess = PsGetCurrentProcess(); CHAR* imageName = PsGetProcessImageFileName(currentProcess); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[!] [HookFunction] 进入 Hook 函数! 当前进程: %s\n", imageName); __debugbreak(); // 强制中断,确认是否执行到这里 if (IsTargetProcess(imageName)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[!] [HookFunction] 拒绝访问目标进程 PID=%d\n", HandleToULong(PsGetCurrentProcessId())); return STATUS_ACCESS_DENIED; } return g_OriginalObReferenceObjectByHandleWithTag( Handle, DesiredAccess, ObjectType, AccessMode, Tag, Object, HandleInformation ); } NTSTATUS InstallHook() { UNICODE_STRING funcName; RtlInitUnicodeString(&funcName, L"ObReferenceObjectByHandleWithTag"); g_OriginalObReferenceObjectByHandleWithTag = (fn_ObReferenceObjectByHandleWithTag)MmGetSystemRoutineAddress(&funcName); if (!g_OriginalObReferenceObjectByHandleWithTag) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [InstallHook] 找不到 ObReferenceObjectByHandleWithTag\n"); return STATUS_NOT_FOUND; } __debugbreak(); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [InstallHook] 找到目标函数地址: %p\n", g_OriginalObReferenceObjectByHandleWithTag); void* targetFunc = (void*)g_OriginalObReferenceObjectByHandleWithTag; void* hookFunc = (void*)MyObReferenceObjectByHandleWithTag; if (!g_PteHookManager->fn_pte_inline_hook_bp_pg(targetProcessId, &targetFunc, hookFunc)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [InstallHook] PTE Hook 安装失败\n"); return STATUS_UNSUCCESSFUL; } g_OriginalObReferenceObjectByHandleWithTag = (fn_ObReferenceObjectByHandleWithTag)targetFunc; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [InstallHook] Hook 成功安装. 跳板地址: %p\n", targetFunc); __debugbreak(); // 强制中断,验证是否执行到这里 return STATUS_SUCCESS; } // 移除 Hook VOID RemoveHook() { if (g_OriginalObReferenceObjectByHandleWithTag && g_PteHookManager) { g_PteHookManager->fn_remove_hook(PsGetCurrentProcessId(), (void*)MyObReferenceObjectByHandleWithTag); } } // 工作线程函数 VOID InstallHookWorker(PVOID Context) { targetProcessId = (HANDLE)Context; DbgPrint("[+] Worker thread started for hook installation on PID: %d\n", HandleToULong(targetProcessId)); InstallHook(); PsTerminateSystemThread(STATUS_SUCCESS); } // 进程创建回调 VOID ProcessNotifyCallback( _In_ HANDLE ParentId, _In_ HANDLE ProcessId, _In_ BOOLEAN Create ) { UNREFERENCED_PARAMETER(ParentId); if (Create) { PEPROCESS process = NULL; if (NT_SUCCESS(PsLookupProcessByProcessId(ProcessId, &process))) { CHAR* imageName = PsGetProcessImageFileName(process); CHAR currentName[16]; RtlCopyMemory(currentName, imageName, 16); currentName[15] = '\0'; for (int i = 15; i >= 0; i--) { if (currentName[i] == ' ') currentName[i] = '\0'; else if (currentName[i] != '\0') break; } if (strcmp(currentName, target_process_name) == 0) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [ProcessNotifyCallback] 目标进程 %s 创建 (PID: %d)\n", currentName, HandleToULong(ProcessId)); HANDLE threadHandle; NTSTATUS status = PsCreateSystemThread( &threadHandle, THREAD_ALL_ACCESS, NULL, NULL, NULL, InstallHookWorker, (PVOID)ProcessId // 关键:传递目标进程ID ); if (NT_SUCCESS(status)) { ZwClose(threadHandle); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [ProcessNotifyCallback] 工作线程已创建\n"); } else { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [ProcessNotifyCallback] 创建线程失败: 0x%X\n", status); } } ObDereferenceObject(process); } } } // 驱动卸载函数 VOID DriverUnload(PDRIVER_OBJECT DriverObject) { UNREFERENCED_PARAMETER(DriverObject); DbgPrint("[+] Driver unloading...\n"); // 移除进程通知回调 PsSetCreateProcessNotifyRoutineEx((PCREATE_PROCESS_NOTIFY_ROUTINE_EX)ProcessNotifyCallback, TRUE); // 移除Hook RemoveHook(); // 清理PTE Hook资源 if (g_PteHookManager) { DbgPrint("[PTE_HOOK] Cleaning up PTE...\n"); // 恢复所有被修改的G位 g_PteHookManager->fn_resume_global_bits(nullptr); // 移除所有活动的Hook HOOK_INFO* hookInfo = g_PteHookManager->GetHookInfo(); UINT32 hookCount = g_PteHookManager->GetHookCount(); for (UINT32 i = 0; i < hookCount; i++) { if (hookInfo[i].IsHooked) { g_PteHookManager->fn_remove_hook(PsGetCurrentProcessId(), hookInfo[i].HookAddress); } } // 释放跳板池内存 char* trampLinePool = g_PteHookManager->GetTrampLinePool(); if (trampLinePool) { ExFreePoolWithTag(trampLinePool, 'JmpP'); } // 释放管理器实例 ExFreePoolWithTag(g_PteHookManager, 'tpHk'); g_PteHookManager = nullptr; } DbgPrint("[+] Driver unloaded successfully\n"); } extern "C" NTSTATUS DriverEntry(PDRIVER_OBJECT DriverObject, PUNICODE_STRING RegistryPath) { UNREFERENCED_PARAMETER(RegistryPath); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [DriverEntry] 驱动加载开始\n"); DriverObject->DriverUnload = DriverUnload; g_PteHookManager = PteHookManager::GetInstance(); if (!g_PteHookManager) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [DriverEntry] 初始化 PteHookManager 失败\n"); return STATUS_INSUFFICIENT_RESOURCES; } NTSTATUS status = PsSetCreateProcessNotifyRoutineEx((PCREATE_PROCESS_NOTIFY_ROUTINE_EX)ProcessNotifyCallback, FALSE); if (!NT_SUCCESS(status)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [DriverEntry] 注册进程通知失败 (0x%X)\n", status); return status; } DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [DriverEntry] 驱动加载成功\n"); return STATUS_SUCCESS; } 严重性 代码 说明 项目 文件 行 禁止显示状态 详细信息 错误(活动) E0392 不能在成员函数 "PteHookManager::get_process_cr3" 的类外部重新声明该函数 obpcallback C:\Users\17116\source\repos\obpcallback\obpcallback\源.cpp 326 错误(活动) E0065 应输入“;” obpcallback C:\Users\17116\source\repos\obpcallback\obpcallback\源.cpp 326 错误(活动) E0020 未定义标识符 "MmIsPhysicalAddressValid" obpcallback C:\Users\17116\source\repos\obpcallback\obpcallback\源.cpp 458 错误(活动) E0020 未定义标识符 "physAddr" obpcallback C:\Users\17116\source\repos\obpcallback\obpcallback\源.cpp 337 修改错误

#include <ntifs.h> #include <ntddk.h> #include <intrin.h> #include "ptehook.h" #define CR0_WP (1 << 16) HANDLE targetProcessId = NULL; typedef INT(*LDE_DISASM)(PVOID address, INT bits); typedef unsigned long DWORD; typedef unsigned __int64 ULONG64; // 使用WDK标准类型 typedef unsigned char BYTE; typedef LONG NTSTATUS; // 修正后的跳转指令结构 #pragma pack(push, 1) typedef struct _JMP_ABS { BYTE opcode[6]; // FF 25 00 00 00 00 ULONG64 address; // 8字节绝对地址 } JMP_ABS, * PJMP_ABS; #pragma pack(pop) LDE_DISASM lde_disasm; // 初始化引擎 VOID lde_init() { lde_disasm = (LDE_DISASM)ExAllocatePool(NonPagedPool, 12800); memcpy(lde_disasm, szShellCode, 12800); } // 得到完整指令长度,避免截断 ULONG GetFullPatchSize(PUCHAR Address) { ULONG LenCount = 0, Len = 0; // 至少需要14字节 while (LenCount <= 14) { Len = lde_disasm(Address, 64); Address = Address + Len; LenCount = LenCount + Len; } return LenCount; } #define PROCESS_NAME_LENGTH 16 #define DRIVER_TAG 'HKOB' EXTERN_C char* PsGetProcessImageFileName(PEPROCESS process); char target_process_name[] = "oxygen.exe"; typedef NTSTATUS(*fn_ObReferenceObjectByHandleWithTag)( HANDLE Handle, ACCESS_MASK DesiredAccess, POBJECT_TYPE ObjectType, KPROCESSOR_MODE AccessMode, ULONG Tag, PVOID* Object, POBJECT_HANDLE_INFORMATION HandleInformation ); fn_ObReferenceObjectByHandleWithTag g_OriginalObReferenceObjectByHandleWithTag = NULL; // PTE Hook Framework #define MAX_G_BIT_RECORDS 128 #define MAX_HOOK_COUNT 64 #define PAGE_ALIGN(va) ((PVOID)((ULONG_PTR)(va) & ~0xFFF)) #define PDPTE_PS_BIT (1 << 7) #define PDE_PS_BIT (1 << 7) #define PTE_NX_BIT (1ULL << 63) #define CACHE_WB (6ULL << 3) // 页表结构定义 typedef struct _PAGE_TABLE { UINT64 LineAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 pat : 1; UINT64 global : 1; UINT64 ignored_1 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_2 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PteAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 large_page : 1; UINT64 global : 1; UINT64 ignored_2 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PdeAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 ignored_1 : 1; UINT64 page_size : 1; UINT64 ignored_2 : 4; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PdpteAddress; UINT64* Pml4Address; BOOLEAN IsLargePage; BOOLEAN Is1GBPage; UINT64 OriginalPte; UINT64 OriginalPde; UINT64 OriginalPdpte; UINT64 OriginalPml4e; HANDLE ProcessId; } PAGE_TABLE, * PPAGE_TABLE; // G位信息记录结构体 typedef struct _G_BIT_INFO { void* AlignAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 large_page : 1; UINT64 global : 1; UINT64 ignored_2 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PdeAddress; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 pat : 1; UINT64 global : 1; UINT64 ignored_1 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_2 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*PteAddress; BOOLEAN IsLargePage; } G_BIT_INFO, * PG_BIT_INFO; typedef struct _HOOK_INFO { void* OriginalAddress; void* HookAddress; UINT8 OriginalBytes[20]; UINT8 HookBytes[20]; UINT32 HookLength; BOOLEAN IsHooked; HANDLE ProcessId; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 pat : 1; UINT64 global : 1; UINT64 ignored_1 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_2 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*HookedPte; union { struct { UINT64 present : 1; UINT64 write : 1; UINT64 user : 1; UINT64 write_through : 1; UINT64 cache_disable : 1; UINT64 accessed : 1; UINT64 dirty : 1; UINT64 large_page : 1; UINT64 global : 1; UINT64 ignored_2 : 3; UINT64 page_frame_number : 36; UINT64 reserved_1 : 4; UINT64 ignored_3 : 7; UINT64 protection_key : 4; UINT64 execute_disable : 1; } flags; UINT64 value; }*HookedPde; } HOOK_INFO; class PteHookManager { public: bool fn_pte_inline_hook_bp_pg(HANDLE process_id, _Inout_ void** ori_addr, void* hk_addr); bool fn_remove_hook(HANDLE process_id, void* hook_addr); static PteHookManager* GetInstance(); HOOK_INFO* GetHookInfo() { return m_HookInfo; } char* GetTrampLinePool() { return m_TrampLinePool; } UINT32 GetHookCount() { return m_HookCount; } bool fn_resume_global_bits(void* align_addr); ~PteHookManager(); // 添加析构函数声明 private: bool WriteTrampolineInstruction(void* trampoline, const JMP_ABS& jmpCmd); void fn_add_g_bit_info(void* align_addr, void* pde_address, void* pte_address); bool fn_isolation_pagetable(UINT64 cr3_val, void* replace_align_addr, void* split_pde); bool fn_isolation_pages(HANDLE process_id, void* ori_addr); bool fn_split_large_pages(void* in_pde, void* out_pde); NTSTATUS get_page_table(UINT64 cr3, PAGE_TABLE& table); void* fn_pa_to_va(UINT64 pa); UINT64 fn_va_to_pa(void* va); __forceinline KIRQL DisableWriteProtection(); __forceinline void EnableWriteProtection(KIRQL oldIrql); void logger(const char* info, bool is_err, LONG err_code = 0); void PrintPageTableInfo(const PAGE_TABLE& table); void PrintHookInfo(const HOOK_INFO& hookInfo); void PrintGBitInfo(const G_BIT_INFO& gbitInfo); static constexpr SIZE_T MAX_HOOKS = 256; // 根据需求调整 G_BIT_INFO m_GbitRecords[MAX_G_BIT_RECORDS]; UINT32 m_GbitCount = 0; void* m_PteBase = 0; HOOK_INFO m_HookInfo[MAX_HOOK_COUNT] = { 0 }; DWORD m_HookCount = 0; char* m_TrampLinePool = nullptr; // 合并为一个声明 UINT32 m_PoolUsed = 0; static PteHookManager* m_Instance; }; PteHookManager* PteHookManager::m_Instance = nullptr; // 实现部分 __forceinline KIRQL PteHookManager::DisableWriteProtection() { KIRQL oldIrql = KeRaiseIrqlToDpcLevel(); UINT64 cr0 = __readcr0(); __writecr0(cr0 & ~0x10000); // 清除CR0.WP位 _mm_mfence(); return oldIrql; } __forceinline void PteHookManager::EnableWriteProtection(KIRQL oldIrql) { _mm_mfence(); UINT64 cr0 = __readcr0(); __writecr0(cr0 | 0x10000); // 设置CR0.WP位 KeLowerIrql(oldIrql); } void PteHookManager::logger(const char* info, bool is_err, LONG err_code) { if (is_err) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] ERROR: %s (0x%X)\n", info, err_code); } else { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] INFO: %s\n", info); } } void PteHookManager::PrintPageTableInfo(const PAGE_TABLE& table) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] Page Table Info for VA: 0x%p\n", (void*)table.LineAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PML4E: 0x%llx (Address: 0x%p)\n", table.OriginalPml4e, table.Pml4Address); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PDPTE: 0x%llx (Address: 0x%p), Is1GBPage: %d\n", table.OriginalPdpte, table.PdpteAddress, table.Is1GBPage); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PDE: 0x%llx (Address: 0x%p), IsLargePage: %d\n", table.OriginalPde, table.PdeAddress, table.IsLargePage); if (!table.IsLargePage && !table.Is1GBPage) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PTE: 0x%llx (Address: 0x%p)\n", table.OriginalPte, table.PteAddress); } } void PteHookManager::PrintHookInfo(const HOOK_INFO& hookInfo) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] Hook Info:\n"); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Original Address: 0x%p\n", hookInfo.OriginalAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Hook Address: 0x%p\n", hookInfo.HookAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Hook Length: %d\n", hookInfo.HookLength); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Is Hooked: %d\n", hookInfo.IsHooked); // 打印原始字节 DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Original Bytes: "); for (UINT32 i = 0; i < sizeof(hookInfo.OriginalBytes); i++) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "%02X ", hookInfo.OriginalBytes[i]); } DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "\n"); // 打印Hook字节 DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Hook Bytes: "); for (UINT32 i = 0; i < sizeof(hookInfo.HookBytes); i++) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "%02X ", hookInfo.HookBytes[i]); } DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "\n"); } void PteHookManager::PrintGBitInfo(const G_BIT_INFO& gbitInfo) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] G-Bit Info:\n"); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " Align Address: 0x%p\n", gbitInfo.AlignAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " IsLargePage: %d\n", gbitInfo.IsLargePage); if (gbitInfo.PdeAddress) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PDE: 0x%llx (Address: 0x%p)\n", gbitInfo.PdeAddress->value, gbitInfo.PdeAddress); } if (gbitInfo.PteAddress) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, " PTE: 0x%llx (Address: 0x%p)\n", gbitInfo.PteAddress->value, gbitInfo.PteAddress); } } void* PteHookManager::fn_pa_to_va(UINT64 pa) { PHYSICAL_ADDRESS physAddr; physAddr.QuadPart = pa; return MmGetVirtualForPhysical(physAddr); } UINT64 PteHookManager::fn_va_to_pa(void* va) { PHYSICAL_ADDRESS physAddr = MmGetPhysicalAddress(va); return physAddr.QuadPart; } NTSTATUS PteHookManager::get_page_table(UINT64 cr3_val, PAGE_TABLE& table) { UINT64 va = table.LineAddress; UINT64 pml4e_index = (va >> 39) & 0x1FF; UINT64 pdpte_index = (va >> 30) & 0x1FF; UINT64 pde_index = (va >> 21) & 0x1FF; UINT64 pte_index = (va >> 12) & 0x1FF; // PML4 UINT64 pml4_pa = cr3_val & ~0xFFF; UINT64* pml4_va = (UINT64*)fn_pa_to_va(pml4_pa); if (!pml4_va) return STATUS_INVALID_ADDRESS; table.Pml4Address = &pml4_va[pml4e_index]; table.OriginalPml4e = *table.Pml4Address; if (!(table.OriginalPml4e & 1)) return STATUS_ACCESS_VIOLATION; // PDPTE UINT64 pdpte_pa = table.OriginalPml4e & ~0xFFF; UINT64* pdpte_va = (UINT64*)fn_pa_to_va(pdpte_pa); if (!pdpte_va) return STATUS_INVALID_ADDRESS; table.PdpteAddress = (decltype(table.PdpteAddress))&pdpte_va[pdpte_index]; table.OriginalPdpte = table.PdpteAddress->value; table.Is1GBPage = (table.PdpteAddress->flags.page_size) ? TRUE : FALSE; if (!(table.OriginalPdpte & 1)) return STATUS_ACCESS_VIOLATION; if (table.Is1GBPage) return STATUS_SUCCESS; // PDE UINT64 pde_pa = table.OriginalPdpte & ~0xFFF; UINT64* pde_va = (UINT64*)fn_pa_to_va(pde_pa); if (!pde_va) return STATUS_INVALID_ADDRESS; table.PdeAddress = (decltype(table.PdeAddress))&pde_va[pde_index]; table.OriginalPde = table.PdeAddress->value; table.IsLargePage = (table.PdeAddress->flags.large_page) ? TRUE : FALSE; if (!(table.OriginalPde & 1)) return STATUS_ACCESS_VIOLATION; if (table.IsLargePage) return STATUS_SUCCESS; // PTE UINT64 pte_pa = table.OriginalPde & ~0xFFF; UINT64* pte_va = (UINT64*)fn_pa_to_va(pte_pa); if (!pte_va) return STATUS_INVALID_ADDRESS; table.PteAddress = (decltype(table.PteAddress))&pte_va[pte_index]; table.OriginalPte = table.PteAddress->value; if (!(table.OriginalPte & 1)) return STATUS_ACCESS_VIOLATION; // 打印页表信息 PrintPageTableInfo(table); return STATUS_SUCCESS; } bool PteHookManager::fn_split_large_pages(void* in_pde_ptr, void* out_pde_ptr) { auto in_pde = (decltype(PAGE_TABLE::PdeAddress))in_pde_ptr; auto out_pde = (decltype(PAGE_TABLE::PdeAddress))out_pde_ptr; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 正在拆分大页: 输入PDE=0x%llx, 输出PDE=0x%p\n", in_pde->value, out_pde); PHYSICAL_ADDRESS LowAddr = { 0 }, HighAddr = { 0 }; HighAddr.QuadPart = MAXULONG64; auto pt = (decltype(PAGE_TABLE::PteAddress))MmAllocateContiguousMemorySpecifyCache( PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); if (!pt) { logger("分配连续内存失败 (用于拆分大页)", true); return false; } UINT64 start_pfn = in_pde->flags.page_frame_number; for (int i = 0; i < 512; i++) { pt[i].value = 0; pt[i].flags.present = 1; pt[i].flags.write = in_pde->flags.write; pt[i].flags.user = in_pde->flags.user; pt[i].flags.write_through = in_pde->flags.write_through; pt[i].flags.cache_disable = in_pde->flags.cache_disable; pt[i].flags.accessed = in_pde->flags.accessed; pt[i].flags.dirty = in_pde->flags.dirty; pt[i].flags.global = 0; pt[i].flags.page_frame_number = start_pfn + i; } out_pde->value = in_pde->value; out_pde->flags.large_page = 0; out_pde->flags.page_frame_number = fn_va_to_pa(pt) >> 12; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 大页拆分完成: 新PTE表物理地址=0x%llx\n", fn_va_to_pa(pt)); return true; } bool PteHookManager::fn_isolation_pagetable(UINT64 cr3_val, void* replace_align_addr, void* split_pde_ptr) { PHYSICAL_ADDRESS LowAddr = { 0 }, HighAddr = { 0 }; HighAddr.QuadPart = MAXULONG64; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 开始隔离页表: CR3=0x%llx, 地址=0x%p\n", cr3_val, replace_align_addr); auto Va4kb = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); auto VaPt = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); auto VaPdt = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); auto VaPdpt = (UINT64*)MmAllocateContiguousMemorySpecifyCache(PAGE_SIZE, LowAddr, HighAddr, LowAddr, MmNonCached); if (!VaPt || !Va4kb || !VaPdt || !VaPdpt) { if (VaPt) MmFreeContiguousMemory(VaPt); if (Va4kb) MmFreeContiguousMemory(Va4kb); if (VaPdt) MmFreeContiguousMemory(VaPdt); if (VaPdpt) MmFreeContiguousMemory(VaPdpt); logger("分配连续内存失败 (用于隔离页表)", true); return false; } PAGE_TABLE Table = { 0 }; Table.LineAddress = (UINT64)replace_align_addr; NTSTATUS status = get_page_table(cr3_val, Table); if (!NT_SUCCESS(status)) { MmFreeContiguousMemory(VaPt); MmFreeContiguousMemory(Va4kb); MmFreeContiguousMemory(VaPdt); MmFreeContiguousMemory(VaPdpt); logger("获取页表信息失败", true, status); return false; } UINT64 pte_index = (Table.LineAddress >> 12) & 0x1FF; UINT64 pde_index = (Table.LineAddress >> 21) & 0x1FF; UINT64 pdpte_index = (Table.LineAddress >> 30) & 0x1FF; UINT64 pml4e_index = (Table.LineAddress >> 39) & 0x1FF; memcpy(Va4kb, replace_align_addr, PAGE_SIZE); if (Table.IsLargePage && split_pde_ptr) { auto split_pde = (decltype(PAGE_TABLE::PdeAddress))split_pde_ptr; memcpy(VaPt, (void*)(split_pde->flags.page_frame_number << 12), PAGE_SIZE); } else { memcpy(VaPt, (void*)(Table.PdeAddress->flags.page_frame_number << 12), PAGE_SIZE); } memcpy(VaPdt, (void*)(Table.PdpteAddress->flags.page_frame_number << 12), PAGE_SIZE); memcpy(VaPdpt, (void*)(Table.Pml4Address[pml4e_index] & ~0xFFF), PAGE_SIZE); auto new_pte = (decltype(PAGE_TABLE::PteAddress))VaPt; new_pte[pte_index].flags.page_frame_number = fn_va_to_pa(Va4kb) >> 12; auto new_pde = (decltype(PAGE_TABLE::PdeAddress))VaPdt; new_pde[pde_index].value = Table.OriginalPde; new_pde[pde_index].flags.large_page = 0; new_pde[pde_index].flags.page_frame_number = fn_va_to_pa(VaPt) >> 12; auto new_pdpte = (decltype(PAGE_TABLE::PdpteAddress))VaPdpt; new_pdpte[pdpte_index].flags.page_frame_number = fn_va_to_pa(VaPdt) >> 12; auto new_pml4 = (UINT64*)fn_pa_to_va(cr3_val & ~0xFFF); new_pml4[pml4e_index] = (new_pml4[pml4e_index] & 0xFFF) | (fn_va_to_pa(VaPdpt) & ~0xFFF); __invlpg(replace_align_addr); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 页表隔离完成: 新PFN=0x%llx\n", fn_va_to_pa(Va4kb) >> 12); return true; } bool PteHookManager::fn_isolation_pages(HANDLE process_id, void* ori_addr) { PEPROCESS Process; if (!NT_SUCCESS(PsLookupProcessByProcessId(process_id, &Process))) { logger("查找进程失败", true); return false; } DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 开始隔离页面: PID=%d, 地址=0x%p\n", (ULONG)(ULONG_PTR)process_id, ori_addr); KAPC_STATE ApcState; KeStackAttachProcess(Process, &ApcState); void* AlignAddr = PAGE_ALIGN(ori_addr); PAGE_TABLE Table = { 0 }; Table.LineAddress = (UINT64)AlignAddr; UINT64 target_cr3 = *(UINT64*)((UCHAR*)Process + 0x28); if (!NT_SUCCESS(get_page_table(target_cr3, Table))) { KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); logger("获取目标进程页表失败", true); return false; } bool success = false; decltype(PAGE_TABLE::PdeAddress) split_pde = nullptr; if (Table.IsLargePage) { split_pde = (decltype(PAGE_TABLE::PdeAddress))ExAllocatePoolWithTag(NonPagedPool, sizeof(*split_pde), 'pdeS'); if (!split_pde || !fn_split_large_pages(Table.PdeAddress, split_pde)) { if (split_pde) ExFreePoolWithTag(split_pde, 'pdeS'); KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); logger("拆分大页失败", true); return false; } if (Table.PdeAddress->flags.global) { Table.PdeAddress->flags.global = 0; fn_add_g_bit_info(AlignAddr, Table.PdeAddress, nullptr); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 清除大页G位: PDE=0x%llx\n", Table.PdeAddress->value); } } else if (Table.PteAddress && Table.PteAddress->flags.global) { Table.PteAddress->flags.global = 0; fn_add_g_bit_info(AlignAddr, nullptr, Table.PteAddress); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 清除PTE G位: PTE=0x%llx\n", Table.PteAddress->value); success = fn_isolation_pagetable(target_cr3, AlignAddr, split_pde); if (split_pde) ExFreePoolWithTag(split_pde, 'pdeS'); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 页表状态: IsLargePage=%d, Is1GBPage=%d\n", Table.IsLargePage, Table.Is1GBPage); if (Table.PteAddress) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] PTE 值: 0x%llx (G位=%d)\n", Table.PteAddress->value, Table.PteAddress->flags.global); } KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); if (success) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 页面隔离成功\n"); } else { logger("页面隔离失败", true); } return success; } KeUnstackDetachProcess(&ApcState); ObDereferenceObject(Process); return true; } bool PteHookManager::WriteTrampolineInstruction(void* trampoline, const JMP_ABS& jmpCmd) { if (!MmIsAddressValid(trampoline)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 错误: 内存地址无效 (VA=%p)\n", trampoline); return false; } PHYSICAL_ADDRESS physAddr = MmGetPhysicalAddress(trampoline); if (physAddr.QuadPart == 0) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 错误: 无法获取物理地址 (VA=%p)\n", trampoline); return false; } KIRQL oldIrql = KeRaiseIrqlToDpcLevel(); BOOLEAN wpEnabled = (__readcr0() & CR0_WP); if (wpEnabled) { __writecr0(__readcr0() & ~CR0_WP); _mm_mfence(); } PMDL pMdl = IoAllocateMdl(trampoline, sizeof(JMP_ABS), FALSE, FALSE, NULL); if (!pMdl) { if (wpEnabled) __writecr0(__readcr0() | CR0_WP); KeLowerIrql(oldIrql); return false; } NTSTATUS status = STATUS_SUCCESS; __try { MmBuildMdlForNonPagedPool(pMdl); MmProtectMdlSystemAddress(pMdl, PAGE_READWRITE); // 正确写入 FF25 00000000 和 8字节地址 memcpy(trampoline, jmpCmd.opcode, 6); // FF25 00000000 *(ULONG64*)((BYTE*)trampoline + 6) = jmpCmd.address; // 地址写入 RIP+0 的位置 _mm_sfence(); _mm_clflush(trampoline); _mm_clflush((BYTE*)trampoline + 8); __invlpg(trampoline); _mm_mfence(); } __except (EXCEPTION_EXECUTE_HANDLER) { status = GetExceptionCode(); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 异常: 写入跳板失败 (代码: 0x%X)\n", status); } IoFreeMdl(pMdl); if (wpEnabled) { __writecr0(__readcr0() | CR0_WP); _mm_mfence(); } KeLowerIrql(oldIrql); if (!NT_SUCCESS(status)) return false; // 验证写入结果 if (*(USHORT*)trampoline != 0x25FF || *(ULONG64*)((BYTE*)trampoline + 6) != jmpCmd.address) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 验证失败: 跳板内容不匹配\n" " 预期: FF25 [%p]\n" " 实际: %02X%02X %02X%02X%02X%02X [%p]\n", jmpCmd.address, ((BYTE*)trampoline)[0], ((BYTE*)trampoline)[1], ((BYTE*)trampoline)[2], ((BYTE*)trampoline)[3], ((BYTE*)trampoline)[4], ((BYTE*)trampoline)[5], *(ULONG64*)((BYTE*)trampoline + 6)); return false; } return true; } bool PteHookManager::fn_pte_inline_hook_bp_pg(HANDLE process_id, _Inout_ void** ori_addr, void* hk_addr) { // [1] 页表隔离 if (!fn_isolation_pages(process_id, *ori_addr)) { return false; } // [2] 获取目标进程上下文 PEPROCESS targetProcess; if (!NT_SUCCESS(PsLookupProcessByProcessId(process_id, &targetProcess))) { return false; } KAPC_STATE apcState; KeStackAttachProcess(targetProcess, &apcState); // [3] 构造跳转指令 JMP_ABS jmpCmd = {}; memcpy(jmpCmd.opcode, "\xFF\x25\x00\x00\x00\x00", 6); // FF25 00000000 jmpCmd.address = reinterpret_cast(hk_addr); // [4] 直接写入被隔离页 void* targetFunc = *ori_addr; bool success = false; // 禁用写保护 KIRQL oldIrql = DisableWriteProtection(); __try { // 保存原始指令 (用于卸载) RtlCopyMemory(m_HookInfo[m_HookCount].OriginalBytes, targetFunc, sizeof(jmpCmd)); // 写入跳转指令到隔离页 memcpy(targetFunc, &jmpCmd, 6); *(ULONG64*)((BYTE*)targetFunc + 6) = jmpCmd.address; // 刷新缓存 _mm_sfence(); _mm_clflush(targetFunc); __invlpg(targetFunc); _mm_mfence(); success = true; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 直接写入隔离页成功: VA=%p -> Hook=%p\n", targetFunc, hk_addr); } __except (EXCEPTION_EXECUTE_HANDLER) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 写入隔离页异常: 0x%X\n", GetExceptionCode()); } // 恢复写保护 EnableWriteProtection(oldIrql); // [5] 记录Hook信息 if (success) { m_HookInfo[m_HookCount].OriginalAddress = targetFunc; m_HookInfo[m_HookCount].HookAddress = hk_addr; m_HookInfo[m_HookCount].ProcessId = process_id; m_HookInfo[m_HookCount].IsHooked = TRUE; m_HookCount++; } // [6] 清理 KeUnstackDetachProcess(&apcState); ObDereferenceObject(targetProcess); return success; } // 析构函数清理资源 PteHookManager::~PteHookManager() { if (m_TrampLinePool) { MmFreeContiguousMemory(m_TrampLinePool); m_TrampLinePool = nullptr; } } bool PteHookManager::fn_remove_hook(HANDLE process_id, void* hook_addr) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 尝试移除Hook: Hook地址=0x%p\n", hook_addr); for (UINT32 i = 0; i < m_HookCount; i++) { if (m_HookInfo[i].HookAddress == hook_addr && m_HookInfo[i].IsHooked) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 找到匹配的Hook: 原始地址=0x%p\n", m_HookInfo[i].OriginalAddress); KIRQL oldIrql = DisableWriteProtection(); memcpy(m_HookInfo[i].OriginalAddress, m_HookInfo[i].OriginalBytes, sizeof(m_HookInfo[i].OriginalBytes)); EnableWriteProtection(oldIrql); m_HookInfo[i].IsHooked = FALSE; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[PTE_HOOK] Hook已成功移除\n"); return true; } } logger("未找到匹配的Hook", true); return false; } void PteHookManager::fn_add_g_bit_info(void* align_addr, void* pde_address, void* pte_address) { if (m_GbitCount >= MAX_G_BIT_RECORDS) { logger("达到最大G位记录数量限制", true); return; } PG_BIT_INFO record = &m_GbitRecords[m_GbitCount++]; record->AlignAddress = align_addr; record->PdeAddress = (decltype(G_BIT_INFO::PdeAddress))pde_address; record->PteAddress = (decltype(G_BIT_INFO::PteAddress))pte_address; record->IsLargePage = (pde_address && ((decltype(PAGE_TABLE::PdeAddress))pde_address)->flags.large_page); // 打印G位信息 PrintGBitInfo(*record); } bool PteHookManager::fn_resume_global_bits(void* align_addr) { KIRQL oldIrql = DisableWriteProtection(); bool found = false; DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, "[PTE_HOOK] 开始恢复G位: 对齐地址=0x%p\n", align_addr); for (UINT32 i = 0; i < m_GbitCount; i++) { PG_BIT_INFO record = &m_GbitRecords[i]; if (align_addr && record->AlignAddress != align_addr) continue; if (record->PteAddress) { record->PteAddress->flags.global = 1; __invlpg(record->AlignAddress); DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, " 恢复PTE G位: PTE=0x%llx, 地址=0x%p\n", record->PteAddress->value, record->AlignAddress); } if (record->PdeAddress) { record->PdeAddress->flags.global = 1; if (record->IsLargePage) { __invlpg(record->AlignAddress); } DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, " 恢复PDE G位: PDE=0x%llx, 地址=0x%p, 大页=%d\n", record->PdeAddress->value, record->AlignAddress, record->IsLargePage); } found = true; if (align_addr) break; } EnableWriteProtection(oldIrql); if (found) { DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, "[PTE_HOOK] G位恢复完成\n"); } else { logger("未找到匹配的G位记录", true); } return found; } PteHookManager* PteHookManager::GetInstance() { if (!m_Instance) { m_Instance = static_cast( ExAllocatePoolWithTag(NonPagedPool, sizeof(PteHookManager), 'tpHk')); if (m_Instance) { RtlZeroMemory(m_Instance, sizeof(PteHookManager)); DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_INFO_LEVEL, "[PTE_HOOK] PTE Hook管理器实例已创建: 地址=0x%p\n", m_Instance); } else { DbgPrintEx(DPFLTR_ERROR_LEVEL, DPFLTR_ERROR_LEVEL, "[PTE_HOOK] 创建PTE Hook管理器实例失败\n"); } } return m_Instance; } // 全局PTE Hook管理器实例 PteHookManager* g_PteHookManager = nullptr; // 辅助函数:检查是否为目标进程 BOOLEAN IsTargetProcess(CHAR* imageName) { CHAR currentName[16]; // 复制到本地缓冲区并确保 NULL 终止 RtlCopyMemory(currentName, imageName, 16); currentName[15] = '\0'; // 确保终止 // 修剪尾部空格 for (int i = 15; i >= 0; i--) { if (currentName[i] == ' ') currentName[i] = '\0'; else if (currentName[i] != '\0') break; } return (strcmp(currentName, target_process_name) == 0); } // Hook 函数 NTSTATUS MyObReferenceObjectByHandleWithTag( HANDLE Handle, ACCESS_MASK DesiredAccess, POBJECT_TYPE ObjectType, KPROCESSOR_MODE AccessMode, ULONG Tag, PVOID* Object, POBJECT_HANDLE_INFORMATION HandleInformation ) { PEPROCESS currentProcess = PsGetCurrentProcess(); CHAR* imageName = PsGetProcessImageFileName(currentProcess); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[!] [HookFunction] 进入 Hook 函数! 当前进程: %s\n", imageName); __debugbreak(); // 强制中断,确认是否执行到这里 if (IsTargetProcess(imageName)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[!] [HookFunction] 拒绝访问目标进程 PID=%d\n", HandleToULong(PsGetCurrentProcessId())); return STATUS_ACCESS_DENIED; } return g_OriginalObReferenceObjectByHandleWithTag( Handle, DesiredAccess, ObjectType, AccessMode, Tag, Object, HandleInformation ); } NTSTATUS InstallHook() { UNICODE_STRING funcName; RtlInitUnicodeString(&funcName, L"ObReferenceObjectByHandleWithTag"); g_OriginalObReferenceObjectByHandleWithTag = (fn_ObReferenceObjectByHandleWithTag)MmGetSystemRoutineAddress(&funcName); if (!g_OriginalObReferenceObjectByHandleWithTag) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [InstallHook] 找不到 ObReferenceObjectByHandleWithTag\n"); return STATUS_NOT_FOUND; } __debugbreak(); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [InstallHook] 找到目标函数地址: %p\n", g_OriginalObReferenceObjectByHandleWithTag); void* targetFunc = (void*)g_OriginalObReferenceObjectByHandleWithTag; void* hookFunc = (void*)MyObReferenceObjectByHandleWithTag; if (!g_PteHookManager->fn_pte_inline_hook_bp_pg(targetProcessId, &targetFunc, hookFunc)) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [InstallHook] PTE Hook 安装失败\n"); return STATUS_UNSUCCESSFUL; } g_OriginalObReferenceObjectByHandleWithTag = (fn_ObReferenceObjectByHandleWithTag)targetFunc; DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [InstallHook] Hook 成功安装. 跳板地址: %p\n", targetFunc); __debugbreak(); // 强制中断,验证是否执行到这里 return STATUS_SUCCESS; } // 移除 Hook VOID RemoveHook() { if (g_OriginalObReferenceObjectByHandleWithTag && g_PteHookManager) { g_PteHookManager->fn_remove_hook(PsGetCurrentProcessId(), (void*)MyObReferenceObjectByHandleWithTag); } } // 工作线程函数 VOID InstallHookWorker(PVOID Context) { targetProcessId = (HANDLE)Context; DbgPrint("[+] Worker thread started for hook installation on PID: %d\n", HandleToULong(targetProcessId)); InstallHook(); PsTerminateSystemThread(STATUS_SUCCESS); } // 进程创建回调 VOID ProcessNotifyCallback( _In_ HANDLE ParentId, _In_ HANDLE ProcessId, _In_ BOOLEAN Create ) { UNREFERENCED_PARAMETER(ParentId); if (Create) { PEPROCESS process = NULL; if (NT_SUCCESS(PsLookupProcessByProcessId(ProcessId, &process))) { CHAR* imageName = PsGetProcessImageFileName(process); CHAR currentName[16]; RtlCopyMemory(currentName, imageName, 16); currentName[15] = '\0'; for (int i = 15; i >= 0; i--) { if (currentName[i] == ' ') currentName[i] = '\0'; else if (currentName[i] != '\0') break; } if (strcmp(currentName, target_process_name) == 0) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [ProcessNotifyCallback] 目标进程 %s 创建 (PID: %d)\n", currentName, HandleToULong(ProcessId)); HANDLE threadHandle; NTSTATUS status = PsCreateSystemThread( &threadHandle, THREAD_ALL_ACCESS, NULL, NULL, NULL, InstallHookWorker, (PVOID)ProcessId // 关键:传递目标进程ID ); if (NT_SUCCESS(status)) { ZwClose(threadHandle); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [ProcessNotifyCallback] 工作线程已创建\n"); } else { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [ProcessNotifyCallback] 创建线程失败: 0x%X\n", status); } } ObDereferenceObject(process); } } } // 驱动卸载函数 VOID DriverUnload(PDRIVER_OBJECT DriverObject) { UNREFERENCED_PARAMETER(DriverObject); DbgPrint("[+] Driver unloading...\n"); // 移除进程通知回调 PsSetCreateProcessNotifyRoutineEx((PCREATE_PROCESS_NOTIFY_ROUTINE_EX)ProcessNotifyCallback, TRUE); // 移除Hook RemoveHook(); // 清理PTE Hook资源 if (g_PteHookManager) { DbgPrint("[PTE_HOOK] Cleaning up PTE...\n"); // 恢复所有被修改的G位 g_PteHookManager->fn_resume_global_bits(nullptr); // 移除所有活动的Hook HOOK_INFO* hookInfo = g_PteHookManager->GetHookInfo(); UINT32 hookCount = g_PteHookManager->GetHookCount(); for (UINT32 i = 0; i < hookCount; i++) { if (hookInfo[i].IsHooked) { g_PteHookManager->fn_remove_hook(PsGetCurrentProcessId(), hookInfo[i].HookAddress); } } // 释放跳板池内存 char* trampLinePool = g_PteHookManager->GetTrampLinePool(); if (trampLinePool) { ExFreePoolWithTag(trampLinePool, 'JmpP'); } // 释放管理器实例 ExFreePoolWithTag(g_PteHookManager, 'tpHk'); g_PteHookManager = nullptr; } DbgPrint("[+] Driver unloaded successfully\n"); } extern "C" NTSTATUS DriverEntry(PDRIVER_OBJECT DriverObject, PUNICODE_STRING RegistryPath) { UNREFERENCED_PARAMETER(RegistryPath); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [DriverEntry] 驱动加载开始\n"); DriverObject->DriverUnload = DriverUnload; g_PteHookManager = PteHookManager::GetInstance(); if (!g_PteHookManager) { DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "[-] [DriverEntry] 初始化 PteHookManager 失败\n"); return STATUS_INSUFFICIENT_RESOURCES; } int ab = 7176; targetProcessId = (HANDLE)ab; InstallHook(); DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_INFO_LEVEL, "[+] [DriverEntry] 驱动加载成功\n"); return STATUS_SUCCESS; } 将这些代码修改正确,网上win10 10240版本

zip

最新推荐

recommend-type

langchain4j-anthropic-spring-boot-starter-0.31.0.jar中文文档.zip

1、压缩文件中包含: 中文文档、jar包下载地址、Maven依赖、Gradle依赖、源代码下载地址。 2、使用方法: 解压最外层zip,再解压其中的zip包,双击 【index.html】 文件,即可用浏览器打开、进行查看。 3、特殊说明: (1)本文档为人性化翻译,精心制作,请放心使用; (2)只翻译了该翻译的内容,如:注释、说明、描述、用法讲解 等; (3)不该翻译的内容保持原样,如:类名、方法名、包名、类型、关键字、代码 等。 4、温馨提示: (1)为了防止解压后路径太长导致浏览器无法打开,推荐在解压时选择“解压到当前文件夹”(放心,自带文件夹,文件不会散落一地); (2)有时,一套Java组件会有多个jar,所以在下载前,请仔细阅读本篇描述,以确保这就是你需要的文件。 5、本文件关键字: jar中文文档.zip,java,jar包,Maven,第三方jar包,组件,开源组件,第三方组件,Gradle,中文API文档,手册,开发手册,使用手册,参考手册。
recommend-type

TMS320F28335电机控制程序详解:BLDC、PMSM无感有感及异步VF源代码与开发资料

TMS320F28335这款高性能数字信号处理器(DSP)在电机控制领域的应用,涵盖了BLDC(无刷直流电机)、PMSM(永磁同步电机)的无感有感控制以及异步VF(变频调速)程序。文章不仅解释了各类型的电机控制原理,还提供了完整的开发资料,包括源代码、原理图和说明文档,帮助读者深入了解其工作原理和编程技巧。 适合人群:从事电机控制系统开发的技术人员,尤其是对TMS320F28335感兴趣的工程师。 使用场景及目标:适用于需要掌握TMS320F28335在不同电机控制应用场景下具体实现方法的专业人士,旨在提高他们对该微控制器的理解和实际操作能力。 其他说明:文中提供的开发资料为读者提供了从硬件到软件的全面支持,有助于加速项目开发进程并提升系统性能。
recommend-type

基于爬山搜索法的风力发电MPPT控制Simulink仿真:定步长与变步长算法性能对比 - 爬山搜索法 最新版

基于爬山搜索法的风力发电最大功率点追踪(MPPT)控制的Simulink仿真模型,重点比较了定步长和变步长算法在不同风速条件下的表现。文中展示了两种算法的具体实现方法及其优缺点。定步长算法虽然结构简单、计算量小,但在风速突变时响应较慢,存在明显的稳态振荡。相比之下,变步长算法能够根据功率变化动态调整步长,表现出更快的响应速度和更高的精度,尤其在风速突变时优势明显。实验数据显示,变步长算法在风速从8m/s突增至10m/s的情况下,仅用0.3秒即可稳定,功率波动范围仅为±15W,而定步长算法则需要0.8秒,功率波动达到±35W。 适合人群:从事风力发电研究的技术人员、对MPPT控制感兴趣的工程技术人员以及相关专业的高校师生。 使用场景及目标:适用于风力发电系统的设计与优化,特别是需要提高系统响应速度和精度的场合。目标是在不同风速条件下,选择合适的MPPT算法以最大化风能利用率。 其他说明:文章还讨论了定步长算法在风速平稳情况下的优势,提出了根据不同应用场景灵活选择或组合使用这两种算法的建议。
recommend-type

基于MatlabSimulink的风电场调频策略研究:虚拟惯性、超速减载与下垂控制的协调优化

内容概要:本文详细探讨了在Matlab/Simulink环境下,针对风电场调频的研究,尤其是双馈风机调频策略的应用及其与其他调频策略的协调工作。文中介绍了三种主要的调频策略——虚拟惯性、超速减载和下垂控制,并基于三机九节点系统进行了改进,模拟了四组风电机组的协同调频过程。研究指出,虚拟惯性的应用虽然可以提供短期频率支持,但也可能导致频率二次跌落的问题。因此,需要通过超速减载和下垂控制来进行补偿,以维持电网的稳定。此外,文章还展示了在变风速条件下,风电机组对电网频率支撑能力的显著提升,尤其是在高比例风电并网渗透的情况下,频率最低点提高了50%,验证了调频策略的有效性。 适合人群:从事电力系统、风电场运营管理和调频技术研发的专业人士,以及对风电调频感兴趣的科研人员和技术爱好者。 使用场景及目标:适用于希望深入理解风电场调频机制及其优化方法的人群。目标是掌握不同调频策略的工作原理及其协调工作的关键点,提高风电场的运行效率和稳定性。 其他说明:本文通过具体的案例研究和仿真数据,展示了调频策略的实际效果,强调了合理运用调频策略对于风电场稳定运行的重要意义。同时,也为未来的风电调频技术创新提供了理论依据和实践经验。
recommend-type

三菱QL系列PLC在3C-FPC组装机中的定位与伺服控制及触摸屏应用解析

三菱Q系列和L系列PLC在3C-FPC组装机中的具体应用,涵盖硬件架构、软件编程以及实际操作技巧。主要内容包括:使用QX42数字输入模块和QY42P晶体管输出模块进行高效信号处理;采用JE系列伺服控制系统实现高精度四轴联动;利用SFC(顺序功能图)和梯形图编程方法构建稳定可靠的控制系统;通过触摸屏实现多用户管理和权限控制;并分享了一些实用的调试和维护技巧,如流水线节拍控制和工程师模式进入方法。最终,该系统的设备综合效率(OEE)达到了92%以上。 适合人群:从事自动化控制领域的工程师和技术人员,尤其是对三菱PLC有初步了解并希望深入了解其高级应用的人群。 使用场景及目标:适用于需要高精度、高效能的工业生产设备控制场合,旨在帮助用户掌握三菱PLC及其相关组件的应用技能,提高生产效率和产品质量。 其他说明:文中提供了详细的编程实例和操作指南,有助于读者更好地理解和实践。同时提醒使用者在调试过程中应注意伺服刚性参数调整,避免不必要的机械损伤。
recommend-type

Visual C++.NET编程技术实战指南

根据提供的文件信息,可以生成以下知识点: ### Visual C++.NET编程技术体验 #### 第2章 定制窗口 - **设置窗口风格**:介绍了如何通过编程自定义窗口的外观和行为。包括改变窗口的标题栏、边框样式、大小和位置等。这通常涉及到Windows API中的`SetWindowLong`和`SetClassLong`函数。 - **创建六边形窗口**:展示了如何创建一个具有特殊形状边界的窗口,这类窗口不遵循标准的矩形形状。它需要使用`SetWindowRgn`函数设置窗口的区域。 - **创建异形窗口**:扩展了定制窗口的内容,提供了创建非标准形状窗口的方法。这可能需要创建一个不规则的窗口区域,并将其应用到窗口上。 #### 第3章 菜单和控制条高级应用 - **菜单编程**:讲解了如何创建和修改菜单项,处理用户与菜单的交互事件,以及动态地添加或删除菜单项。 - **工具栏编程**:阐述了如何使用工具栏,包括如何创建工具栏按钮、分配事件处理函数,并实现工具栏按钮的响应逻辑。 - **状态栏编程**:介绍了状态栏的创建、添加不同类型的指示器(如文本、进度条等)以及状态信息的显示更新。 - **为工具栏添加皮肤**:展示了如何为工具栏提供更加丰富的视觉效果,通常涉及到第三方的控件库或是自定义的绘图代码。 #### 第5章 系统编程 - **操作注册表**:解释了Windows注册表的结构和如何通过程序对其进行读写操作,这对于配置软件和管理软件设置非常关键。 - **系统托盘编程**:讲解了如何在系统托盘区域创建图标,并实现最小化到托盘、从托盘恢复窗口的功能。 - **鼠标钩子程序**:介绍了钩子(Hook)技术,特别是鼠标钩子,如何拦截和处理系统中的鼠标事件。 - **文件分割器**:提供了如何将文件分割成多个部分,并且能够重新组合文件的技术示例。 #### 第6章 多文档/多视图编程 - **单文档多视**:展示了如何在同一个文档中创建多个视图,这在文档编辑软件中非常常见。 #### 第7章 对话框高级应用 - **实现无模式对话框**:介绍了无模式对话框的概念及其应用场景,以及如何实现和管理无模式对话框。 - **使用模式属性表及向导属性表**:讲解了属性表的创建和使用方法,以及如何通过向导性质的对话框引导用户完成多步骤的任务。 - **鼠标敏感文字**:提供了如何实现点击文字触发特定事件的功能,这在阅读器和编辑器应用中很有用。 #### 第8章 GDI+图形编程 - **图像浏览器**:通过图像浏览器示例,展示了GDI+在图像处理和展示中的应用,包括图像的加载、显示以及基本的图像操作。 #### 第9章 多线程编程 - **使用全局变量通信**:介绍了在多线程环境下使用全局变量进行线程间通信的方法和注意事项。 - **使用Windows消息通信**:讲解了通过消息队列在不同线程间传递信息的技术,包括发送消息和处理消息。 - **使用CriticalSection对象**:阐述了如何使用临界区(CriticalSection)对象防止多个线程同时访问同一资源。 - **使用Mutex对象**:介绍了互斥锁(Mutex)的使用,用以同步线程对共享资源的访问,保证资源的安全。 - **使用Semaphore对象**:解释了信号量(Semaphore)对象的使用,它允许一个资源由指定数量的线程同时访问。 #### 第10章 DLL编程 - **创建和使用Win32 DLL**:介绍了如何创建和链接Win32动态链接库(DLL),以及如何在其他程序中使用这些DLL。 - **创建和使用MFC DLL**:详细说明了如何创建和使用基于MFC的动态链接库,适用于需要使用MFC类库的场景。 #### 第11章 ATL编程 - **简单的非属性化ATL项目**:讲解了ATL(Active Template Library)的基础使用方法,创建一个不使用属性化组件的简单项目。 - **使用ATL开发COM组件**:详细阐述了使用ATL开发COM组件的步骤,包括创建接口、实现类以及注册组件。 #### 第12章 STL编程 - **list编程**:介绍了STL(标准模板库)中的list容器的使用,讲解了如何使用list实现复杂数据结构的管理。 #### 第13章 网络编程 - **网上聊天应用程序**:提供了实现基本聊天功能的示例代码,包括客户端和服务器的通信逻辑。 - **简单的网页浏览器**:演示了如何创建一个简单的Web浏览器程序,涉及到网络通信和HTML解析。 - **ISAPI服务器扩展编程**:介绍了如何开发ISAPI(Internet Server API)服务器扩展来扩展IIS(Internet Information Services)的功能。 #### 第14章 数据库编程 - **ODBC数据库编程**:解释了ODBC(开放数据库互联)的概念,并提供了使用ODBC API进行数据库访问的示例。 - **ADO编程**:介绍了ADO(ActiveX Data Objects)技术,讲解了如何使用ADO进行数据库的增删改查等操作。 #### 第15章 HTML帮助系统编程 - **创建HTML帮助文件**:讲解了如何创建HTML帮助文件(.chm),这是微软帮助系统的一种形式。 - **区分上下文帮助**:提供了在应用程序中实现上下文敏感帮助的技术示例。 #### 第16章 安装和部署项目 - **用安装程序向导创建安装项目**:介绍了如何使用Visual Studio中的安装和部署项目向导,快速创建安装程序。 - **部署应用程序**:阐述了部署应用程序到目标系统的过程,以及如何确保应用程序的正确安装和运行。 以上内容涵盖了Visual C++.NET编程技术体验一书中涉及的各个技术点,并对每个章节中的示例做了详细的说明。这些知识点对于理解和掌握Visual C++.NET编程非常重要,并能够帮助读者在实际项目开发中应用这些技术。
recommend-type

HarmonyOS内核深度探秘:优化自由行旅游系统的策略

# 1. HarmonyOS内核概述与旅游系统需求分析 ## 1.1 HarmonyOS内核概述 HarmonyOS(鸿蒙操作系统)是由华为开发的一个分布式操作系统,旨在提供跨多种设备的无缝体验。其内核设计理念强调高效、安全和模块化,以微内核架构和分布式能力
recommend-type

tkinter模块所有控件

<think>好的,我现在需要回答用户关于Tkinter模块中所有控件列表的问题。首先,我需要回忆Tkinter的基础知识,确保自己了解各个控件的名称和用途。用户希望得到所有支持的控件列表,所以必须全面覆盖,不能遗漏。 首先,我应该从Tkinter的标准控件开始。常见的控件包括Label、Button、Entry这些基础部件。然后是Frame,用于布局,还有Canvas用于绘图。接下来是Checkbutton、Radiobutton,这些属于选择类控件。Listbox和Scrollbar通常一起使用,处理滚动内容。还有Scale(滑块)、Spinbox、Menu、Menubutton这些可能
recommend-type

局域网五子棋游戏:娱乐与聊天的完美结合

标题“网络五子棋”和描述“适合于局域网之间娱乐和聊天!”以及标签“五子棋 网络”所涉及的知识点主要围绕着五子棋游戏的网络版本及其在局域网中的应用。以下是详细的知识点: 1. 五子棋游戏概述: 五子棋是一种两人对弈的纯策略型棋类游戏,又称为连珠、五子连线等。游戏的目标是在一个15x15的棋盘上,通过先后放置黑白棋子,使得任意一方先形成连续五个同色棋子的一方获胜。五子棋的规则简单,但策略丰富,适合各年龄段的玩家。 2. 网络五子棋的意义: 网络五子棋是指可以在互联网或局域网中连接进行对弈的五子棋游戏版本。通过网络版本,玩家不必在同一地点即可进行游戏,突破了空间限制,满足了现代人们快节奏生活的需求,同时也为玩家们提供了与不同对手切磋交流的机会。 3. 局域网通信原理: 局域网(Local Area Network,LAN)是一种覆盖较小范围如家庭、学校、实验室或单一建筑内的计算机网络。它通过有线或无线的方式连接网络内的设备,允许用户共享资源如打印机和文件,以及进行游戏和通信。局域网内的计算机之间可以通过网络协议进行通信。 4. 网络五子棋的工作方式: 在局域网中玩五子棋,通常需要一个客户端程序(如五子棋.exe)和一个服务器程序。客户端负责显示游戏界面、接受用户输入、发送落子请求给服务器,而服务器负责维护游戏状态、处理玩家的游戏逻辑和落子请求。当一方玩家落子时,客户端将该信息发送到服务器,服务器确认无误后将更新后的棋盘状态传回给所有客户端,更新显示。 5. 五子棋.exe程序: 五子棋.exe是一个可执行程序,它使得用户可以在个人计算机上安装并运行五子棋游戏。该程序可能包含了游戏的图形界面、人工智能算法(如果支持单机对战AI的话)、网络通信模块以及游戏规则的实现。 6. put.wav文件: put.wav是一个声音文件,很可能用于在游戏进行时提供声音反馈,比如落子声。在网络环境中,声音文件可能被用于提升玩家的游戏体验,尤其是在局域网多人游戏场景中。当玩家落子时,系统会播放.wav文件中的声音,为游戏增添互动性和趣味性。 7. 网络五子棋的技术要求: 为了确保多人在线游戏的顺利进行,网络五子棋需要具备一些基本的技术要求,包括但不限于稳定的网络连接、高效的数据传输协议(如TCP/IP)、以及安全的数据加密措施(如果需要的话)。此外,还需要有一个良好的用户界面设计来提供直观和舒适的用户体验。 8. 社交与娱乐: 网络五子棋除了是一个娱乐游戏外,它还具有社交功能。玩家可以通过游戏内的聊天系统进行交流,分享经验和策略,甚至通过网络寻找新的朋友。这使得网络五子棋不仅是一个个人娱乐工具,同时也是一种社交活动。 总结来说,网络五子棋结合了五子棋游戏的传统魅力和现代网络技术,使得不同地区的玩家能够在局域网内进行娱乐和聊天,既丰富了人们的娱乐生活,又加强了人际交流。而实现这一切的基础在于客户端程序的设计、服务器端的稳定运行、局域网的高效通信,以及音效文件增强的游戏体验。
recommend-type

自由行旅游新篇章:HarmonyOS技术融合与系统架构深度解析

# 1. HarmonyOS技术概述 ## 1.1 HarmonyOS的起源与发展 HarmonyOS(鸿蒙操作系统)由华为公司开发,旨在构建全场景分布式OS,以应对不同设备间的互联问题。自从2019年首次发布以来,HarmonyOS迅速成长,并迅速应用于智能手机、平板、智能穿戴、车载设备等多种平台。该系