diff options
Diffstat (limited to 'utils/hwstub/stub/main.c')
-rw-r--r-- | utils/hwstub/stub/main.c | 72 |
1 files changed, 70 insertions, 2 deletions
diff --git a/utils/hwstub/stub/main.c b/utils/hwstub/stub/main.c index 4e54b8fd89..3604cfdae0 100644 --- a/utils/hwstub/stub/main.c +++ b/utils/hwstub/stub/main.c | |||
@@ -356,6 +356,49 @@ static void handle_get_log(struct usb_ctrlrequest *req) | |||
356 | enable_logf(true); | 356 | enable_logf(true); |
357 | } | 357 | } |
358 | 358 | ||
359 | /* default implementation, relying on the compiler to produce correct code, | ||
360 | * targets should reimplement this... */ | ||
361 | uint8_t __attribute__((weak)) target_read8(const void *addr) | ||
362 | { | ||
363 | return *(volatile uint8_t *)addr; | ||
364 | } | ||
365 | |||
366 | uint16_t __attribute__((weak)) target_read16(const void *addr) | ||
367 | { | ||
368 | return *(volatile uint16_t *)addr; | ||
369 | } | ||
370 | |||
371 | uint32_t __attribute__((weak)) target_read32(const void *addr) | ||
372 | { | ||
373 | return *(volatile uint32_t *)addr; | ||
374 | } | ||
375 | |||
376 | void __attribute__((weak)) target_write8(void *addr, uint8_t val) | ||
377 | { | ||
378 | *(volatile uint8_t *)addr = val; | ||
379 | } | ||
380 | |||
381 | void __attribute__((weak)) target_write16(void *addr, uint16_t val) | ||
382 | { | ||
383 | *(volatile uint16_t *)addr = val; | ||
384 | } | ||
385 | |||
386 | void __attribute__((weak)) target_write32(void *addr, uint32_t val) | ||
387 | { | ||
388 | *(volatile uint32_t *)addr = val; | ||
389 | } | ||
390 | |||
391 | static bool read_atomic(void *dst, void *src, size_t sz) | ||
392 | { | ||
393 | switch(sz) | ||
394 | { | ||
395 | case 1: *(uint8_t *)dst = target_read8(src); return true; | ||
396 | case 2: *(uint16_t *)dst = target_read16(src); return true; | ||
397 | case 4: *(uint32_t *)dst = target_read32(src); return true; | ||
398 | default: return false; | ||
399 | } | ||
400 | } | ||
401 | |||
359 | static void handle_read(struct usb_ctrlrequest *req) | 402 | static void handle_read(struct usb_ctrlrequest *req) |
360 | { | 403 | { |
361 | static uint32_t last_addr = 0; | 404 | static uint32_t last_addr = 0; |
@@ -377,13 +420,30 @@ static void handle_read(struct usb_ctrlrequest *req) | |||
377 | { | 420 | { |
378 | if(id != last_id) | 421 | if(id != last_id) |
379 | return usb_drv_stall(EP_CONTROL, true, true); | 422 | return usb_drv_stall(EP_CONTROL, true, true); |
380 | memcpy(usb_buffer, (void *)last_addr, req->wLength); | 423 | if(req->bRequest == HWSTUB_READ2_ATOMIC) |
424 | { | ||
425 | if(!read_atomic(usb_buffer, (void *)last_addr, req->wLength)) | ||
426 | return usb_drv_stall(EP_CONTROL, true, true); | ||
427 | } | ||
428 | else | ||
429 | memcpy(usb_buffer, (void *)last_addr, req->wLength); | ||
381 | asm volatile("nop" : : : "memory"); | 430 | asm volatile("nop" : : : "memory"); |
382 | usb_drv_send(EP_CONTROL, usb_buffer, req->wLength); | 431 | usb_drv_send(EP_CONTROL, usb_buffer, req->wLength); |
383 | usb_drv_recv(EP_CONTROL, NULL, 0); | 432 | usb_drv_recv(EP_CONTROL, NULL, 0); |
384 | } | 433 | } |
385 | }; | 434 | }; |
386 | 435 | ||
436 | static bool write_atomic(void *dst, void *src, size_t sz) | ||
437 | { | ||
438 | switch(sz) | ||
439 | { | ||
440 | case 1: target_write8(dst, *(uint8_t *)src); return true; | ||
441 | case 2: target_write16(dst, *(uint16_t *)src); return true; | ||
442 | case 4: target_write32(dst, *(uint32_t *)src); return true; | ||
443 | default: return false; | ||
444 | } | ||
445 | } | ||
446 | |||
387 | static void handle_write(struct usb_ctrlrequest *req) | 447 | static void handle_write(struct usb_ctrlrequest *req) |
388 | { | 448 | { |
389 | int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength); | 449 | int size = usb_drv_recv(EP_CONTROL, usb_buffer, req->wLength); |
@@ -392,7 +452,13 @@ static void handle_write(struct usb_ctrlrequest *req) | |||
392 | int sz_hdr = sizeof(struct hwstub_write_req_t); | 452 | int sz_hdr = sizeof(struct hwstub_write_req_t); |
393 | if(size < sz_hdr) | 453 | if(size < sz_hdr) |
394 | return usb_drv_stall(EP_CONTROL, true, true); | 454 | return usb_drv_stall(EP_CONTROL, true, true); |
395 | memcpy((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr); | 455 | if(req->bRequest == HWSTUB_WRITE_ATOMIC) |
456 | { | ||
457 | if(!write_atomic((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr)) | ||
458 | return usb_drv_stall(EP_CONTROL, true, true); | ||
459 | } | ||
460 | else | ||
461 | memcpy((void *)write->dAddress, usb_buffer + sz_hdr, size - sz_hdr); | ||
396 | usb_drv_send(EP_CONTROL, NULL, 0); | 462 | usb_drv_send(EP_CONTROL, NULL, 0); |
397 | } | 463 | } |
398 | 464 | ||
@@ -451,8 +517,10 @@ static void handle_class_intf_req(struct usb_ctrlrequest *req) | |||
451 | return handle_get_log(req); | 517 | return handle_get_log(req); |
452 | case HWSTUB_READ: | 518 | case HWSTUB_READ: |
453 | case HWSTUB_READ2: | 519 | case HWSTUB_READ2: |
520 | case HWSTUB_READ2_ATOMIC: | ||
454 | return handle_read(req); | 521 | return handle_read(req); |
455 | case HWSTUB_WRITE: | 522 | case HWSTUB_WRITE: |
523 | case HWSTUB_WRITE_ATOMIC: | ||
456 | return handle_write(req); | 524 | return handle_write(req); |
457 | case HWSTUB_EXEC: | 525 | case HWSTUB_EXEC: |
458 | return handle_exec(req); | 526 | return handle_exec(req); |