diff options
Diffstat (limited to 'utils/hwstub/stub/main.c')
-rw-r--r-- | utils/hwstub/stub/main.c | 112 |
1 files changed, 60 insertions, 52 deletions
diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c index 3eee8b6a18..7b6b02b83a 100644 --- a/utils/hwstub/stub/main.c +++ b/utils/hwstub/stub/main.c | |||
@@ -427,34 +427,25 @@ static void handle_read(struct usb_ctrlrequest *req) | |||
427 | return usb_drv_stall(EP_CONTROL, true, true); | 427 | return usb_drv_stall(EP_CONTROL, true, true); |
428 | size_t len = MIN(req->wLength, last_read_max_size); | 428 | size_t len = MIN(req->wLength, last_read_max_size); |
429 | 429 | ||
430 | if(req->bRequest == HWSTUB_READ2_ATOMIC) | 430 | int ret = set_exception_jmp(); |
431 | if(ret == 0) | ||
431 | { | 432 | { |
432 | if(set_data_abort_jmp() == 0) | 433 | if(req->bRequest == HWSTUB_READ2_ATOMIC) |
433 | { | 434 | { |
434 | if(!read_atomic(usb_buffer, last_read_addr, len)) | 435 | if(!read_atomic(usb_buffer, last_read_addr, len)) |
435 | return usb_drv_stall(EP_CONTROL, true, true); | 436 | return usb_drv_stall(EP_CONTROL, true, true); |
436 | } | 437 | } |
437 | else | 438 | else |
438 | { | 439 | { |
439 | logf("trapped read data abort in [0x%x,0x%x]\n", last_read_addr, | 440 | memcpy(usb_buffer, last_read_addr, len); |
440 | last_read_addr + len); | 441 | asm volatile("nop" : : : "memory"); |
441 | return usb_drv_stall(EP_CONTROL, true, true); | ||
442 | } | 442 | } |
443 | } | 443 | } |
444 | else | 444 | else |
445 | { | 445 | { |
446 | if(set_data_abort_jmp() == 0) | 446 | logf("trapped exception %d in read [0x%x,0x%x]\n", ret, last_read_addr, |
447 | { | 447 | last_read_addr + len); |
448 | memcpy(usb_buffer, last_read_addr, len); | 448 | return usb_drv_stall(EP_CONTROL, true, true); |
449 | asm volatile("nop" : : : "memory"); | ||
450 | } | ||
451 | else | ||
452 | { | ||
453 | logf("trapped read data abort in [0x%x,0x%x]\n", last_read_addr, | ||
454 | last_read_addr + len); | ||
455 | return usb_drv_stall(EP_CONTROL, true, true); | ||
456 | } | ||
457 | |||
458 | } | 449 | } |
459 | 450 | ||
460 | usb_drv_send(EP_CONTROL, usb_buffer, len); | 451 | usb_drv_send(EP_CONTROL, usb_buffer, len); |
@@ -482,9 +473,10 @@ static void handle_write(struct usb_ctrlrequest *req) | |||
482 | if(size < sz_hdr) | 473 | if(size < sz_hdr) |
483 | return usb_drv_stall(EP_CONTROL, true, true); | 474 | return usb_drv_stall(EP_CONTROL, true, true); |
484 | 475 | ||
485 | if(req->bRequest == HWSTUB_WRITE_ATOMIC) | 476 | int ret = set_exception_jmp(); |
477 | if(ret == 0) | ||
486 | { | 478 | { |
487 | if(set_data_abort_jmp() == 0) | 479 | if(req->bRequest == HWSTUB_WRITE_ATOMIC) |
488 | { | 480 | { |
489 | if(!write_atomic((void *)write->dAddress, | 481 | if(!write_atomic((void *)write->dAddress, |
490 | usb_buffer + sz_hdr, size - sz_hdr)) | 482 | usb_buffer + sz_hdr, size - sz_hdr)) |
@@ -492,29 +484,57 @@ static void handle_write(struct usb_ctrlrequest *req) | |||
492 | } | 484 | } |
493 | else | 485 | else |
494 | { | 486 | { |
495 | logf("trapped write data abort in [0x%x,0x%x]\n", write->dAddress, | 487 | memcpy((void *)write->dAddress, |
496 | write->dAddress + size - sz_hdr); | 488 | usb_buffer + sz_hdr, size - sz_hdr); |
497 | return usb_drv_stall(EP_CONTROL, true, true); | ||
498 | } | 489 | } |
499 | } | 490 | } |
500 | else | 491 | else |
501 | { | 492 | { |
502 | if(set_data_abort_jmp() == 0) | 493 | logf("trapped exception %d in write [0x%x,0x%x]\n", ret, write->dAddress, |
503 | { | 494 | write->dAddress + size - sz_hdr); |
504 | memcpy((void *)write->dAddress, | 495 | return usb_drv_stall(EP_CONTROL, true, true); |
505 | usb_buffer + sz_hdr, size - sz_hdr); | ||
506 | } | ||
507 | else | ||
508 | { | ||
509 | logf("trapped write data abort in [0x%x,0x%x]\n", write->dAddress, | ||
510 | write->dAddress + size - sz_hdr); | ||
511 | return usb_drv_stall(EP_CONTROL, true, true); | ||
512 | } | ||
513 | } | 496 | } |
514 | 497 | ||
515 | usb_drv_send(EP_CONTROL, NULL, 0); | 498 | usb_drv_send(EP_CONTROL, NULL, 0); |
516 | } | 499 | } |
517 | 500 | ||
501 | static bool do_call(uint32_t addr) | ||
502 | { | ||
503 | /* trap exceptions */ | ||
504 | int ret = set_exception_jmp(); | ||
505 | if(ret == 0) | ||
506 | { | ||
507 | #if defined(CPU_ARM) | ||
508 | /* in case of call, respond after return */ | ||
509 | asm volatile("blx %0\n" : : "r"(addr) : "memory"); | ||
510 | return true; | ||
511 | #elif defined(CPU_MIPS) | ||
512 | asm volatile("jalr %0\nnop\n" : : "r"(addr) : "memory"); | ||
513 | return true; | ||
514 | #else | ||
515 | #warning call is unsupported on this platform | ||
516 | return false; | ||
517 | #endif | ||
518 | } | ||
519 | else | ||
520 | { | ||
521 | logf("trapped exception %d in call\n", ret); | ||
522 | return false; | ||
523 | } | ||
524 | } | ||
525 | |||
526 | static void do_jump(uint32_t addr) | ||
527 | { | ||
528 | #if defined(CPU_ARM) | ||
529 | asm volatile("bx %0\n" : : "r" (addr) : "memory"); | ||
530 | #elif defined(CPU_MIPS) | ||
531 | asm volatile("jr %0\nnop\n" : : "r" (addr) : "memory"); | ||
532 | #else | ||
533 | #warning jump is unsupported on this platform | ||
534 | #define NO_JUMP | ||
535 | #endif | ||
536 | } | ||
537 | |||
518 | static void handle_exec(struct usb_ctrlrequest *req) | 538 | static void handle_exec(struct usb_ctrlrequest *req) |
519 | { | 539 | { |
520 | int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength); | 540 | int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength); |
@@ -537,31 +557,19 @@ static void handle_exec(struct usb_ctrlrequest *req) | |||
537 | 557 | ||
538 | if(exec->bmFlags & HWSTUB_EXEC_CALL) | 558 | if(exec->bmFlags & HWSTUB_EXEC_CALL) |
539 | { | 559 | { |
540 | #if defined(CPU_ARM) | 560 | if(do_call(addr)) |
541 | /* in case of call, respond after return */ | 561 | usb_drv_send(EP_CONTROL, NULL, 0); |
542 | asm volatile("blx %0\n" : : "r"(addr) : "memory"); | 562 | else |
543 | usb_drv_send(EP_CONTROL, NULL, 0); | 563 | usb_drv_stall(EP_CONTROL, true, true); |
544 | #elif defined(CPU_MIPS) | ||
545 | asm volatile("jalr %0\nnop\n" : : "r"(addr) : "memory"); | ||
546 | usb_drv_send(EP_CONTROL, NULL, 0); | ||
547 | #else | ||
548 | #warning call is unsupported on this platform | ||
549 | usb_drv_stall(EP_CONTROL, true, true); | ||
550 | #endif | ||
551 | } | 564 | } |
552 | else | 565 | else |
553 | { | 566 | { |
567 | #ifndef NO_JUMP | ||
554 | /* in case of jump, respond immediately and disconnect usb */ | 568 | /* in case of jump, respond immediately and disconnect usb */ |
555 | #if defined(CPU_ARM) | ||
556 | usb_drv_send(EP_CONTROL, NULL, 0); | ||
557 | usb_drv_exit(); | ||
558 | asm volatile("bx %0\n" : : "r" (addr) : "memory"); | ||
559 | #elif defined(CPU_MIPS) | ||
560 | usb_drv_send(EP_CONTROL, NULL, 0); | 569 | usb_drv_send(EP_CONTROL, NULL, 0); |
561 | usb_drv_exit(); | 570 | usb_drv_exit(); |
562 | asm volatile("jr %0\nnop\n" : : "r" (addr) : "memory"); | 571 | do_jump(addr); |
563 | #else | 572 | #else |
564 | #warning jump is unsupported on this platform | ||
565 | usb_drv_stall(EP_CONTROL, true, true); | 573 | usb_drv_stall(EP_CONTROL, true, true); |
566 | #endif | 574 | #endif |
567 | } | 575 | } |