// Basic infrastructure commands. // // Copyright (C) 2016-2020 Kevin O'Connor // // This file may be distributed under the terms of the GNU GPLv3 license. #include // memset #include "basecmd.h" // oid_lookup #include "board/irq.h" // irq_save #include "board/misc.h" // alloc_maxsize #include "board/pgm.h" // READP #include "command.h" // DECL_COMMAND #include "sched.h" // sched_clear_shutdown /**************************************************************** * Low level allocation ****************************************************************/ static void *alloc_end; void alloc_init(void) { alloc_end = (void*)ALIGN((size_t)dynmem_start(), __alignof__(void*)); } DECL_INIT(alloc_init); // Allocate an area of memory void * alloc_chunk(size_t size) { if (alloc_end + size > dynmem_end()) shutdown("alloc_chunk failed"); void *data = alloc_end; alloc_end += ALIGN(size, __alignof__(void*)); memset(data, 0, size); return data; } // Allocate an array of chunks static void * alloc_chunks(size_t size, size_t count, uint16_t *avail) { size_t can_alloc = 0; void *p = alloc_end, *end = dynmem_end(); while (can_alloc < count && p + size <= end) can_alloc++, p += size; if (!can_alloc) shutdown("alloc_chunks failed"); void *data = alloc_chunk(p - alloc_end); *avail = can_alloc; return data; } /**************************************************************** * Move queue ****************************************************************/ static struct move_node *move_free_list; static void *move_list; static uint16_t move_count; static uint8_t move_item_size; // Is the config and move queue finalized? static int is_finalized(void) { return !!move_count; } // Free previously allocated storage from move_alloc(). Caller must // disable irqs. void move_free(void *m) { struct move_node *mf = m; mf->next = move_free_list; move_free_list = mf; } // Allocate runtime storage void * move_alloc(void) { irqstatus_t flag = irq_save(); struct move_node *mf = move_free_list; if (!mf) shutdown("Move queue overflow"); move_free_list = mf->next; irq_restore(flag); return mf; } // Check if a move_queue is empty int move_queue_empty(struct move_queue_head *mh) { return mh->first == NULL; } // Return first node in a move queue struct move_node * move_queue_first(struct move_queue_head *mh) { return mh->first; } // Add move to queue int move_queue_push(struct move_node *m, struct move_queue_head *mh) { m->next = NULL; if (mh->first) { mh->last->next = m; mh->last = m; return 0; } mh->first = mh->last = m; return 1; } // Remove first item from queue (caller must ensure queue not empty) struct move_node * move_queue_pop(struct move_queue_head *mh) { struct move_node *mn = mh->first; mh->first = mn->next; return mn; } // Completely clear move queue (used in shutdown handlers) void move_queue_clear(struct move_queue_head *mh) { mh->first = NULL; } // Initialize a move_queue with nodes of the give size void move_queue_setup(struct move_queue_head *mh, int size) { mh->first = mh->last = NULL; if (size > UINT8_MAX || is_finalized()) shutdown("Invalid move request size"); if (size > move_item_size) move_item_size = size; } void move_reset(void) { if (!move_count) return; // Add everything in move_list to the free list. uint32_t i; for (i=0; inext = move_list + (i + 1)*move_item_size; } struct move_node *mf = move_list + (move_count - 1)*move_item_size; mf->next = NULL; move_free_list = move_list; } DECL_SHUTDOWN(move_reset); static void move_finalize(void) { if (is_finalized()) shutdown("Already finalized"); struct move_queue_head dummy; move_queue_setup(&dummy, sizeof(*move_free_list)); move_list = alloc_chunks(move_item_size, 1024, &move_count); move_reset(); } /**************************************************************** * Generic object ids (oid) ****************************************************************/ struct oid_s { void *type, *data; }; static struct oid_s *oids; static uint8_t oid_count; void * oid_lookup(uint8_t oid, void *type) { if (oid >= oid_count || type != oids[oid].type) shutdown("Invalid oid type"); return oids[oid].data; } void * oid_alloc(uint8_t oid, void *type, uint16_t size) { if (oid >= oid_count || oids[oid].type || is_finalized()) shutdown("Can't assign oid"); oids[oid].type = type; void *data = alloc_chunk(size); oids[oid].data = data; return data; } void * oid_next(uint8_t *i, void *type) { uint8_t oid = *i; for (;;) { oid++; if (oid >= oid_count) return NULL; if (oids[oid].type == type) { *i = oid; return oids[oid].data; } } } void command_allocate_oids(uint32_t *args) { if (oids) shutdown("oids already allocated"); uint8_t count = args[0]; oids = alloc_chunk(sizeof(oids[0]) * count); oid_count = count; } DECL_COMMAND(command_allocate_oids, "allocate_oids count=%c"); /**************************************************************** * Config CRC ****************************************************************/ static uint32_t config_crc; void command_get_config(uint32_t *args) { sendf("config is_config=%c crc=%u is_shutdown=%c move_count=%hu" , is_finalized(), config_crc, sched_is_shutdown(), move_count); } DECL_COMMAND_FLAGS(command_get_config, HF_IN_SHUTDOWN, "get_config"); void command_finalize_config(uint32_t *args) { move_finalize(); config_crc = args[0]; } DECL_COMMAND(command_finalize_config, "finalize_config crc=%u"); // Attempt a full manual reset of the config void config_reset(uint32_t *args) { if (! sched_is_shutdown()) shutdown("config_reset only available when shutdown"); irq_disable(); config_crc = 0; oid_count = 0; oids = NULL; move_free_list = NULL; move_list = NULL; move_count = move_item_size = 0; alloc_init(); sched_timer_reset(); sched_clear_shutdown(); irq_enable(); } /**************************************************************** * Timing and load stats ****************************************************************/ void command_get_clock(uint32_t *args) { sendf("clock clock=%u", timer_read_time()); } DECL_COMMAND_FLAGS(command_get_clock, HF_IN_SHUTDOWN, "get_clock"); static uint32_t stats_send_time, stats_send_time_high; void command_get_uptime(uint32_t *args) { uint32_t cur = timer_read_time(); uint32_t high = stats_send_time_high + (cur < stats_send_time); sendf("uptime high=%u clock=%u", high, cur); } DECL_COMMAND_FLAGS(command_get_uptime, HF_IN_SHUTDOWN, "get_uptime"); #define SUMSQ_BASE 256 DECL_CONSTANT("STATS_SUMSQ_BASE", SUMSQ_BASE); void stats_update(uint32_t start, uint32_t cur) { static uint32_t count, sum, sumsq; uint32_t diff = cur - start; count++; sum += diff; // Calculate sum of diff^2 - be careful of integer overflow uint32_t nextsumsq; if (diff <= 0xffff) { nextsumsq = sumsq + DIV_ROUND_UP(diff * diff, SUMSQ_BASE); } else if (diff <= 0xfffff) { nextsumsq = sumsq + DIV_ROUND_UP(diff, SUMSQ_BASE) * diff; } else { nextsumsq = 0xffffffff; } if (nextsumsq < sumsq) nextsumsq = 0xffffffff; sumsq = nextsumsq; if (timer_is_before(cur, stats_send_time + timer_from_us(5000000))) return; sendf("stats count=%u sum=%u sumsq=%u", count, sum, sumsq); if (cur < stats_send_time) stats_send_time_high++; stats_send_time = cur; count = sum = sumsq = 0; } /**************************************************************** * Misc commands ****************************************************************/ void command_emergency_stop(uint32_t *args) { shutdown("Command request"); } DECL_COMMAND_FLAGS(command_emergency_stop, HF_IN_SHUTDOWN, "emergency_stop"); void command_clear_shutdown(uint32_t *args) { sched_clear_shutdown(); } DECL_COMMAND_FLAGS(command_clear_shutdown, HF_IN_SHUTDOWN, "clear_shutdown"); void command_identify(uint32_t *args) { uint32_t offset = args[0]; uint8_t count = args[1]; uint32_t isize = READP(command_identify_size); if (offset >= isize) count = 0; else if (offset + count > isize) count = isize - offset; sendf("identify_response offset=%u data=%.*s" , offset, count, &command_identify_data[offset]); } DECL_COMMAND_FLAGS(command_identify, HF_IN_SHUTDOWN, "identify offset=%u count=%c");