| /* |
| * Copyright 2017, Data61 |
| * Commonwealth Scientific and Industrial Research Organisation (CSIRO) |
| * ABN 41 687 119 230. |
| * |
| * This software may be distributed and modified according to the terms of |
| * the BSD 2-Clause license. Note that NO WARRANTY is provided. |
| * See "LICENSE_BSD2.txt" for details. |
| * |
| * @TAG(DATA61_BSD) |
| */ |
| #include <autoconf.h> |
| #include <sel4serialserver/gen_config.h> |
| |
| #include <stdio.h> |
| #include <string.h> |
| |
| #include <sel4/sel4.h> |
| #include <vka/vka.h> |
| #include <vka/object.h> |
| #include <vka/capops.h> |
| |
| #include <utils/arith.h> |
| #include <utils/ansi.h> |
| #include <sel4utils/api.h> |
| #include <sel4utils/strerror.h> |
| #include <sel4platsupport/platsupport.h> |
| |
| #include "serial_server.h" |
| #include <serial_server/client.h> |
| #include <serial_server/parent.h> |
| |
| /* Define global instance. */ |
| static serial_server_context_t serial_server; |
| |
| static char *colors[] = { |
| ANSI_COLOR(RED), |
| ANSI_COLOR(GREEN), |
| ANSI_COLOR(YELLOW), |
| ANSI_COLOR(BLUE), |
| ANSI_COLOR(MAGENTA), |
| ANSI_COLOR(CYAN), |
| |
| ANSI_COLOR(RED, BOLD), |
| ANSI_COLOR(GREEN, BOLD), |
| ANSI_COLOR(YELLOW, BOLD), |
| ANSI_COLOR(BLUE, BOLD), |
| ANSI_COLOR(MAGENTA, BOLD), |
| ANSI_COLOR(CYAN, BOLD), |
| |
| ANSI_COLOR(RED, ITALIC), |
| ANSI_COLOR(GREEN, ITALIC), |
| ANSI_COLOR(YELLOW, ITALIC), |
| ANSI_COLOR(BLUE, ITALIC), |
| ANSI_COLOR(MAGENTA, ITALIC), |
| ANSI_COLOR(CYAN, ITALIC), |
| |
| ANSI_COLOR(RED, UNDERLINE), |
| ANSI_COLOR(GREEN, UNDERLINE), |
| ANSI_COLOR(YELLOW, UNDERLINE), |
| ANSI_COLOR(BLUE, UNDERLINE), |
| ANSI_COLOR(MAGENTA, UNDERLINE), |
| ANSI_COLOR(CYAN, UNDERLINE), |
| }; |
| |
| #define NUM_COLORS ARRAY_SIZE(colors) |
| #define BADGE_TO_COLOR(badge) (colors[(badge) % NUM_COLORS]) |
| |
| serial_server_context_t *get_serial_server(void) |
| { |
| return &serial_server; |
| } |
| |
| static inline seL4_MessageInfo_t recv(seL4_Word *sender_badge) |
| { |
| return api_recv(get_serial_server()->server_ep_obj.cptr, sender_badge, get_serial_server()->server_thread.reply.cptr); |
| } |
| |
| static inline void reply(seL4_MessageInfo_t tag) |
| { |
| api_reply(get_serial_server()->server_thread.reply.cptr, tag); |
| } |
| |
| serial_server_registry_entry_t *serial_server_registry_get_entry_by_badge(seL4_Word badge_value) |
| { |
| if (badge_value == SERIAL_SERVER_BADGE_VALUE_EMPTY |
| || get_serial_server()->registry == NULL |
| || badge_value > get_serial_server()->registry_n_entries) { |
| return NULL; |
| } |
| /* If the badge value has been released, return NULL. */ |
| if (get_serial_server()->registry[badge_value - 1].badge_value |
| == SERIAL_SERVER_BADGE_VALUE_EMPTY) { |
| return NULL; |
| } |
| |
| return &get_serial_server()->registry[badge_value - 1]; |
| } |
| |
| bool serial_server_badge_is_allocated(seL4_Word badge_value) |
| { |
| serial_server_registry_entry_t *tmp; |
| |
| tmp = serial_server_registry_get_entry_by_badge(badge_value); |
| if (tmp == NULL) { |
| return false; |
| } |
| |
| return tmp->badge_value != SERIAL_SERVER_BADGE_VALUE_EMPTY; |
| } |
| |
| seL4_Word serial_server_badge_value_get_unused(void) |
| { |
| if (get_serial_server()->registry == NULL) { |
| return SERIAL_SERVER_BADGE_VALUE_EMPTY; |
| } |
| |
| for (int i = 0; i < get_serial_server()->registry_n_entries; i++) { |
| if (get_serial_server()->registry[i].badge_value != SERIAL_SERVER_BADGE_VALUE_EMPTY) { |
| continue; |
| } |
| |
| /* Badge value 0 will never be allocated, so index 0 is actually |
| * badge 1, and index 1 is badge 2, and so on ad infinitum. |
| */ |
| get_serial_server()->registry[i].badge_value = i + 1; |
| return get_serial_server()->registry[i].badge_value; |
| } |
| |
| return SERIAL_SERVER_BADGE_VALUE_EMPTY; |
| } |
| |
| seL4_Word serial_server_badge_value_alloc(void) |
| { |
| serial_server_registry_entry_t *tmp; |
| seL4_Word ret; |
| |
| ret = serial_server_badge_value_get_unused(); |
| if (ret != SERIAL_SERVER_BADGE_VALUE_EMPTY) { |
| /* Success */ |
| return ret; |
| } |
| |
| tmp = realloc(get_serial_server()->registry, |
| sizeof(*get_serial_server()->registry) |
| * (get_serial_server()->registry_n_entries + 1)); |
| if (tmp == NULL) { |
| ZF_LOGD(SERSERVS"badge_value_alloc: Failed resize pool."); |
| return SERIAL_SERVER_BADGE_VALUE_EMPTY; |
| } |
| |
| get_serial_server()->registry = tmp; |
| get_serial_server()->registry[get_serial_server()->registry_n_entries].badge_value = |
| SERIAL_SERVER_BADGE_VALUE_EMPTY; |
| get_serial_server()->registry_n_entries++; |
| |
| /* If it fails again (some other caller raced us and got the new ID before |
| * we did) that's tough luck -- the caller should probably look into |
| * serializing the calls. |
| */ |
| return serial_server_badge_value_get_unused(); |
| } |
| |
| void serial_server_badge_value_free(seL4_Word badge_value) |
| { |
| serial_server_registry_entry_t *tmp; |
| |
| if (badge_value == SERIAL_SERVER_BADGE_VALUE_EMPTY) { |
| return; |
| } |
| |
| tmp = serial_server_registry_get_entry_by_badge(badge_value); |
| if (tmp == NULL) { |
| return; |
| } |
| |
| tmp->badge_value = SERIAL_SERVER_BADGE_VALUE_EMPTY; |
| } |
| |
| static void serial_server_registry_insert(seL4_Word badge_value, void *shmem, |
| seL4_CPtr *shmem_frame_caps, |
| size_t shmem_size) |
| { |
| serial_server_registry_entry_t *tmp; |
| |
| tmp = serial_server_registry_get_entry_by_badge(badge_value); |
| /* If this is NULL, something went very wrong, because this function is only |
| * called after the server checks to ensure that the badge value has been |
| * allocated in the registry. Technically, this should never happen, but |
| * an assert doesn't hurt. |
| */ |
| assert(tmp != NULL); |
| |
| tmp->badge_value = badge_value; |
| tmp->shmem = shmem; |
| tmp->shmem_size = shmem_size; |
| tmp->shmem_frame_caps = shmem_frame_caps; |
| } |
| |
| static void serial_server_registry_remove(seL4_Word badge_value) |
| { |
| serial_server_registry_entry_t *tmp; |
| |
| tmp = serial_server_registry_get_entry_by_badge(badge_value); |
| if (tmp == NULL) { |
| return; |
| } |
| serial_server_badge_value_free(badge_value); |
| } |
| |
| static void serial_server_set_frame_recv_path(void) |
| { |
| seL4_SetCapReceivePath(get_serial_server()->server_cspace, |
| get_serial_server()->frame_cap_recv_cspaths[0].capPtr, |
| get_serial_server()->frame_cap_recv_cspaths[0].capDepth); |
| } |
| |
| /** Processes all FUNC_CONNECT_REQ IPC messages. Establishes |
| * shared mem mappings with new clients and sets up book-keeping metadata. |
| * |
| * Clients calling connect() will pass us a list of Frame caps which we must |
| * map in order to establish shared mem with those clients. In this function, |
| * the library maps the client's frames into the server's VSpace. |
| */ |
| seL4_Error serial_server_func_connect(seL4_MessageInfo_t tag, |
| seL4_Word client_badge_value, |
| size_t client_shmem_size) |
| { |
| seL4_Error error; |
| size_t client_shmem_n_pages; |
| void *shmem_tmp = NULL; |
| seL4_CPtr *client_frame_caps = NULL; |
| cspacepath_t client_frame_cspath_tmp; |
| |
| if (client_shmem_size == 0) { |
| ZF_LOGW(SERSERVS"connect: Invalid shared mem window size of 0B.\n"); |
| return seL4_InvalidArgument; |
| } |
| |
| client_shmem_n_pages = BYTES_TO_4K_PAGES(client_shmem_size); |
| /* The client should be allocated a badge value by the Parent, before it |
| * attempts to connect to the Server. |
| * |
| * The reason being that when the badge is allocated, the metadata array |
| * is resized as well, so badge allocation is also metadata allocation. |
| */ |
| if (client_badge_value == SERIAL_SERVER_BADGE_VALUE_EMPTY |
| || !serial_server_badge_is_allocated(client_badge_value)) { |
| ZF_LOGW(SERSERVS"connect: Please allocate a badge value to this new " |
| "client.\n"); |
| return -1; |
| } |
| /* Make sure that the client didn't request a shmem mapping larger than the |
| * server is willing to handle. |
| */ |
| if (client_shmem_n_pages > BYTES_TO_4K_PAGES(get_serial_server()->shmem_max_size)) { |
| /* If the client asks for a shmem mapping too large, we refuse, and |
| * send the value of shmem_max_size in SSMSGREG_RESPONSE |
| * so it can try again. |
| */ |
| ZF_LOGW(SERSERVS"connect: New client badge %x is asking to establish " |
| "shmem mapping of %dB, but server max accepted shmem size is " |
| "%dB.", |
| client_badge_value, client_shmem_size, |
| get_serial_server()->shmem_max_size); |
| return SERIAL_SERVER_ERROR_SHMEM_TOO_LARGE; |
| } |
| |
| if (seL4_MessageInfo_get_extraCaps(tag) != client_shmem_n_pages) { |
| ZF_LOGW(SERSERVS"connect: Received %d Frame caps from client " |
| "badge %x.\n\tbut client requested shmem mapping of %d " |
| "frames. Possible cap transfer error.", |
| seL4_MessageInfo_get_extraCaps(tag), client_badge_value, |
| client_shmem_n_pages); |
| return seL4_InvalidCapability; |
| } |
| |
| /* Prepare an array of the client's shmem Frame caps to be mapped into our |
| * VSpace. |
| */ |
| client_frame_caps = calloc((client_shmem_n_pages + 1), sizeof(seL4_CPtr)); |
| if (client_frame_caps == NULL) { |
| ZF_LOGE(SERSERVS"connect: Failed to alloc frame cap list for client " |
| "shmem."); |
| return seL4_NotEnoughMemory; |
| } |
| for (size_t i = 0; i < client_shmem_n_pages; i++) { |
| /* For each frame, we need to make an seL4_CNode_Copy of it before |
| * we zero-out the receive slots, or else when we delete the receive |
| * slots, the frames will be revoked and unmapped. |
| */ |
| error = vka_cspace_alloc_path(get_serial_server()->server_vka, |
| &client_frame_cspath_tmp); |
| if (error != 0) { |
| ZF_LOGE(SERSERVS"connect: Failed to alloc CSpace slot for frame " |
| "%zd of %zd received from client badge %lx.", |
| i + 1, client_shmem_n_pages, (long)client_badge_value); |
| goto out; |
| } |
| |
| /* Copy the frame cap from the recv slot to the perm slot now. */ |
| error = vka_cnode_move(&client_frame_cspath_tmp, |
| &get_serial_server()->frame_cap_recv_cspaths[i]); |
| if (error != 0) { |
| ZF_LOGE(SERSERVS"connect: Failed to move %zuth frame-cap received " |
| " from client badge %lx.", i + 1, (long)client_badge_value); |
| goto out; |
| } |
| |
| client_frame_caps[i] = client_frame_cspath_tmp.capPtr; |
| ZF_LOGD("connect: moved received client Frame cap %d from recv slot %"PRIxPTR" to slot %"PRIxPTR".", |
| i + 1, get_serial_server()->frame_cap_recv_cspaths[i].capPtr, |
| client_frame_caps[i]); |
| } |
| |
| /* Map the frames into the vspace. */ |
| shmem_tmp = vspace_map_pages(get_serial_server()->server_vspace, client_frame_caps, |
| NULL, |
| seL4_AllRights, client_shmem_n_pages, |
| seL4_PageBits, |
| true); |
| if (shmem_tmp == NULL) { |
| ZF_LOGE(SERSERVS"connect: Failed to map shmem."); |
| error = seL4_NotEnoughMemory; |
| goto out; |
| } |
| |
| serial_server_registry_insert(client_badge_value, shmem_tmp, |
| client_frame_caps, client_shmem_size); |
| |
| ZF_LOGI(SERSERVS"connect: New client: badge %x, shmem %p, %d pages.", |
| client_badge_value, shmem_tmp, client_shmem_n_pages); |
| |
| return seL4_NoError; |
| |
| out: |
| if (shmem_tmp != NULL) { |
| vspace_unmap_pages(get_serial_server()->server_vspace, shmem_tmp, |
| client_shmem_n_pages, seL4_PageBits, |
| VSPACE_PRESERVE); |
| } |
| |
| if (client_frame_caps != NULL) { |
| for (size_t i = 0; i < client_shmem_n_pages; i++) { |
| /* Because client_frame_caps was alloc'd with calloc, we can depend on |
| * the unused slots being filled with 0s. Break on first unallocated. |
| */ |
| if (client_frame_caps[i] == 0) { |
| break; |
| } |
| client_frame_cspath_tmp.capPtr = client_frame_caps[i]; |
| vka_cspace_free_path(get_serial_server()->server_vka, |
| client_frame_cspath_tmp); |
| } |
| } |
| |
| free(client_frame_caps); |
| return error; |
| } |
| |
| static int serial_server_func_write(serial_server_registry_entry_t *client_data, |
| size_t message_len, size_t *bytes_written) |
| { |
| *bytes_written = 0; |
| |
| if (client_data == NULL || bytes_written == NULL) { |
| ZF_LOGE(SERSERVS"printf: Got NULL for required argument."); |
| return seL4_InvalidArgument; |
| } |
| if (message_len > client_data->shmem_size) { |
| return seL4_RangeError; |
| } |
| |
| /* Write out */ |
| if (config_set(CONFIG_SERIAL_SERVER_COLOURED_OUTPUT)) { |
| printf("%s", COLOR_RESET); |
| printf("%s", BADGE_TO_COLOR(client_data->badge_value)); |
| } |
| fwrite((void *)client_data->shmem, message_len, 1, stdout); |
| if (config_set(CONFIG_SERIAL_SERVER_COLOURED_OUTPUT)) { |
| printf("%s", COLOR_RESET); |
| } |
| *bytes_written = message_len; |
| return 0; |
| } |
| |
| static void serial_server_func_disconnect(serial_server_registry_entry_t *client_data) |
| { |
| /* Tear down shmem and release the badge value for reuse. */ |
| vspace_unmap_pages(get_serial_server()->server_vspace, |
| (void *)client_data->shmem, |
| BYTES_TO_4K_PAGES(client_data->shmem_size), |
| seL4_PageBits, get_serial_server()->server_vka); |
| free(client_data->shmem_frame_caps); |
| serial_server_registry_remove(client_data->badge_value); |
| } |
| |
| static void serial_server_func_kill(void) |
| { |
| /* Tear down all existing connections. */ |
| for (int i = 0; i < get_serial_server()->registry_n_entries; i++) { |
| serial_server_registry_entry_t *curr = &get_serial_server()->registry[i]; |
| |
| if (curr->badge_value == SERIAL_SERVER_BADGE_VALUE_EMPTY |
| || BYTES_TO_4K_PAGES(curr->shmem_size) == 0) { |
| continue; |
| } |
| |
| serial_server_func_disconnect(&get_serial_server()->registry[i]); |
| } |
| } |
| |
| /** Debugging function -- prints out the state of the registry and the server's |
| * current list of clients. |
| * |
| * Can either print shallow or deep. |
| * @param dump_frame_caps If true, print the array of frames caps that underlie |
| * the shared mem mapping to each client. |
| */ |
| void serial_server_registry_dump(bool dump_frame_caps) |
| { |
| serial_server_registry_entry_t *tmp; |
| |
| if (get_serial_server()->registry == NULL) { |
| ZF_LOGD("Registry is NULL."); |
| } |
| for (int i = 0; i < get_serial_server()->registry_n_entries; i++) { |
| tmp = &get_serial_server()->registry[i]; |
| ZF_LOGD("Reg: idx %d, badge %d, shmem_size %d, shmem %p, caps %p.", |
| i, tmp->badge_value, tmp->shmem_size, tmp->shmem, |
| tmp->shmem_frame_caps); |
| |
| if (!dump_frame_caps) { |
| continue; |
| } |
| |
| for (int j = 0; j < BYTES_TO_4K_PAGES(tmp->shmem_size); j++) { |
| ZF_LOGD("Reg badge %d: frame cap %d: %"PRIxPTR".", |
| tmp->badge_value, j + 1, tmp->shmem_frame_caps[j]); |
| } |
| } |
| } |
| |
| void serial_server_main(void) |
| { |
| seL4_MessageInfo_t tag; |
| seL4_Word sender_badge; |
| enum serial_server_funcs func; |
| int keep_going = 1; |
| UNUSED seL4_Error error; |
| serial_server_registry_entry_t *client_data = NULL; |
| size_t buff_len, bytes_written; |
| |
| /* Bind to the serial driver. */ |
| error = platsupport_serial_setup_simple(get_serial_server()->server_vspace, |
| get_serial_server()->server_simple, |
| get_serial_server()->server_vka); |
| if (error != 0) { |
| ZF_LOGE(SERSERVS"main: Failed to bind to serial."); |
| } else { |
| ZF_LOGI(SERSERVS"main: Bound to the serial driver."); |
| } |
| |
| /* The Parent will seL4_Call() the us, the Server, right after spawning us. |
| * It will expect us to seL4_Reply() with an error status code - we will |
| * send this Reply regardless of the outcome of the platform serial bind |
| * operation. |
| * |
| * First call seL4_Recv() to get the Reply cap back to the Parent, and then |
| * seL4_Reply to report our status. |
| */ |
| recv(&sender_badge); |
| |
| seL4_SetMR(SSMSGREG_FUNC, FUNC_SERVER_SPAWN_SYNC_ACK); |
| tag = seL4_MessageInfo_new(error, 0, 0, SSMSGREG_SPAWN_SYNC_ACK_END); |
| reply(tag); |
| |
| /* If the bind failed, this thread has essentially failed its mandate, so |
| * there is no reason to leave it scheduled. Kill it (to whatever extent |
| * that is possible). |
| */ |
| if (error != 0) { |
| seL4_TCB_Suspend(get_serial_server()->server_thread.tcb.cptr); |
| } |
| |
| ZF_LOGI(SERSERVS"main: Entering main loop and accepting requests."); |
| while (keep_going) { |
| /* Set the CNode slots where caps from clients will go */ |
| serial_server_set_frame_recv_path(); |
| |
| tag = recv(&sender_badge); |
| ZF_LOGD(SERSERVS "main: Got message from %x", sender_badge); |
| |
| func = seL4_GetMR(SSMSGREG_FUNC); |
| |
| /* Lookup the registry entry for this sender to make sure that the sender |
| * has a shmem buffer registered with us. If not, ignore the message. |
| * New connection requests are of course, exempt from the requirement to |
| * already have an established connection. |
| */ |
| if (func != FUNC_CONNECT_REQ) { |
| client_data = serial_server_registry_get_entry_by_badge(sender_badge); |
| if (client_data == NULL) { |
| ZF_LOGW(SERSERVS"main: Got message from unregistered client " |
| "badge %x. Ignoring.", |
| sender_badge); |
| continue; |
| } |
| } |
| |
| switch (func) { |
| case FUNC_CONNECT_REQ: |
| ZF_LOGD(SERSERVS"main: Got connect request from client badge %x.", |
| sender_badge); |
| error = serial_server_func_connect(tag, |
| sender_badge, |
| seL4_GetMR(SSMSGREG_CONNECT_REQ_SHMEM_SIZE)); |
| |
| seL4_SetMR(SSMSGREG_FUNC, FUNC_CONNECT_ACK); |
| seL4_SetMR(SSMSGREG_CONNECT_ACK_MAX_SHMEM_SIZE, |
| get_serial_server()->shmem_max_size); |
| tag = seL4_MessageInfo_new(error, 0, 0, SSMSGREG_CONNECT_ACK_END); |
| reply(tag); |
| break; |
| |
| case FUNC_WRITE_REQ: |
| /* The Server's ABI for the write() request is as follows: |
| * |
| * The server returns the number of bytes it actually wrote out to the |
| * serial in a msg-reg (SSMSGREG_WRITE_ACK_N_BYTES_WRITTEN), |
| * aside from also returning an error code in the "label" of the header. |
| * |
| * This was done because attempting to overload the "label" of the |
| * header to hold a value that can be either: |
| * (1) a negative integer error code, (2) a positive size_t |
| * byte length |
| * Was not very clean especially since the bit-width of the "label" |
| * field shouldn't be assumed lightly, so separating the error code |
| * from the number of bytes written was the cleaner approach. |
| * |
| * At the client end, the client can decide to overload an ssize_t |
| * to encode both types of values. |
| */ |
| |
| ZF_LOGD(SERSERVS"main: Got write request from client badge %x.", |
| sender_badge); |
| buff_len = seL4_GetMR(SSMSGREG_WRITE_REQ_BUFF_LEN); |
| error = serial_server_func_write(client_data, buff_len, |
| &bytes_written); |
| |
| seL4_SetMR(SSMSGREG_FUNC, FUNC_WRITE_ACK); |
| seL4_SetMR(SSMSGREG_WRITE_ACK_N_BYTES_WRITTEN, bytes_written); |
| tag = seL4_MessageInfo_new(error, 0, 0, SSMSGREG_WRITE_ACK_END); |
| reply(tag); |
| break; |
| |
| case FUNC_DISCONNECT_REQ: |
| ZF_LOGD(SERSERVS"main: Got disconnect request from client badge %x.", |
| sender_badge); |
| serial_server_func_disconnect(client_data); |
| |
| seL4_SetMR(SSMSGREG_FUNC, FUNC_DISCONNECT_ACK); |
| tag = seL4_MessageInfo_new(error, 0, 0, SSMSGREG_DISCONNECT_ACK_END); |
| reply(tag); |
| break; |
| |
| case FUNC_KILL_REQ: |
| ZF_LOGI(SERSERVS"main: Got KILL request from client badge %x.", |
| sender_badge); |
| /* The actual contents of the Reply don't matter here. */ |
| seL4_SetMR(SSMSGREG_FUNC, FUNC_KILL_ACK); |
| tag = seL4_MessageInfo_new(0, 0, 0, SSMSGREG_KILL_ACK_END); |
| reply(tag); |
| /* Break out of the loop */ |
| keep_going = 0; |
| break; |
| |
| default: |
| ZF_LOGW(SERSERVS "main: Unknown function %d requested.", func); |
| break; |
| } |
| } |
| |
| serial_server_func_kill(); |
| /* After we break out of the loop, seL4_TCB_Suspend ourselves */ |
| ZF_LOGI(SERSERVS"main: Suspending."); |
| seL4_TCB_Suspend(get_serial_server()->server_thread.tcb.cptr); |
| } |
