diff --git a/bricks/ev3/mphalport.c b/bricks/ev3/mphalport.c index b5de81c2a..7f869fc36 100644 --- a/bricks/ev3/mphalport.c +++ b/bricks/ev3/mphalport.c @@ -8,10 +8,8 @@ #include #include -#include - #include -#include +#include #include "py/runtime.h" #include "py/mphal.h" @@ -32,49 +30,53 @@ void mp_hal_delay_ms(mp_uint_t Delay) { } while (pbdrv_clock_get_ms() - start < Delay); } -extern uint32_t pbdrv_usb_rx_data_available(void); - -extern int32_t pbdrv_usb_get_char(void); - uintptr_t mp_hal_stdio_poll(uintptr_t poll_flags) { - uintptr_t ret = 0; - if ((poll_flags & MP_STREAM_POLL_RD) && pbdrv_usb_rx_data_available() > 0) { + if ((poll_flags & MP_STREAM_POLL_RD) && pbsys_host_stdin_get_available()) { ret |= MP_STREAM_POLL_RD; } return ret; } +// Receive single character int mp_hal_stdin_rx_chr(void) { - int c; - while (((c = pbdrv_uart_debug_get_char()) == -1) && (c = pbdrv_usb_get_char()) == -1) { - MICROPY_EVENT_POLL_HOOK; + uint32_t size; + uint8_t c; + + // wait for rx interrupt + while (size = 1, pbsys_host_stdin_read(&c, &size) != PBIO_SUCCESS) { + MICROPY_EVENT_POLL_HOOK } + return c; } -extern uint32_t pbdrv_usb_write(const uint8_t *data, uint32_t size); - +// Send string of given length mp_uint_t mp_hal_stdout_tx_strn(const char *str, size_t len) { - pbdrv_uart_debug_printf("%.*s", len, str); - - uint32_t done = 0; - while (done < len) { - done += pbdrv_usb_write((uint8_t *)str + done, len - done); - MICROPY_VM_HOOK_LOOP; - } + size_t remaining = len; + + while (remaining) { + uint32_t size = remaining; + pbio_error_t err = pbsys_host_stdout_write((const uint8_t *)str, &size); + if (err == PBIO_SUCCESS) { + str += size; + remaining -= size; + } else if (err != PBIO_ERROR_AGAIN) { + // Ignoring error for now. This means stdout is lost if Bluetooth/USB + // is disconnected. + return len - remaining; + } - while (!pbdrv_uart_debug_is_done()) { - MICROPY_VM_HOOK_LOOP; + MICROPY_EVENT_POLL_HOOK } return len; } -extern void pbdrv_usb_tx_flush(void); - void mp_hal_stdout_tx_flush(void) { - pbdrv_usb_tx_flush(); + while (!pbsys_host_tx_is_idle()) { + MICROPY_EVENT_POLL_HOOK + } } diff --git a/lib/pbio/drv/ioport/ioport.c b/lib/pbio/drv/ioport/ioport.c index 192a46fe5..dbd7a7ff9 100644 --- a/lib/pbio/drv/ioport/ioport.c +++ b/lib/pbio/drv/ioport/ioport.c @@ -65,6 +65,7 @@ pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_u return PBIO_ERROR_NOT_SUPPORTED; } +#if PBDRV_CONFIG_HAS_PORT_VCC_CONTROL void pbdrv_ioport_enable_vcc(bool enable) { if (enable) { pbdrv_gpio_out_high(&pbdrv_ioport_platform_data_vcc_pin); @@ -72,5 +73,6 @@ void pbdrv_ioport_enable_vcc(bool enable) { pbdrv_gpio_out_low(&pbdrv_ioport_platform_data_vcc_pin); } } +#endif // PBDRV_CONFIG_HAS_PORT_VCC_CONTROL #endif // PBDRV_CONFIG_IOPORT diff --git a/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h b/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h index 042e89c84..c040d9813 100644 --- a/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h +++ b/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h @@ -45,28 +45,6 @@ extern "C" { * @} */ -// NOTE: These enums values are sent over the wire, so cannot be changed. Also, -// 0 is skipped to avoid a zeroed buffer from being misinterpreted as a message. - -/** Hub to host messages via the Pybricks interface IN endpoint. */ -enum { - /** - * Analog of BLE status response. Emitted in response to every OUT message - * received. - */ - USBD_PYBRICKS_IN_EP_MSG_RESPONSE = 1, - /**Analog to BLE notification. Only emitted if subscribed. */ - USBD_PYBRICKS_IN_EP_MSG_EVENT = 2, -}; - -/** Host to hub messages via the Pybricks USB interface OUT endpoint. */ -enum { - /** Analog of BLE Client Characteristic Configuration Descriptor (CCCD). */ - USBD_PYBRICKS_OUT_EP_MSG_SUBSCRIBE = 1, - /** Analog of BLE Client Characteristic Write with response. */ - USBD_PYBRICKS_OUT_EP_MSG_COMMAND = 2, -}; - /** @defgroup USBD_Pybricks_Exported_TypesDefinitions * @{ diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index 7ee7f4da1..122551498 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -10,12 +10,19 @@ #include #include +#include +#include #include #include #include #include #include +#include +#include +#include +#include +#include #include @@ -179,13 +186,15 @@ static const pbdrv_usb_ev3_conf_1_union_t configuration_1_desc_fs = { } }; -// We generate a serial number string descriptors at runtime -// so this dynamic buffer is needed -#define STRING_DESC_MAX_SZ 64 +// This dynamic buffer is needed in order to have an aligned, +// global-lifetime buffer for sending dynamic data in response +// to control transfers. This is used for the serial number string +// and for Pybricks protocol requests. static union { - uint8_t b[STRING_DESC_MAX_SZ]; - uint32_t u[STRING_DESC_MAX_SZ / 4]; -} usb_string_desc_buffer; + uint8_t b[EP0_BUF_SZ]; + uint32_t u[EP0_BUF_SZ / sizeof(uint32_t)]; +} pbdrv_usb_ev3_ep0_buffer; +_Static_assert(PBIO_PYBRICKS_HUB_CAPABILITIES_VALUE_SIZE <= EP0_BUF_SZ); // Defined in pbio/platform/ev3/platform.c extern uint8_t pbdrv_ev3_bluetooth_mac_address[6]; @@ -207,6 +216,9 @@ static uint32_t pbdrv_usb_setup_misc_tx_byte; // Whether the device is using USB high-speed mode or not static bool pbdrv_usb_is_usb_hs; +// Whether there is a host app listening to events +static bool pbdrv_usb_is_events_subscribed; + // Buffers, used for different logical flows on the data endpoint static uint8_t ep1_rx_buf[PYBRICKS_EP_PKT_SZ_HS]; static uint8_t ep1_tx_response_buf[PYBRICKS_EP_PKT_SZ_HS]; @@ -456,17 +468,17 @@ static bool usb_get_descriptor(uint16_t wValue) { return true; case STRING_DESC_SERIAL: - usb_string_desc_buffer.b[0] = 2 * 2 * 6 + 2; - usb_string_desc_buffer.b[1] = DESC_TYPE_STRING; + pbdrv_usb_ev3_ep0_buffer.b[0] = 2 * 2 * 6 + 2; + pbdrv_usb_ev3_ep0_buffer.b[1] = DESC_TYPE_STRING; for (i = 0; i < 6; i++) { - usb_string_desc_buffer.b[2 + 4 * i + 0] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] >> 4]; - usb_string_desc_buffer.b[2 + 4 * i + 1] = 0; - usb_string_desc_buffer.b[2 + 4 * i + 2] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] & 0xf]; - usb_string_desc_buffer.b[2 + 4 * i + 3] = 0; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 0] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] >> 4]; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 1] = 0; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 2] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] & 0xf]; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 3] = 0; } - pbdrv_usb_setup_data_to_send = usb_string_desc_buffer.u; - pbdrv_usb_setup_data_to_send_sz = usb_string_desc_buffer.b[0]; + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + pbdrv_usb_setup_data_to_send_sz = pbdrv_usb_ev3_ep0_buffer.b[0]; return true; } break; @@ -484,6 +496,15 @@ static void usb_device_intr(void) { IntSystemStatusClear(SYS_INT_USB0); uint32_t intr_src = HWREG(USB_0_OTGBASE + USB_0_INTR_SRC); + if (intr_src & USBOTG_INTR_DISCON) { + // USB cable disconnected + + // Mark config and address as 0 for main loop to detect + pbdrv_usb_addr = 0; + pbdrv_usb_config = 0; + pbio_os_request_poll(); + } + if (intr_src & USBOTG_INTR_RESET) { // USB reset @@ -678,6 +699,67 @@ static void usb_device_intr(void) { } break; } + break; + + case BM_REQ_TYPE_CLASS: + if ((setup_pkt.s.bmRequestType & BM_REQ_RECIP_MASK) != BM_REQ_RECIP_IF) { + // Pybricks class requests must be directed at the interface + break; + } + + switch (setup_pkt.s.bRequest) { + const char *s; + + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT: + // Standard GATT characteristic + switch (setup_pkt.s.wValue) { + case 0x2A00: + // GATT Device Name characteristic + s = pbdrv_bluetooth_get_hub_name(); + pbdrv_usb_setup_data_to_send_sz = strlen(s); + memcpy(pbdrv_usb_ev3_ep0_buffer.b, s, pbdrv_usb_setup_data_to_send_sz); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + handled = true; + break; + + case 0x2A26: + // GATT Firmware Revision characteristic + s = PBIO_VERSION_STR; + pbdrv_usb_setup_data_to_send_sz = strlen(s); + memcpy(pbdrv_usb_ev3_ep0_buffer.b, s, pbdrv_usb_setup_data_to_send_sz); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + handled = true; + break; + + case 0x2A28: + // GATT Software Revision characteristic + s = PBIO_PROTOCOL_VERSION_STR; + pbdrv_usb_setup_data_to_send_sz = strlen(s); + memcpy(pbdrv_usb_ev3_ep0_buffer.b, s, pbdrv_usb_setup_data_to_send_sz); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + handled = true; + break; + } + break; + + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: + // Pybricks characteristic + switch (setup_pkt.s.wValue) { + case 0x0003: + pbio_pybricks_hub_capabilities( + pbdrv_usb_ev3_ep0_buffer.b, + (pbdrv_usb_is_usb_hs ? PYBRICKS_EP_PKT_SZ_HS : PYBRICKS_EP_PKT_SZ_FS) - 1, + PBSYS_CONFIG_APP_FEATURE_FLAGS, + pbsys_storage_get_maximum_program_size(), + PBSYS_CONFIG_HMI_NUM_SLOTS); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + pbdrv_usb_setup_data_to_send_sz = PBIO_PYBRICKS_HUB_CAPABILITIES_VALUE_SIZE; + handled = true; + break; + } + break; + } + break; } // Note regarding the setting of the USB_CSRL0_RXRDYC bit: @@ -770,7 +852,7 @@ static void usb_device_intr(void) { pbio_os_request_poll(); } - if (dma_q_pend_0 & (1 << TX_COMPQ1)) { + while (dma_q_pend_0 & (1 << TX_COMPQ1)) { // DMA for EP 1 IN is done // Pop the descriptor from the queue @@ -786,6 +868,10 @@ static void usb_device_intr(void) { usb_tx_stdout_is_not_ready = false; pbio_os_request_poll(); } + + // Multiple TX completions can happen at once + // (since we have up to three descriptors in flight) + dma_q_pend_0 = HWREG(USB_0_OTGBASE + CPDMA_PEND_0_REGISTER); } HWREG(USB_0_OTGBASE + USB_0_INTR_SRC_CLEAR) = intr_src; @@ -795,10 +881,22 @@ static void usb_device_intr(void) { static pbio_os_process_t pbdrv_usb_ev3_process; static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *context) { + static pbio_os_timer_t delay_timer; + static pbio_os_timer_t tx_timeout_timer; + static pbio_os_timer_t keepalive_timer; + static bool was_transmitting = false; + static bool is_transmitting = false; + static uint32_t prev_status_flags = ~0; + static uint32_t new_status_flags = 0; + PBIO_OS_ASYNC_BEGIN(state); for (;;) { - PBIO_OS_AWAIT_UNTIL(state, usb_rx_is_ready); + if (pbdrv_usb_config == 0) { + pbdrv_usb_is_events_subscribed = false; + // Resend status flags when host subscribes + prev_status_flags = ~0; + } if (usb_rx_is_ready) { // This barrier prevents *subsequent* memory reads from being @@ -807,29 +905,89 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c pbdrv_compiler_memory_barrier(); uint32_t usb_rx_sz = cppi_descriptors[CPPI_DESC_RX].word0.pktLength; - - // TODO: Remove this echo test - unsigned int i; - for (i = 0; i < usb_rx_sz; i++) { - ep1_tx_response_buf[i] = ep1_rx_buf[i] + 1; - } - for (; i < 512; i++) { - ep1_tx_response_buf[i] = 0xaa; + pbio_pybricks_error_t result; + bool usb_send_response = false; + // Skip empty commands, or commands sent when previous response is still busy + if (usb_rx_sz && !usb_tx_response_is_not_ready) { + switch (ep1_rx_buf[0]) { + case PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE: + pbio_os_timer_set(&keepalive_timer, 1000); + pbdrv_usb_is_events_subscribed = ep1_rx_buf[1]; + ep1_tx_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE; + pbio_set_uint32_le(&ep1_tx_response_buf[1], PBIO_PYBRICKS_ERROR_OK); + usb_send_response = true; + break; + case PBIO_PYBRICKS_OUT_EP_MSG_COMMAND: + result = pbsys_command(ep1_rx_buf + 1, usb_rx_sz - 1); + ep1_tx_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE; + pbio_set_uint32_le(&ep1_tx_response_buf[1], result); + usb_send_response = true; + break; + } } - (void)ep1_tx_status_buf; - (void)ep1_tx_stdout_buf; - - unsigned int tx_sz = pbdrv_usb_is_usb_hs ? PYBRICKS_EP_PKT_SZ_HS : PYBRICKS_EP_PKT_SZ_FS; - if (!usb_tx_response_is_not_ready) { + if (usb_send_response) { usb_tx_response_is_not_ready = true; - usb_setup_tx_dma_desc(CPPI_DESC_TX_RESPONSE, ep1_tx_response_buf, tx_sz); + usb_setup_tx_dma_desc(CPPI_DESC_TX_RESPONSE, ep1_tx_response_buf, PBIO_PYBRICKS_USB_MESSAGE_SIZE(sizeof(uint32_t))); } // Re-queue RX buffer after processing is complete usb_rx_is_ready = false; usb_setup_rx_dma_desc(); } + + // Send status flags if they've changed (and we can) + new_status_flags = pbsys_status_get_flags(); + if (pbdrv_usb_is_events_subscribed && !usb_tx_status_is_not_ready && + (new_status_flags != prev_status_flags || pbio_os_timer_is_expired(&keepalive_timer))) { + ep1_tx_status_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_EVENT; + uint32_t usb_status_sz = PBIO_PYBRICKS_USB_MESSAGE_SIZE(pbsys_status_get_status_report(&ep1_tx_status_buf[1])); + + usb_tx_status_is_not_ready = true; + usb_setup_tx_dma_desc(CPPI_DESC_TX_STATUS, ep1_tx_status_buf, usb_status_sz); + + prev_status_flags = new_status_flags; + + if (new_status_flags != prev_status_flags) { + // If we are sending a status because the flags have changed, + // we can bump out the keepalive timer. + pbio_os_timer_set(&keepalive_timer, 1000); + } else { + // Otherwise, we want to send keepalives at a particular rate. + pbio_os_timer_extend(&keepalive_timer); + } + } + + // Handle timeouts + is_transmitting = usb_tx_response_is_not_ready || usb_tx_status_is_not_ready || usb_tx_stdout_is_not_ready; + if (was_transmitting && is_transmitting) { + if (pbio_os_timer_is_expired(&tx_timeout_timer)) { + // Flush _all_ TX packets + while (HWREGB(USB0_BASE + USB_O_TXCSRL1) & USB_TXCSRL1_TXRDY) { + HWREGB(USB0_BASE + USB_O_TXCSRL1) = USB_TXCSRL1_FLUSH; + // We need to wait a bit until the DMA refills the FIFO. + // There doesn't seem to be a good way to figure out if + // there are packets in flight *within* the DMA engine itself + // (i.e. no longer in the queue but in the transfer engine). + PBIO_OS_AWAIT_MS(state, &delay_timer, 1); + } + usb_tx_response_is_not_ready = false; + usb_tx_status_is_not_ready = false; + usb_tx_stdout_is_not_ready = false; + pbdrv_usb_is_events_subscribed = false; + // Resend status flags when host subscribes + prev_status_flags = ~0; + } + } else if (!was_transmitting && is_transmitting) { + pbio_os_timer_set(&tx_timeout_timer, 5); + } + was_transmitting = is_transmitting; + + // Make this process loop run from the beginning. + // Because we check so many different conditions, + // we don't use the normal await macros. + PBIO_OS_ASYNC_RESET(state); + return PBIO_ERROR_AGAIN; } PBIO_OS_ASYNC_END(PBIO_SUCCESS); @@ -868,7 +1026,6 @@ void pbdrv_usb_init(void) { ~CFGCHIP2_OTGMODE & ~CFGCHIP2_PHYPWRDN & // Make sure PHY is on ~CFGCHIP2_OTGPWRDN) | // Make sure OTG subsystem is on - CFGCHIP2_FORCE_DEVICE | // We only ever want device operation CFGCHIP2_DATPOL | // Data lines are *not* inverted CFGCHIP2_SESENDEN | // Enable various analog comparators CFGCHIP2_VBDTCTEN; @@ -901,6 +1058,7 @@ void pbdrv_usb_init(void) { // Enable the interrupts we actually care about HWREG(USB_0_OTGBASE + USB_0_INTR_MASK_SET) = + USBOTG_INTR_DISCON | USBOTG_INTR_RESET | USBOTG_INTR_EP1_OUT | USBOTG_INTR_EP1_IN | @@ -926,26 +1084,74 @@ pbdrv_usb_bcd_t pbdrv_usb_get_bcd(void) { return PBDRV_USB_BCD_NONE; } -// TODO: Reimplement the following functions as appropriate -// (mphalport.c and the "host" layer are currently being refactored) -uint32_t pbdrv_usb_write(const uint8_t *data, uint32_t size) { - // Return the size requested so that caller doesn't block. - return size; +bool pbdrv_usb_connection_is_active(void) { + return pbdrv_usb_is_events_subscribed; } -uint32_t pbdrv_usb_rx_data_available(void) { - return 0; -} +/** + * Queues data to be transmitted via USB. + * @param data [in] The data to be sent. + * @param size [in, out] The size of @p data in bytes. After return, @p size + * contains the number of bytes actually written. + * @return ::PBIO_SUCCESS if some @p data was queued, ::PBIO_ERROR_AGAIN + * if no @p data could not be queued at this time (e.g. buffer + * is full), ::PBIO_ERROR_INVALID_OP if there is not an + * active USB connection + */ +pbio_error_t pbdrv_usb_stdout_tx(const uint8_t *data, uint32_t *size) { + if (!pbdrv_usb_is_events_subscribed) { + // If the app hasn't subscribed to events, we can't send stdout. + return PBIO_ERROR_INVALID_OP; + } + + uint8_t *ptr = ep1_tx_stdout_buf; + uint32_t ptr_len = pbdrv_usb_is_usb_hs ? PYBRICKS_EP_PKT_SZ_HS : PYBRICKS_EP_PKT_SZ_FS; + + if (usb_tx_stdout_is_not_ready) { + *size = 0; + return PBIO_ERROR_AGAIN; + } + + *ptr++ = PBIO_PYBRICKS_IN_EP_MSG_EVENT; + ptr_len--; + + *ptr++ = PBIO_PYBRICKS_EVENT_WRITE_STDOUT; + ptr_len--; + + if (*size > ptr_len) { + *size = ptr_len; + } + memcpy(ptr, data, *size); + + usb_tx_stdout_is_not_ready = true; + usb_setup_tx_dma_desc(CPPI_DESC_TX_STDOUT, ep1_tx_stdout_buf, 2 + *size); -int32_t pbdrv_usb_get_char(void) { - return -1; + return PBIO_SUCCESS; } -void pbdrv_usb_tx_flush(void) { +// REVISIT: These two functions do not keep track of how much data +// is held in the DMA engine or the packet FIFO (i.e. the DMA engine +// has returned the descriptor to us (and thus we can queue more), +// but the hardware is still buffering data which we cannot see without +// performing much more accurate accounting) +uint32_t pbdrv_usb_stdout_tx_available(void) { + if (!pbdrv_usb_is_events_subscribed) { + return UINT32_MAX; + } + + if (usb_tx_stdout_is_not_ready) { + return 0; + } + + // Subtract 2 bytes for header + if (pbdrv_usb_is_usb_hs) { + return PYBRICKS_EP_PKT_SZ_HS - 2; + } + return PYBRICKS_EP_PKT_SZ_FS - 2; } -bool pbdrv_usb_connection_is_active(void) { - return false; +bool pbdrv_usb_stdout_tx_is_idle(void) { + return !usb_tx_stdout_is_not_ready; } #endif // PBDRV_CONFIG_USB_EV3 diff --git a/lib/pbio/drv/usb/usb_nxt.c b/lib/pbio/drv/usb/usb_nxt.c index bc81e5b0e..48ad6877b 100644 --- a/lib/pbio/drv/usb/usb_nxt.c +++ b/lib/pbio/drv/usb/usb_nxt.c @@ -549,7 +549,8 @@ static void pbdrv_usb_nxt_handle_class_request(pbdrv_usb_nxt_setup_packet_t *pac case USB_BMREQUEST_RCPT_INT: // Ignoring wIndex for now as we only have one interface. switch (packet->request) { - case 0x01: // Standard GATT characteristic + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT: + // Standard GATT characteristic switch (packet->value) { case 0x2A00: { // device name const char *name = pbdrv_bluetooth_get_hub_name(); @@ -574,7 +575,8 @@ static void pbdrv_usb_nxt_handle_class_request(pbdrv_usb_nxt_setup_packet_t *pac break; } break; - case 0x02: // Pybricks characteristic + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: + // Pybricks characteristic switch (packet->value) { case 0x0003: { // hub capabilities uint8_t caps[PBIO_PYBRICKS_HUB_CAPABILITIES_VALUE_SIZE]; diff --git a/lib/pbio/drv/usb/usb_stm32.c b/lib/pbio/drv/usb/usb_stm32.c index 5a4b0c950..d97707abf 100644 --- a/lib/pbio/drv/usb/usb_stm32.c +++ b/lib/pbio/drv/usb/usb_stm32.c @@ -35,8 +35,8 @@ PROCESS(pbdrv_usb_process, "USB"); // These buffers need to be 32-bit aligned because the USB driver moves data // to/from FIFOs in 32-bit chunks. static uint8_t usb_in_buf[USBD_PYBRICKS_MAX_PACKET_SIZE] __aligned(4); -static uint8_t usb_response_buf[1 + sizeof(uint32_t)] __aligned(4); -static uint8_t usb_status_buf[1 + PBSYS_STATUS_REPORT_SIZE] __aligned(4); +static uint8_t usb_response_buf[PBIO_PYBRICKS_USB_MESSAGE_SIZE(sizeof(uint32_t))] __aligned(4); +static uint8_t usb_status_buf[PBIO_PYBRICKS_USB_MESSAGE_SIZE(PBSYS_STATUS_REPORT_SIZE)] __aligned(4); static uint8_t usb_stdout_buf[USBD_PYBRICKS_MAX_PACKET_SIZE] __aligned(4); static volatile uint32_t usb_in_sz; static volatile uint32_t usb_response_sz; @@ -174,7 +174,7 @@ pbio_error_t pbdrv_usb_stdout_tx(const uint8_t *data, uint32_t *size) { return PBIO_ERROR_AGAIN; } - *ptr++ = USBD_PYBRICKS_IN_EP_MSG_EVENT; + *ptr++ = PBIO_PYBRICKS_IN_EP_MSG_EVENT; ptr_len--; *ptr++ = PBIO_PYBRICKS_EVENT_WRITE_STDOUT; @@ -292,14 +292,11 @@ static USBD_StatusTypeDef Pybricks_Itf_TransmitCplt(uint8_t *Buf, uint32_t Len, return ret; } -#define USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_GATT 0x01 -#define USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_PYBRICKS 0x02 - static USBD_StatusTypeDef Pybricks_Itf_ReadCharacteristic(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) { USBD_StatusTypeDef ret = USBD_OK; switch (req->bRequest) { - case USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_GATT: + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT: switch (req->wValue) { case 0x2A00: { // GATT Device Name characteristic @@ -328,7 +325,7 @@ static USBD_StatusTypeDef Pybricks_Itf_ReadCharacteristic(USBD_HandleTypeDef *pd break; } break; - case USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: switch (req->wValue) { case 0x0003: { // Pybricks hub capabilities characteristic @@ -455,16 +452,16 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) { if (usb_in_sz) { switch (usb_in_buf[0]) { - case USBD_PYBRICKS_OUT_EP_MSG_SUBSCRIBE: + case PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE: pbdrv_usb_stm32_is_events_subscribed = usb_in_buf[1]; - usb_response_buf[0] = USBD_PYBRICKS_IN_EP_MSG_RESPONSE; + usb_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE; pbio_set_uint32_le(&usb_response_buf[1], PBIO_PYBRICKS_ERROR_OK); usb_response_sz = sizeof(usb_response_buf); break; - case USBD_PYBRICKS_OUT_EP_MSG_COMMAND: + case PBIO_PYBRICKS_OUT_EP_MSG_COMMAND: if (usb_response_sz == 0) { result = pbsys_command(usb_in_buf + 1, usb_in_sz - 1); - usb_response_buf[0] = USBD_PYBRICKS_IN_EP_MSG_RESPONSE; + usb_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE; pbio_set_uint32_le(&usb_response_buf[1], result); usb_response_sz = sizeof(usb_response_buf); } @@ -495,10 +492,10 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) { USBD_Pybricks_TransmitPacket(&husbd, usb_response_buf, usb_response_sz); } else if (pbdrv_usb_stm32_is_events_subscribed) { if ((new_status_flags != prev_status_flags) || etimer_expired(&status_timer)) { - usb_status_buf[0] = USBD_PYBRICKS_IN_EP_MSG_EVENT; + usb_status_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_EVENT; _Static_assert(sizeof(usb_status_buf) + 1 >= PBIO_PYBRICKS_EVENT_STATUS_REPORT_SIZE, "size of status report does not match size of event"); - usb_status_sz = 1 + pbsys_status_get_status_report(&usb_status_buf[1]); + usb_status_sz = PBIO_PYBRICKS_USB_MESSAGE_SIZE(pbsys_status_get_status_report(&usb_status_buf[1])); // REVISIT: we really shouldn't need a status timer on USB since // it's not a lossy transport. We just need to make sure we send diff --git a/lib/pbio/include/pbdrv/ioport.h b/lib/pbio/include/pbdrv/ioport.h index cbfe5390d..c4dd749a7 100644 --- a/lib/pbio/include/pbdrv/ioport.h +++ b/lib/pbio/include/pbdrv/ioport.h @@ -118,12 +118,17 @@ typedef struct { uint32_t supported_modes; } pbdrv_ioport_platform_data_t; +#if PBDRV_CONFIG_HAS_PORT_VCC_CONTROL /** * Enables or disables VCC on pin 4 of all ioports. * * @param [in] enable Choose true to enable VCC, false to disable. */ void pbdrv_ioport_enable_vcc(bool enable); +#else +static inline void pbdrv_ioport_enable_vcc(bool enable) { +} +#endif // PBDRV_CONFIG_HAS_PORT_VCC_CONTROL /** * Sets the mode of the P5P6 pins on this port. @@ -143,7 +148,9 @@ pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_u extern const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPORT_NUM_DEV]; +#if PBDRV_CONFIG_HAS_PORT_VCC_CONTROL extern const pbdrv_gpio_t pbdrv_ioport_platform_data_vcc_pin; +#endif // PBDRV_CONFIG_HAS_PORT_VCC_CONTROL #endif // PBDRV_IOPORT_H diff --git a/lib/pbio/include/pbio/protocol.h b/lib/pbio/include/pbio/protocol.h index 95f661947..992301c34 100644 --- a/lib/pbio/include/pbio/protocol.h +++ b/lib/pbio/include/pbio/protocol.h @@ -427,6 +427,44 @@ extern const uint8_t pbio_nus_tx_char_uuid[]; /** USB bDeviceProtocol for Pybricks hubs */ #define PBIO_PYBRICKS_USB_DEVICE_PROTOCOL 0xF5 +/** USB bRequest for Pybricks class-specific requests */ +enum { + /** Retrieve GATT characteristics */ + PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT = 0x01, + /** Retrieve Pybricks characteristics */ + PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS = 0x02, +}; + +// NOTE: These enums values are sent over the wire, so cannot be changed. Also, +// 0 is skipped to avoid a zeroed buffer from being misinterpreted as a message. + +/** Hub to host messages via the Pybricks interface IN endpoint. */ +enum { + /** + * Analog of BLE status response. Emitted in response to every OUT message + * received. + */ + PBIO_PYBRICKS_IN_EP_MSG_RESPONSE = 1, + /**Analog to BLE notification. Only emitted if subscribed. */ + PBIO_PYBRICKS_IN_EP_MSG_EVENT = 2, +}; + +/** Host to hub messages via the Pybricks USB interface OUT endpoint. */ +enum { + /** Analog of BLE Client Characteristic Configuration Descriptor (CCCD). */ + PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE = 1, + /** Analog of BLE Client Characteristic Write with response. */ + PBIO_PYBRICKS_OUT_EP_MSG_COMMAND = 2, +}; + +/** + * Size of USB messages for Pybricks USB interface. + * + * USB has one extra byte header for a message type discriminator + * compared to BLE messages. + */ +#define PBIO_PYBRICKS_USB_MESSAGE_SIZE(n) (1 + n) + #endif // _PBIO_PROTOCOL_H_ /** @} */ diff --git a/lib/pbio/platform/city_hub/pbdrvconfig.h b/lib/pbio/platform/city_hub/pbdrvconfig.h index 7d6a99a5d..82daaefda 100644 --- a/lib/pbio/platform/city_hub/pbdrvconfig.h +++ b/lib/pbio/platform/city_hub/pbdrvconfig.h @@ -82,6 +82,7 @@ #define PBDRV_CONFIG_HAS_PORT_A (1) #define PBDRV_CONFIG_HAS_PORT_B (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 48000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/essential_hub/pbdrvconfig.h b/lib/pbio/platform/essential_hub/pbdrvconfig.h index 3930106ed..fef78a134 100644 --- a/lib/pbio/platform/essential_hub/pbdrvconfig.h +++ b/lib/pbio/platform/essential_hub/pbdrvconfig.h @@ -113,6 +113,7 @@ #define PBDRV_CONFIG_HAS_PORT_A (1) #define PBDRV_CONFIG_HAS_PORT_B (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 96000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/ev3/exceptionhandler.S b/lib/pbio/platform/ev3/exceptionhandler.S index 14ee6eb30..147c13370 100644 --- a/lib/pbio/platform/ev3/exceptionhandler.S +++ b/lib/pbio/platform/ev3/exceptionhandler.S @@ -1,5 +1,7 @@ @ SPDX-License-Identifier: MPL-1.0 @ Copyright (c) 2016 Tobias Schießl +#include "exceptionhandler.h" + .set MODE_USR, 0x10 .set MODE_FIQ, 0x11 .set MODE_IRQ, 0x12 @@ -12,10 +14,6 @@ .equ I_F_BIT, 0xC0 .equ MASK_SWI_NUM, 0xFF000000 .equ MASK_SR_MODE, 0x1F - .equ EXCEPT_TYPE_UNKNOWN, 0 - .equ EXCEPT_TYPE_UNDEF, 1 - .equ EXCEPT_TYPE_PREFETCH_ABORT, 2 - .equ EXCEPT_TYPE_DATA_ABORT, 3 @ This will be placed at 0xffff0000 by the linker script .section .vector,"ax",%progbits @@ -35,25 +33,25 @@ ResetToEntry: UndefHandlder: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_UNDEF + mov r7, #EV3_PANIC_UNDEFINED_INSTR b CommonPanicHandler PrefetchAbortHandler: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_PREFETCH_ABORT + mov r7, #EV3_PANIC_PREFETCH_ABORT b CommonPanicHandler DataAbortHandler: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_DATA_ABORT + mov r7, #EV3_PANIC_DATA_ABORT b CommonPanicHandler UnknownExceptionHandler: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_UNKNOWN + mov r7, #EV3_PANIC_UNKNOWN @ b CommonPanicHandler @ fall through diff --git a/lib/pbio/platform/ev3/exceptionhandler.h b/lib/pbio/platform/ev3/exceptionhandler.h new file mode 100644 index 000000000..bf1b58e5e --- /dev/null +++ b/lib/pbio/platform/ev3/exceptionhandler.h @@ -0,0 +1,9 @@ +#ifndef PBIO_EV3_EXCEPTIONHANDLER_H_ +#define PBIO_EV3_EXCEPTIONHANDLER_H_ + +#define EV3_PANIC_UNKNOWN 0 +#define EV3_PANIC_UNDEFINED_INSTR 1 +#define EV3_PANIC_PREFETCH_ABORT 2 +#define EV3_PANIC_DATA_ABORT 3 + +#endif // PBIO_EV3_EXCEPTIONHANDLER_H_ diff --git a/lib/pbio/platform/ev3/platform.c b/lib/pbio/platform/ev3/platform.c index 9fe6cb51f..c89c888b7 100644 --- a/lib/pbio/platform/ev3/platform.c +++ b/lib/pbio/platform/ev3/platform.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -54,9 +55,12 @@ #include +#include #include #include +#include "exceptionhandler.h" + #include "../../drv/block_device/block_device_ev3.h" #include "../../drv/button/button_gpio.h" #include "../../drv/display/display_ev3.h" @@ -253,12 +257,6 @@ const pbdrv_uart_ev3_platform_data_t }, }; -// TODO. -const pbdrv_gpio_t pbdrv_ioport_platform_data_vcc_pin = { - .bank = NULL, - .pin = 0, -}; - const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPORT_NUM_DEV] = { { .port_id = PBIO_PORT_ID_A, @@ -421,6 +419,7 @@ static const char *const panic_types[] = { "Prefetch Abort", "Data Abort", }; + typedef struct { uint32_t r13; uint32_t r14; @@ -485,10 +484,27 @@ void ev3_panic_handler(int except_type, ev3_panic_ctx *except_data) { panic_puts("\r\nR14: 0x"); panic_putu32(except_data->r14); panic_puts("\r\nR15: 0x"); - panic_putu32(except_data->exc_lr); + switch (except_type) { + case EV3_PANIC_PREFETCH_ABORT: + panic_putu32(except_data->exc_lr - 4); + break; + case EV3_PANIC_DATA_ABORT: + panic_putu32(except_data->exc_lr - 8); + break; + default: + panic_putu32(except_data->exc_lr); + break; + } panic_puts("\r\nSPSR: 0x"); panic_putu32(except_data->spsr); + panic_puts("\r\nDFSR: 0x"); + panic_putu32(CP15GetDFSR()); + panic_puts("\r\nIFSR: 0x"); + panic_putu32(CP15GetIFSR()); + panic_puts("\r\nFAR: 0x"); + panic_putu32(CP15GetFAR()); + panic_puts("\r\nSystem will now reboot...\r\n"); // Poke the watchdog timer with a bad value to immediately trigger it @@ -633,6 +649,103 @@ static void Edma3CCErrHandlerIsr(void) { } } +#define MMU_SECTION_SHIFT 20 +#define MMU_SECTION_SZ (1 << MMU_SECTION_SHIFT) +#define MMU_L1_ENTS (1 << (32 - 20)) +#define MMU_L1_ALIGN (16 * 1024) +#define MMU_L1_SECTION(addr, ap, domain, c, b) ( \ + ((addr) & ~(MMU_SECTION_SZ - 1)) | \ + (((ap) & 3) << 10) | \ + (((domain) & 0xf) << 5) | \ + (1 << 4) | \ + ((!!(c)) << 3) | \ + ((!!(b)) << 2) | \ + (1 << 1) \ + ) +static uint32_t l1_page_table[MMU_L1_ENTS] __attribute__((aligned(MMU_L1_ALIGN))); +#define SYSTEM_RAM_SZ_MB 64 + +static void mmu_tlb_lock(uint32_t addr) { + uint32_t tmp; + __asm__ volatile ( + "mrc p15, 0, %0, c10, c0, 0\n" // read lockdown register + "orr %0, #1\n" // set P bit + "mcr p15, 0, %0, c10, c0, 0\n" // write lockdown register + "ldr %0, [%1]\n" // force a TLB load + "mrc p15, 0, %0, c10, c0, 0\n" // read lockdown register + "bic %0, #1\n" // clear P bit + "mcr p15, 0, %0, c10, c0, 0\n" // write lockdown register + : "=&r" (tmp) + : "r" (addr) + ); +} + +static void mmu_init(void) { + // Invalidate TLB + CP15InvTLB(); + + // Program domain D0 = no access, D1 = manager (no permission checks) + // We generally don't bother with permission checks, anything valid is RWX + CP15DomainAccessSet(0b1100); + + // For simplicity, everything is mapped as sections (1 MiB chunks) + // This potentially fails to catch certain out-of-bounds accesses, + // but as a tradeoff we do not need any L2 sections. + + // MMIO register ranges + l1_page_table[0x01C00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01C00000, 0, 1, 0, 0); + l1_page_table[0x01D00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01D00000, 0, 1, 0, 0); + l1_page_table[0x01E00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01E00000, 0, 1, 0, 0); + l1_page_table[0x01F00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01F00000, 0, 1, 0, 0); + + // On-chip RAM, which is used to share control structures with the PRUs + l1_page_table[0x80000000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x80000000, 0, 1, 0, 0); + + // Off-chip main DDR RAM + for (unsigned int i = 0; i < SYSTEM_RAM_SZ_MB; i++) { + uint32_t addr = 0xC0000000 + i * MMU_SECTION_SZ; + // TODO: Enable caching once DMA code is upgraded to handle cache + l1_page_table[addr >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(addr, 0, 1, 0, 0); + } + // Off-chip main DDR RAM, uncacheable mirror @ 0xD0000000 + for (unsigned int i = 0; i < SYSTEM_RAM_SZ_MB; i++) { + uint32_t addr_phys = 0xC0000000 + i * MMU_SECTION_SZ; + uint32_t addr_virt = 0xD0000000 + i * MMU_SECTION_SZ; + l1_page_table[addr_virt >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(addr_phys, 0, 1, 0, 0); + } + + // ARM local RAM, interrupt controller + l1_page_table[0xFFF00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0xFFF00000, 0, 1, 0, 0); + + // Make sure this all makes its way into memory + pbdrv_compiler_memory_barrier(); + + // Set TTBR + CP15TtbSet((uint32_t)l1_page_table); + + uint32_t c15_control = CP15ControlGet(); + // Clear R and S protection bits (even though we don't use them) + c15_control &= ~((1 << 9) | (1 << 8)); + // Enable I-cache, D-cache, alignment faults, and MMU + c15_control |= (1 << 12) | (1 << 2) | (1 << 1) | (1 << 0); + CP15ControlSet(c15_control); + + // Set victim field in TLB lockdown register to 0 + __asm__ volatile ( + "movs r0, #0\n" + "mcr p15, 0, r0, c10, c0, 0" + ::: "r0" + ); + // Lock all the TLB entries other than main DDR RAM + // This helps improve real-time performance as we will never TLB miss on them + mmu_tlb_lock(0x01C00000); + mmu_tlb_lock(0x01D00000); + mmu_tlb_lock(0x01E00000); + mmu_tlb_lock(0x01F00000); + mmu_tlb_lock(0x80000000); + mmu_tlb_lock(0xFFF00000); +} + enum { BOOT_EEPROM_I2C_ADDRESS = 0x50, }; @@ -643,6 +756,7 @@ uint8_t pbdrv_ev3_bluetooth_mac_address[6]; // initialization (low level in pbdrv, high level in pbio), and system level // functions for running user code (currently a hardcoded MicroPython script). void SystemInit(void) { + mmu_init(); SysCfgRegistersUnlock(); diff --git a/lib/pbio/platform/ev3/start.S b/lib/pbio/platform/ev3/start.S index 1673fbf06..c9e721787 100755 --- a/lib/pbio/platform/ev3/start.S +++ b/lib/pbio/platform/ev3/start.S @@ -58,6 +58,7 @@ Entry: mrc p15, #0, r0, c1, c0,#0 @ Read System Control Register bic r0, r0, #0x1000 @ Clear bit 12 to disable ICache bic r0, r0, #0x0004 @ Clear bit 2 to disable DCache + bic r0, r0, #0x0001 @ Clear bit 0 to disable MMU mcr p15, #0, r0, c1, c0, #0 @ Write back to System Control Register mov r0, #0 @ Set r0 to 0 mcr p15, #0, r0, c7, c7, #0 @ Invalidate all caches diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index 9be8dece7..4f9d0155d 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -80,6 +80,7 @@ #define PBDRV_CONFIG_HAS_PORT_B (1) #define PBDRV_CONFIG_HAS_PORT_C (1) #define PBDRV_CONFIG_HAS_PORT_D (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 48000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/nxt/pbdrvconfig.h b/lib/pbio/platform/nxt/pbdrvconfig.h index 04d0e78ed..0bb64dad2 100644 --- a/lib/pbio/platform/nxt/pbdrvconfig.h +++ b/lib/pbio/platform/nxt/pbdrvconfig.h @@ -35,6 +35,7 @@ #define PBDRV_CONFIG_HAS_PORT_2 (1) #define PBDRV_CONFIG_HAS_PORT_3 (1) #define PBDRV_CONFIG_HAS_PORT_4 (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_RESET (1) #define PBDRV_CONFIG_RESET_NXT (1) diff --git a/lib/pbio/platform/prime_hub/pbdrvconfig.h b/lib/pbio/platform/prime_hub/pbdrvconfig.h index cece15f76..5f65324c8 100644 --- a/lib/pbio/platform/prime_hub/pbdrvconfig.h +++ b/lib/pbio/platform/prime_hub/pbdrvconfig.h @@ -137,6 +137,7 @@ #define PBDRV_CONFIG_HAS_PORT_D (1) #define PBDRV_CONFIG_HAS_PORT_E (1) #define PBDRV_CONFIG_HAS_PORT_F (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 96000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/technic_hub/pbdrvconfig.h b/lib/pbio/platform/technic_hub/pbdrvconfig.h index e0e90a48f..0a63e9480 100644 --- a/lib/pbio/platform/technic_hub/pbdrvconfig.h +++ b/lib/pbio/platform/technic_hub/pbdrvconfig.h @@ -94,6 +94,7 @@ #define PBDRV_CONFIG_HAS_PORT_B (1) #define PBDRV_CONFIG_HAS_PORT_C (1) #define PBDRV_CONFIG_HAS_PORT_D (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 80000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/test/pbdrvconfig.h b/lib/pbio/platform/test/pbdrvconfig.h index 798aa09d0..0d11040e2 100644 --- a/lib/pbio/platform/test/pbdrvconfig.h +++ b/lib/pbio/platform/test/pbdrvconfig.h @@ -44,3 +44,4 @@ #define PBDRV_CONFIG_HAS_PORT_D (1) #define PBDRV_CONFIG_HAS_PORT_E (1) #define PBDRV_CONFIG_HAS_PORT_F (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) diff --git a/lib/pbio/platform/virtual_hub/pbdrvconfig.h b/lib/pbio/platform/virtual_hub/pbdrvconfig.h index 029e601af..6cbc45b38 100644 --- a/lib/pbio/platform/virtual_hub/pbdrvconfig.h +++ b/lib/pbio/platform/virtual_hub/pbdrvconfig.h @@ -29,3 +29,4 @@ #define PBDRV_CONFIG_HAS_PORT_D (1) #define PBDRV_CONFIG_HAS_PORT_E (1) #define PBDRV_CONFIG_HAS_PORT_F (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) diff --git a/lib/pbio/src/port.c b/lib/pbio/src/port.c index f6655bd6c..c34b7bddf 100644 --- a/lib/pbio/src/port.c +++ b/lib/pbio/src/port.c @@ -447,7 +447,9 @@ void pbio_port_stop_user_actions(bool reset) { } // Stops and resets motors. Also stops higher level controls like servo. - pbio_dcmotor_reset(port->dcmotor, reset); + if (port->dcmotor) { + pbio_dcmotor_reset(port->dcmotor, reset); + } } } diff --git a/lib/tiam1808/system_config/armv5/gcc/cp15.c b/lib/tiam1808/system_config/armv5/gcc/cp15.c index b700a4229..39d5f28dc 100644 --- a/lib/tiam1808/system_config/armv5/gcc/cp15.c +++ b/lib/tiam1808/system_config/armv5/gcc/cp15.c @@ -37,6 +37,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include + /****************************************************************************** ** FUNCTION DEFINITIONS ******************************************************************************/ @@ -218,21 +220,10 @@ void CP15DCacheCleanBuff(unsigned int bufPtr, unsigned int size) **/ void CP15TtbSet(unsigned int ttb) { - /* Invalidates all TLBs.Domain access is selected as - * client by configuring domain access register, - * in that case access controlled by permission value - * set by page table entry - */ - __asm(" mov r1, #0\n\t" - " mcr p15, #0, r1, c8, c7, #0\n\t" - " ldr r1, =0x55555555\n\t" - " mcr p15, #0, r1, c3, c0, #0\n\t"); - /* sets translation table base resgister with page table * starting address. */ __asm(" mcr p15, #0, %[value], c2, c0, 0":: [value] "r" (ttb)); - } /** @@ -268,4 +259,64 @@ void CP15MMUEnable(void) " mcr p15, #0, r0, c1, c0, #0\n\t"); } +/** + * \brief This function gets the data fault status register + */ +uint32_t CP15GetDFSR(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c5, c0, 0" : "=r" (ret)); + return ret; +} + +/** + * \brief This function gets the instruction fault status register + */ +uint32_t CP15GetIFSR(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c5, c0, 1" : "=r" (ret)); + return ret; +} + +/** + * \brief This function gets the fault address register + */ +uint32_t CP15GetFAR(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c6, c0, 0" : "=r" (ret)); + return ret; +} + +/** + * \brief This function invalidates the TLB + */ +void CP15InvTLB(void) +{ + __asm(" mov r0, #0\n\t" + " mcr p15, #0, r0, c8, c7, #0" + ::: "r0"); +} + +/** + * \brief This function programs the domain access control register + */ +void CP15DomainAccessSet(uint32_t domains) +{ + __asm("mcr p15, 0, %0, c3, c0, 0" :: "r"(domains)); +} + +uint32_t CP15ControlGet(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c1, c0, 0" : "=r" (ret)); + return ret; +} + +void CP15ControlSet(uint32_t control) +{ + __asm("mcr p15, 0, %0, c1, c0, 0" :: "r" (control)); +} + /********************************* End Of File *******************************/ diff --git a/lib/tiam1808/tiam1808/armv5/cp15.h b/lib/tiam1808/tiam1808/armv5/cp15.h index 84ba4110e..847e0cf69 100644 --- a/lib/tiam1808/tiam1808/armv5/cp15.h +++ b/lib/tiam1808/tiam1808/armv5/cp15.h @@ -41,6 +41,8 @@ #ifndef __CP15_H #define __CP15_H +#include + #ifdef __cplusplus extern "C" { #endif @@ -60,6 +62,13 @@ extern void CP15MMUDisable(void); extern void CP15MMUEnable(void); extern void CP15ICacheFlushBuff(unsigned int ptr, unsigned int size); extern void CP15DCacheCleanBuff(unsigned int bufPtr, unsigned int size); +extern uint32_t CP15GetDFSR(void); +extern uint32_t CP15GetIFSR(void); +extern uint32_t CP15GetFAR(void); +extern void CP15InvTLB(void); +extern void CP15DomainAccessSet(uint32_t domains); +extern uint32_t CP15ControlGet(void); +extern void CP15ControlSet(uint32_t control); #ifdef __cplusplus }