| /* | |
| * 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); | |
| } |