From: Joseph Coffland Date: Thu, 31 Dec 2015 03:19:11 +0000 (-0800) Subject: Replaced xio X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=e85775cccde981c9a59c2c61ebc9da7306efba55;p=bbctrl-firmware Replaced xio --- diff --git a/Makefile b/Makefile index 79bbcdf..edd6a26 100755 --- a/Makefile +++ b/Makefile @@ -38,7 +38,6 @@ FUSE5=0xeb # SRC SRC = $(wildcard src/*.c) -SRC += $(wildcard src/xio/*.c) SRC += $(wildcard src/xmega/*.c) OBJ = $(patsubst src/%.c,build/%.o,$(SRC)) diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 53f5d3b..34cbb2c 100755 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -103,7 +103,7 @@ #include "switch.h" #include "hardware.h" #include "util.h" -#include "xio/xio.h" // for serial queue flush +#include "usart.h" // for serial queue flush cmSingleton_t cm; // canonical machine controller singleton @@ -347,7 +347,7 @@ void cm_update_model_position_from_runtime() {copy_vector(cm.gmx.position, mr.gm */ stat_t cm_deferred_write_callback() { if ((cm.cycle_state == CYCLE_OFF) && (cm.deferred_write_flag == true)) { - if (xio_isbusy()) return STAT_OK; // don't write back if serial RX is not empty + if (!usart_rx_empty()) return STAT_OK; // don't write back if serial RX is not empty cm.deferred_write_flag = false; nvObj_t nv; for (uint8_t i=1; i<=COORDS; i++) @@ -1161,7 +1161,7 @@ stat_t cm_feedhold_sequencing_callback() { stat_t cm_queue_flush() { if (cm_get_runtime_busy() == true) return STAT_COMMAND_NOT_ACCEPTED; - xio_reset_usb_rx_buffers(); // flush serial queues + usart_rx_flush(); // flush serial queues mp_flush_planner(); // flush planner queue qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available rx_request_rx_report(); diff --git a/src/config.c b/src/config.c index f6ab617..b16e532 100755 --- a/src/config.c +++ b/src/config.c @@ -29,7 +29,7 @@ */ #include "tinyg.h" // #1 -#include "config.h" // #2 +#include "config.h" // #2 #include "report.h" #include "controller.h" #include "canonical_machine.h" @@ -39,7 +39,6 @@ #include "hardware.h" #include "help.h" #include "util.h" -#include "xio/xio.h" static void _set_defa(nvObj_t *nv); @@ -64,36 +63,33 @@ nvList_t nvl; * * !!!! NOTE: nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround */ -stat_t nv_set(nvObj_t *nv) -{ - if (nv->index >= nv_index_max()) - return(STAT_INTERNAL_RANGE_ERROR); - return ((fptrCmd)GET_TABLE_WORD(set))(nv); +stat_t nv_set(nvObj_t *nv) { + if (nv->index >= nv_index_max()) + return(STAT_INTERNAL_RANGE_ERROR); + return ((fptrCmd)GET_TABLE_WORD(set))(nv); } -stat_t nv_get(nvObj_t *nv) -{ - if (nv->index >= nv_index_max()) return STAT_INTERNAL_RANGE_ERROR; - return ((fptrCmd)GET_TABLE_WORD(get))(nv); + +stat_t nv_get(nvObj_t *nv) { + if (nv->index >= nv_index_max()) return STAT_INTERNAL_RANGE_ERROR; + return ((fptrCmd)GET_TABLE_WORD(get))(nv); } -void nv_print(nvObj_t *nv) -{ - if (nv->index >= nv_index_max()) return; - ((fptrCmd)GET_TABLE_WORD(print))(nv); + +void nv_print(nvObj_t *nv) { + if (nv->index >= nv_index_max()) return; + ((fptrCmd)GET_TABLE_WORD(print))(nv); } -stat_t nv_persist(nvObj_t *nv) // nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround -{ -#ifndef __DISABLE_PERSISTENCE // cutout for faster simulation in test - if (nv_index_lt_groups(nv->index) == false) - return(STAT_INTERNAL_RANGE_ERROR); - if (GET_TABLE_BYTE(flags) & F_PERSIST) - return(write_persistent_value(nv)); -#endif - return STAT_OK; + +stat_t nv_persist(nvObj_t *nv) { // nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround + if (nv_index_lt_groups(nv->index) == false) return(STAT_INTERNAL_RANGE_ERROR); + if (GET_TABLE_BYTE(flags) & F_PERSIST) return(write_persistent_value(nv)); + + return STAT_OK; } + /************************************************************************************ * config_init() - called once on hard reset * @@ -106,87 +102,84 @@ stat_t nv_persist(nvObj_t *nv) // nv_persist() cannot be called from an inter * * NOTE: Config assertions are handled from the controller */ -void config_init() -{ - nvObj_t *nv = nv_reset_nv_list(); - config_init_assertions(); - - cm_set_units_mode(MILLIMETERS); // must do inits in millimeter mode - nv->index = 0; // this will read the first record in NVM - - read_persistent_value(nv); - if (nv->value != cs.fw_build) { // case (1) NVM is not setup or not in revision - _set_defa(nv); - } else { // case (2) NVM is setup and in revision - rpt_print_loading_configs_message(); - for (nv->index=0; nv_index_is_single(nv->index); nv->index++) { - if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { - strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); // read the token from the array - read_persistent_value(nv); - nv_set(nv); - } - } - sr_init_status_report(); - } +void config_init() { + nvObj_t *nv = nv_reset_nv_list(); + config_init_assertions(); + + cm_set_units_mode(MILLIMETERS); // must do inits in millimeter mode + nv->index = 0; // this will read the first record in NVM + + read_persistent_value(nv); + if (nv->value != cs.fw_build) // case (1) NVM is not setup or not in revision + _set_defa(nv); + else { // case (2) NVM is setup and in revision + rpt_print_loading_configs_message(); + + for (nv->index=0; nv_index_is_single(nv->index); nv->index++) + if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { + strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); // read the token from the array + read_persistent_value(nv); + nv_set(nv); + } + + sr_init_status_report(); + } } + /* * set_defaults() - reset persistence with default values for machine profile * _set_defa() - helper function and called directly from config_init() */ - -static void _set_defa(nvObj_t *nv) -{ - cm_set_units_mode(MILLIMETERS); // must do inits in MM mode - for (nv->index=0; nv_index_is_single(nv->index); nv->index++) { - if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { - nv->value = GET_TABLE_FLOAT(def_value); - strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); - nv_set(nv); - nv_persist(nv); - } +static void _set_defa(nvObj_t *nv) { + cm_set_units_mode(MILLIMETERS); // must do inits in MM mode + for (nv->index=0; nv_index_is_single(nv->index); nv->index++) + if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { + nv->value = GET_TABLE_FLOAT(def_value); + strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); + nv_set(nv); + nv_persist(nv); } - sr_init_status_report(); // reset status reports - rpt_print_initializing_message(); // don't start TX until all the NVM persistence is done + + sr_init_status_report(); // reset status reports + rpt_print_initializing_message(); // don't start TX until all the NVM persistence is done } -stat_t set_defaults(nvObj_t *nv) -{ - // failsafe. nv->value must be true or no action occurs - if (fp_FALSE(nv->value)) return(help_defa(nv)); - _set_defa(nv); - // The values in nv are now garbage. Mark the nv as $defa so it displays nicely. - nv->valuetype = TYPE_INTEGER; - nv->value = 1; - return STAT_OK; +stat_t set_defaults(nvObj_t *nv) { + // failsafe. nv->value must be true or no action occurs + if (fp_FALSE(nv->value)) return(help_defa(nv)); + _set_defa(nv); + + // The values in nv are now garbage. Mark the nv as $defa so it displays nicely. + nv->valuetype = TYPE_INTEGER; + nv->value = 1; + return STAT_OK; } + /* * config_init_assertions() * config_test_assertions() - check memory integrity of config sub-system */ - -void config_init_assertions() -{ - cfg.magic_start = MAGICNUM; - cfg.magic_end = MAGICNUM; - nvl.magic_start = MAGICNUM; - nvl.magic_end = MAGICNUM; - nvStr.magic_start = MAGICNUM; - nvStr.magic_end = MAGICNUM; +void config_init_assertions() { + cfg.magic_start = MAGICNUM; + cfg.magic_end = MAGICNUM; + nvl.magic_start = MAGICNUM; + nvl.magic_end = MAGICNUM; + nvStr.magic_start = MAGICNUM; + nvStr.magic_end = MAGICNUM; } -stat_t config_test_assertions() -{ - if ((cfg.magic_start != MAGICNUM) || (cfg.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; - if ((nvl.magic_start != MAGICNUM) || (nvl.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; - if ((nvStr.magic_start != MAGICNUM) || (nvStr.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; - if (global_string_buf[MESSAGE_LEN-1] != 0) return STAT_CONFIG_ASSERTION_FAILURE; - return STAT_OK; + +stat_t config_test_assertions() { + if ((cfg.magic_start != MAGICNUM) || (cfg.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; + if ((nvl.magic_start != MAGICNUM) || (nvl.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; + if ((nvStr.magic_start != MAGICNUM) || (nvStr.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; + if (global_string_buf[MESSAGE_LEN-1] != 0) return STAT_CONFIG_ASSERTION_FAILURE; + return STAT_OK; } -/***** Generic Internal Functions *********************************************/ /* Generic gets() * get_nul() - get nothing (returns STAT_PARAMETER_CANNOT_BE_READ) @@ -196,42 +189,42 @@ stat_t config_test_assertions() * get_flt() - get value as float * get_format() - internal accessor for printf() format string */ -stat_t get_nul(nvObj_t *nv) -{ - nv->valuetype = TYPE_0; - return STAT_PARAMETER_CANNOT_BE_READ; +stat_t get_nul(nvObj_t *nv) { + nv->valuetype = TYPE_0; + return STAT_PARAMETER_CANNOT_BE_READ; } -stat_t get_ui8(nvObj_t *nv) -{ - nv->value = (float)*((uint8_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; + +stat_t get_ui8(nvObj_t *nv) { + nv->value = (float)*((uint8_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; } -stat_t get_int(nvObj_t *nv) -{ - nv->value = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INTEGER; - return STAT_OK; + +stat_t get_int(nvObj_t *nv) { + nv->value = *((uint32_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; } -stat_t get_data(nvObj_t *nv) -{ - uint32_t *v = (uint32_t*)&nv->value; - *v = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_DATA; - return STAT_OK; + +stat_t get_data(nvObj_t *nv) { + uint32_t *v = (uint32_t*)&nv->value; + *v = *((uint32_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_DATA; + return STAT_OK; } -stat_t get_flt(nvObj_t *nv) -{ - nv->value = *((float *)GET_TABLE_WORD(target)); - nv->precision = (int8_t)GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return STAT_OK; + +stat_t get_flt(nvObj_t *nv) { + nv->value = *((float *)GET_TABLE_WORD(target)); + nv->precision = (int8_t)GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return STAT_OK; } + /* Generic sets() * set_nul() - set nothing (returns STAT_PARAMETER_IS_READ_ONLY) * set_ui8() - set value as 8 bit uint8_t value @@ -244,57 +237,54 @@ stat_t get_flt(nvObj_t *nv) */ stat_t set_nul(nvObj_t *nv) { return STAT_PARAMETER_IS_READ_ONLY; } -stat_t set_ui8(nvObj_t *nv) -{ - *((uint8_t *)GET_TABLE_WORD(target)) = nv->value; - nv->valuetype = TYPE_INTEGER; - return(STAT_OK); +stat_t set_ui8(nvObj_t *nv) { + *((uint8_t *)GET_TABLE_WORD(target)) = nv->value; + nv->valuetype = TYPE_INTEGER; + return(STAT_OK); } -stat_t set_01(nvObj_t *nv) -{ - if ((uint8_t)nv->value > 1) - return STAT_INPUT_VALUE_RANGE_ERROR; // if - return set_ui8(nv); // else + +stat_t set_01(nvObj_t *nv) { + if ((uint8_t)nv->value > 1) return STAT_INPUT_VALUE_RANGE_ERROR; + return set_ui8(nv); } -stat_t set_012(nvObj_t *nv) -{ - if ((uint8_t)nv->value > 2) - return STAT_INPUT_VALUE_RANGE_ERROR; // if - return set_ui8(nv); // else + +stat_t set_012(nvObj_t *nv) { + if ((uint8_t)nv->value > 2) return STAT_INPUT_VALUE_RANGE_ERROR; + return set_ui8(nv); } -stat_t set_0123(nvObj_t *nv) -{ - if ((uint8_t)nv->value > 3) - return STAT_INPUT_VALUE_RANGE_ERROR; // if - return set_ui8(nv); // else + +stat_t set_0123(nvObj_t *nv) { + if ((uint8_t)nv->value > 3) return STAT_INPUT_VALUE_RANGE_ERROR; + return set_ui8(nv); } -stat_t set_int(nvObj_t *nv) -{ - *((uint32_t *)GET_TABLE_WORD(target)) = (uint32_t)nv->value; - nv->valuetype = TYPE_INTEGER; - return(STAT_OK); + +stat_t set_int(nvObj_t *nv) { + *((uint32_t *)GET_TABLE_WORD(target)) = (uint32_t)nv->value; + nv->valuetype = TYPE_INTEGER; + return(STAT_OK); } -stat_t set_data(nvObj_t *nv) -{ - uint32_t *v = (uint32_t*)&nv->value; - *((uint32_t *)GET_TABLE_WORD(target)) = *v; - nv->valuetype = TYPE_DATA; - return(STAT_OK); + +stat_t set_data(nvObj_t *nv) { + uint32_t *v = (uint32_t*)&nv->value; + *((uint32_t *)GET_TABLE_WORD(target)) = *v; + nv->valuetype = TYPE_DATA; + return(STAT_OK); } -stat_t set_flt(nvObj_t *nv) -{ - *((float *)GET_TABLE_WORD(target)) = nv->value; - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return(STAT_OK); + +stat_t set_flt(nvObj_t *nv) { + *((float *)GET_TABLE_WORD(target)) = nv->value; + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return(STAT_OK); } + /************************************************************************************ * Group operations * @@ -334,21 +324,22 @@ stat_t set_flt(nvObj_t *nv) * The sys group is an exception where the children carry a blank group field, even though * the sys parent is labeled as a TYPE_PARENT. */ +stat_t get_grp(nvObj_t *nv) { + char_t *parent_group = nv->token; // token in the parent nv object is the group + char_t group[GROUP_LEN+1]; // group string retrieved from cfgArray child + nv->valuetype = TYPE_PARENT; // make first object the parent -stat_t get_grp(nvObj_t *nv) -{ - char_t *parent_group = nv->token; // token in the parent nv object is the group - char_t group[GROUP_LEN+1]; // group string retrieved from cfgArray child - nv->valuetype = TYPE_PARENT; // make first object the parent - for (index_t i=0; nv_index_is_single(i); i++) { - strcpy_P(group, cfgArray[i].group); // don't need strncpy as it's always terminated - if (strcmp(parent_group, group) != 0) continue; - (++nv)->index = i; - nv_get_nvObj(nv); - } - return STAT_OK; + for (index_t i=0; nv_index_is_single(i); i++) { + strcpy_P(group, cfgArray[i].group); // don't need strncpy as it's always terminated + if (strcmp(parent_group, group) != 0) continue; + (++nv)->index = i; + nv_get_nvObj(nv); + } + + return STAT_OK; } + /* * set_grp() - get or set one or more values in a group * @@ -358,45 +349,38 @@ stat_t get_grp(nvObj_t *nv) * * This function serves JSON mode only as text mode shouldn't call it. */ - -stat_t set_grp(nvObj_t *nv) -{ - if (cfg.comm_mode == TEXT_MODE) - return STAT_INVALID_OR_MALFORMED_COMMAND; - - for (uint8_t i=0; inx) == 0) break; - if (nv->valuetype == TYPE_EMPTY) break; - else if (nv->valuetype == TYPE_0) // 0 means GET the value - nv_get(nv); - else { - nv_set(nv); - nv_persist(nv); - } +stat_t set_grp(nvObj_t *nv) { + if (cfg.comm_mode == TEXT_MODE) + return STAT_INVALID_OR_MALFORMED_COMMAND; + + for (uint8_t i=0; inx) == 0) break; + if (nv->valuetype == TYPE_EMPTY) break; + else if (nv->valuetype == TYPE_0) nv_get(nv); // 0 means GET the value + else { + nv_set(nv); + nv_persist(nv); } - return STAT_OK; + } + + return STAT_OK; } + /* * nv_group_is_prefixed() - hack * * This little function deals with the exception cases that some groups don't use * the parent token as a prefix to the child elements; SR being a good example. */ -uint8_t nv_group_is_prefixed(char_t *group) -{ - if (strcmp("sr",group) == 0) return false; - if (strcmp("sys",group) == 0) return false; - return true; +uint8_t nv_group_is_prefixed(char_t *group) { + if (strcmp("sr",group) == 0) return false; + if (strcmp("sys",group) == 0) return false; + return true; } -/*********************************************************************************** - ***** nvObj functions ************************************************************ - ***********************************************************************************/ -/*********************************************************************************** - * nvObj helper functions and other low-level nv helpers - */ +// nvObj helper functions and other low-level nv helpers /* nv_get_index() - get index from mnenonic token + group * @@ -404,47 +388,45 @@ uint8_t nv_group_is_prefixed(char_t *group) * linear table scan of the PROGMEM strings, which of course could be further * optimized with indexes or hashing. */ -index_t nv_get_index(const char_t *group, const char_t *token) -{ - char_t c; - char_t str[TOKEN_LEN + GROUP_LEN+1]; // should actually never be more than TOKEN_LEN+1 - strncpy(str, group, GROUP_LEN+1); - strncat(str, token, TOKEN_LEN+1); - - index_t i; - index_t index_max = nv_index_max(); - - for (i=0; i < index_max; i++) { - if ((c = GET_TOKEN_BYTE(token[0])) != str[0]) { continue; } // 1st character mismatch - if ((c = GET_TOKEN_BYTE(token[1])) == 0) { if (str[1] == 0) return(i);} // one character match - if (c != str[1]) continue; // 2nd character mismatch - if ((c = GET_TOKEN_BYTE(token[2])) == 0) { if (str[2] == 0) return(i);} // two character match - if (c != str[2]) continue; // 3rd character mismatch - if ((c = GET_TOKEN_BYTE(token[3])) == 0) { if (str[3] == 0) return(i);} // three character match - if (c != str[3]) continue; // 4th character mismatch - if ((c = GET_TOKEN_BYTE(token[4])) == 0) { if (str[4] == 0) return(i);} // four character match - if (c != str[4]) continue; // 5th character mismatch - return i; // five character match - } - return NO_MATCH; +index_t nv_get_index(const char_t *group, const char_t *token) { + char_t c; + char_t str[TOKEN_LEN + GROUP_LEN+1]; // should actually never be more than TOKEN_LEN+1 + strncpy(str, group, GROUP_LEN+1); + strncat(str, token, TOKEN_LEN+1); + + index_t i; + index_t index_max = nv_index_max(); + + for (i = 0; i < index_max; i++) { + if ((c = GET_TOKEN_BYTE(token[0])) != str[0]) continue; // 1st character mismatch + if ((c = GET_TOKEN_BYTE(token[1])) == 0) {if (str[1] == 0) return(i);} // one character match + if (c != str[1]) continue; // 2nd character mismatch + if ((c = GET_TOKEN_BYTE(token[2])) == 0) {if (str[2] == 0) return(i);} // two character match + if (c != str[2]) continue; // 3rd character mismatch + if ((c = GET_TOKEN_BYTE(token[3])) == 0) {if (str[3] == 0) return(i);} // three character match + if (c != str[3]) continue; // 4th character mismatch + if ((c = GET_TOKEN_BYTE(token[4])) == 0) {if (str[4] == 0) return(i);} // four character match + if (c != str[4]) continue; // 5th character mismatch + return i; // five character match + } + + return NO_MATCH; } -/* - * nv_get_type() - returns command type as a NV_TYPE enum - */ +// nv_get_type() - returns command type as a NV_TYPE enum +uint8_t nv_get_type(nvObj_t *nv) { + if (nv->token[0] == 0) return NV_TYPE_0; + if (strcmp("gc", nv->token) == 0) return NV_TYPE_GCODE; + if (strcmp("sr", nv->token) == 0) return NV_TYPE_REPORT; + if (strcmp("qr", nv->token) == 0) return NV_TYPE_REPORT; + if (strcmp("msg",nv->token) == 0) return NV_TYPE_MESSAGE; + if (strcmp("err",nv->token) == 0) return NV_TYPE_MESSAGE; // errors are reported as messages + if (strcmp("n", nv->token) == 0) return NV_TYPE_LINENUM; -uint8_t nv_get_type(nvObj_t *nv) -{ - if (nv->token[0] == 0) return NV_TYPE_0; - if (strcmp("gc", nv->token) == 0) return NV_TYPE_GCODE; - if (strcmp("sr", nv->token) == 0) return NV_TYPE_REPORT; - if (strcmp("qr", nv->token) == 0) return NV_TYPE_REPORT; - if (strcmp("msg",nv->token) == 0) return NV_TYPE_MESSAGE; - if (strcmp("err",nv->token) == 0) return NV_TYPE_MESSAGE; // errors are reported as messages - if (strcmp("n", nv->token) == 0) return NV_TYPE_LINENUM; - return NV_TYPE_CONFIG; + return NV_TYPE_CONFIG; } + /****************************************************************************** * nvObj low-level object and list operations * nv_get_nvObj() - setup a nv object by providing the index @@ -472,183 +454,189 @@ uint8_t nv_get_type(nvObj_t *nv) * On the AVR this will save a little static RAM. The "msg" string will occupy flash * as an initializer and be instantiated in stack RAM when the function executes. */ +void nv_get_nvObj(nvObj_t *nv) { + if (nv->index >= nv_index_max()) { return; } // sanity -void nv_get_nvObj(nvObj_t *nv) -{ - if (nv->index >= nv_index_max()) { return; } // sanity - - index_t tmp = nv->index; - nv_reset_nv(nv); - nv->index = tmp; + index_t tmp = nv->index; + nv_reset_nv(nv); + nv->index = tmp; - strcpy_P(nv->token, cfgArray[nv->index].token); // token field is always terminated - strcpy_P(nv->group, cfgArray[nv->index].group); // group field is always terminated + strcpy_P(nv->token, cfgArray[nv->index].token); // token field is always terminated + strcpy_P(nv->group, cfgArray[nv->index].group); // group field is always terminated - // special processing for system groups and stripping tokens for groups - if (nv->group[0] != 0) { - if (GET_TABLE_BYTE(flags) & F_NOSTRIP) { - nv->group[0] = 0; - } else { - strcpy(nv->token, &nv->token[strlen(nv->group)]); // strip group from the token - } + // special processing for system groups and stripping tokens for groups + if (nv->group[0] != 0) { + if (GET_TABLE_BYTE(flags) & F_NOSTRIP) { + nv->group[0] = 0; + } else { + strcpy(nv->token, &nv->token[strlen(nv->group)]); // strip group from the token } - ((fptrCmd)GET_TABLE_WORD(get))(nv); // populate the value + } + + ((fptrCmd)GET_TABLE_WORD(get))(nv); // populate the value +} + + +nvObj_t *nv_reset_nv(nvObj_t *nv) { // clear a single nvObj structure + nv->valuetype = TYPE_EMPTY; // selective clear is much faster than calling memset + nv->index = 0; + nv->value = 0; + nv->precision = 0; + nv->token[0] = 0; + nv->group[0] = 0; + nv->stringp = 0; + + if (nv->pv == 0) nv->depth = 0; // set depth correctly + else { + if (nv->pv->valuetype == TYPE_PARENT) nv->depth = nv->pv->depth + 1; + else nv->depth = nv->pv->depth; + } + + return nv; // return pointer to nv as a convenience to callers } -nvObj_t *nv_reset_nv(nvObj_t *nv) // clear a single nvObj structure -{ - nv->valuetype = TYPE_EMPTY; // selective clear is much faster than calling memset + +nvObj_t *nv_reset_nv_list() { // clear the header and response body + nvStr.wp = 0; // reset the shared string + nvObj_t *nv = nvl.list; // set up linked list and initialize elements + + for (uint8_t i=0; ipv = (nv-1); // the ends are bogus & corrected later + nv->nx = (nv+1); nv->index = 0; - nv->value = 0; + nv->depth = 1; // header and footer are corrected later nv->precision = 0; + nv->valuetype = TYPE_EMPTY; nv->token[0] = 0; - nv->group[0] = 0; - nv->stringp = 0; + } - if (nv->pv == 0) { // set depth correctly - nv->depth = 0; - } else { - if (nv->pv->valuetype == TYPE_PARENT) { - nv->depth = nv->pv->depth + 1; - } else { - nv->depth = nv->pv->depth; - } - } - return nv; // return pointer to nv as a convenience to callers -} - -nvObj_t *nv_reset_nv_list() // clear the header and response body -{ - nvStr.wp = 0; // reset the shared string - nvObj_t *nv = nvl.list; // set up linked list and initialize elements - for (uint8_t i=0; ipv = (nv-1); // the ends are bogus & corrected later - nv->nx = (nv+1); - nv->index = 0; - nv->depth = 1; // header and footer are corrected later - nv->precision = 0; - nv->valuetype = TYPE_EMPTY; - nv->token[0] = 0; - } - (--nv)->nx = 0; - nv = nvl.list; // setup response header element ('r') - nv->pv = 0; - nv->depth = 0; - nv->valuetype = TYPE_PARENT; - strcpy(nv->token, "r"); - return nv_body; // this is a convenience for calling routines -} - -stat_t nv_copy_string(nvObj_t *nv, const char_t *src) -{ - if ((nvStr.wp + strlen(src)) > NV_SHARED_STRING_LEN) - return STAT_BUFFER_FULL; - - char_t *dst = &nvStr.string[nvStr.wp]; - strcpy(dst, src); // copy string to current head position - // string has already been tested for overflow, above - nvStr.wp += strlen(src)+1; // advance head for next string - nv->stringp = (char_t (*)[])dst; - return STAT_OK; -} - -nvObj_t *nv_add_object(const char_t *token) // add an object to the body using a token -{ - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety - continue; - } - // load the index from the token or die trying - if ((nv->index = nv_get_index((const char_t *)"",token)) == NO_MATCH) { return 0;} - nv_get_nvObj(nv); // populate the object from the index - return nv; + (--nv)->nx = 0; + nv = nvl.list; // setup response header element ('r') + nv->pv = 0; + nv->depth = 0; + nv->valuetype = TYPE_PARENT; + strcpy(nv->token, "r"); + return nv_body; // this is a convenience for calling routines +} + + +stat_t nv_copy_string(nvObj_t *nv, const char_t *src) { + if ((nvStr.wp + strlen(src)) > NV_SHARED_STRING_LEN) + return STAT_BUFFER_FULL; + + char_t *dst = &nvStr.string[nvStr.wp]; + strcpy(dst, src); // copy string to current head position + // string has already been tested for overflow, above + nvStr.wp += strlen(src)+1; // advance head for next string + nv->stringp = (char_t (*)[])dst; + return STAT_OK; +} + + +nvObj_t *nv_add_object(const char_t *token) { // add an object to the body using a token + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; } - return 0; -} - -nvObj_t *nv_add_integer(const char_t *token, const uint32_t value)// add an integer object to the body -{ - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety - continue; - } - strncpy(nv->token, token, TOKEN_LEN); - nv->value = (float) value; - nv->valuetype = TYPE_INTEGER; - return nv; + + // load the index from the token or die trying + if ((nv->index = nv_get_index((const char_t *)"",token)) == NO_MATCH) { return 0;} + nv_get_nvObj(nv); // populate the object from the index + return nv; + } + + return 0; +} + + +nvObj_t *nv_add_integer(const char_t *token, const uint32_t value) { // add an integer object to the body + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; } - return 0; -} - -nvObj_t *nv_add_data(const char_t *token, const uint32_t value)// add an integer object to the body -{ - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety - continue; - } - strcpy(nv->token, token); - float *v = (float*)&value; - nv->value = *v; - nv->valuetype = TYPE_DATA; - return nv; + + strncpy(nv->token, token, TOKEN_LEN); + nv->value = (float) value; + nv->valuetype = TYPE_INTEGER; + return nv; + } + + return 0; +} + + +nvObj_t *nv_add_data(const char_t *token, const uint32_t value) { // add an integer object to the body + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; } - return 0; -} - -nvObj_t *nv_add_float(const char_t *token, const float value) // add a float object to the body -{ - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety - continue; - } - strncpy(nv->token, token, TOKEN_LEN); - nv->value = value; - nv->valuetype = TYPE_FLOAT; - return nv; + + strcpy(nv->token, token); + float *v = (float*)&value; + nv->value = *v; + nv->valuetype = TYPE_DATA; + return nv; + } + + return 0; +} + +nvObj_t *nv_add_float(const char_t *token, const float value) { // add a float object to the body + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; } - return 0; + + strncpy(nv->token, token, TOKEN_LEN); + nv->value = value; + nv->valuetype = TYPE_FLOAT; + return nv; + } + + return 0; } + // ASSUMES A RAM STRING. If you need to post a FLASH string use pstr2str to convert it to a RAM string -nvObj_t *nv_add_string(const char_t *token, const char_t *string) // add a string object to the body -{ - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety - continue; - } - strncpy(nv->token, token, TOKEN_LEN); - if (nv_copy_string(nv, string) != STAT_OK) - return 0; - - nv->index = nv_get_index((const char_t *)"", nv->token); - nv->valuetype = TYPE_STRING; - return nv; +nvObj_t *nv_add_string(const char_t *token, const char_t *string) { // add a string object to the body + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return 0; // not supposed to find a 0; here for safety + continue; } - return 0; + + strncpy(nv->token, token, TOKEN_LEN); + if (nv_copy_string(nv, string) != STAT_OK) return 0; + + nv->index = nv_get_index((const char_t *)"", nv->token); + nv->valuetype = TYPE_STRING; + return nv; + } + + return 0; } + /* * cm_conditional_message() - queue a RAM string as a message in the response (conditionally) * * Note: If you need to post a FLASH string use pstr2str to convert it to a RAM string */ - -nvObj_t *nv_add_conditional_message(const char_t *string) // conditionally add a message object to the body -{ - if ((cfg.comm_mode == JSON_MODE) && (js.echo_json_messages != true)) { return 0;} - return(nv_add_string((const char_t *)"msg", string)); +nvObj_t *nv_add_conditional_message(const char_t *string) { // conditionally add a message object to the body + if ((cfg.comm_mode == JSON_MODE) && (js.echo_json_messages != true)) return 0; + return nv_add_string((const char_t *)"msg", string); } + /**** nv_print_list() - print nv_array as JSON or text ********************** * * Generate and print the JSON and text mode output strings. Use this function @@ -663,29 +651,14 @@ nvObj_t *nv_add_conditional_message(const char_t *string) // conditionally ad * text_flags = TEXT_INLINE_VALUES - print text as comma separated values on a single line * text_flags = TEXT_MULTILINE_FORMATTED - print text one value per line with formatting string */ - -void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags) -{ - if (cfg.comm_mode == JSON_MODE) { - json_print_list(status, json_flags); - } else { - text_print_list(status, text_flags); - } +void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags) { + if (cfg.comm_mode == JSON_MODE) json_print_list(status, json_flags); + else text_print_list(status, text_flags); } -/**************************************************************************** - ***** Diagnostics ********************************************************** - ****************************************************************************/ - -void nv_dump_nv(nvObj_t *nv) -{ - printf ("i:%d, d:%d, t:%d, p:%d, v:%f, g:%s, t:%s, s:%s\n", - nv->index, - nv->depth, - nv->valuetype, - nv->precision, - (double)nv->value, - nv->group, - nv->token, - (char *)nv->stringp); + +void nv_dump_nv(nvObj_t *nv) { + printf("i:%d, d:%d, t:%d, p:%d, v:%f, g:%s, t:%s, s:%s\n", nv->index, nv->depth, + nv->valuetype, nv->precision, (double)nv->value, nv->group, nv->token, + (char *)nv->stringp); } diff --git a/src/config_app.c b/src/config_app.c index c2273e0..87a110f 100755 --- a/src/config_app.c +++ b/src/config_app.c @@ -42,8 +42,7 @@ #include "test.h" #include "util.h" #include "help.h" -#include "network.h" -#include "xio/xio.h" +#include "usart.h" cfgParameters_t cfg; // application specific configuration parameters @@ -62,7 +61,7 @@ static stat_t _do_all(nvObj_t *nv); // print all parameters static stat_t set_ec(nvObj_t *nv); // expand CRLF on TX output static stat_t set_ee(nvObj_t *nv); // enable character echo static stat_t set_ex(nvObj_t *nv); // enable XON/XOFF and RTS/CTS flow control -static stat_t set_baud(nvObj_t *nv); // set USB baud rate +static stat_t set_baud(nvObj_t *nv); // set baud rate static stat_t get_rx(nvObj_t *nv); // get bytes in RX buffer @@ -417,11 +416,10 @@ const cfgItem_t cfgArray[] PROGMEM = { {"sys","sv", _fipn, 0, sr_print_sv, get_ui8, set_012, (float *)&sr.status_report_verbosity, STATUS_REPORT_VERBOSITY}, {"sys","si", _fipn, 0, sr_print_si, get_int, sr_set_si, (float *)&sr.status_report_interval, STATUS_REPORT_INTERVAL_MS}, - {"sys","ec", _fipn, 0, cfg_print_ec, get_ui8, set_ec, (float *)&cfg.enable_cr, COM_EXPAND_CR}, - {"sys","ee", _fipn, 0, cfg_print_ee, get_ui8, set_ee, (float *)&cfg.enable_echo, COM_ENABLE_ECHO}, - {"sys","ex", _fipn, 0, cfg_print_ex, get_ui8, set_ex, (float *)&cfg.enable_flow_control, COM_ENABLE_FLOW_CONTROL}, - {"sys","baud",_fn, 0, cfg_print_baud,get_ui8, set_baud, (float *)&cfg.usb_baud_rate, XIO_BAUD_115200}, - {"sys","net", _fipn, 0, cfg_print_net, get_ui8, set_ui8, (float *)&cs.network_mode, NETWORK_MODE}, + {"sys","ec", _fipn, 0, cfg_print_ec, get_ui8, set_ec, (float *)&cfg.enable_cr, COM_EXPAND_CR}, + {"sys","ee", _fipn, 0, cfg_print_ee, get_ui8, set_ee, (float *)&cfg.enable_echo, COM_ENABLE_ECHO}, + {"sys","ex", _fipn, 0, cfg_print_ex, get_ui8, set_ex, (float *)&cfg.enable_flow_control, COM_ENABLE_FLOW_CONTROL}, + {"sys","baud",_fn, 0, cfg_print_baud,get_ui8, set_baud, (float *)&cfg.baud_rate, USART_BAUD_115200}, // NOTE: The ordering within the gcode defaults is important for token resolution {"sys","gpl", _fipn, 0, cm_print_gpl, get_ui8, set_012, (float *)&cm.select_plane, GCODE_DEFAULT_PLANE}, @@ -779,10 +777,8 @@ static stat_t _do_all(nvObj_t *nv) { // print all parameters * set_baud() - set baud rate * get_rx() - get bytes available in RX buffer */ -static stat_t _set_comm_helper(nvObj_t *nv, uint32_t yes, uint32_t no) { - if (fp_NOT_ZERO(nv->value)) xio_ctrl(XIO_DEV_USB, yes); - else xio_ctrl(XIO_DEV_USB, no); - +static stat_t _set_comm_helper(nvObj_t *nv, int cmd) { + usart_ctrl(cmd, fp_NOT_ZERO(nv->value)); return STAT_OK; } @@ -790,35 +786,35 @@ static stat_t _set_comm_helper(nvObj_t *nv, uint32_t yes, uint32_t no) { static stat_t set_ec(nvObj_t *nv) { // expand CR to CRLF on TX if (nv->value > true) return STAT_INPUT_VALUE_RANGE_ERROR; cfg.enable_cr = (uint8_t)nv->value; - return _set_comm_helper(nv, XIO_CRLF, XIO_NOCRLF); + return _set_comm_helper(nv, USART_CRLF); } static stat_t set_ee(nvObj_t *nv) { // enable character echo if (nv->value > true) return STAT_INPUT_VALUE_RANGE_ERROR; cfg.enable_echo = (uint8_t)nv->value; - return _set_comm_helper(nv, XIO_ECHO, XIO_NOECHO); + return _set_comm_helper(nv, USART_ECHO); } static stat_t set_ex(nvObj_t *nv) { // enable XON/XOFF or RTS/CTS flow control if (nv->value > FLOW_CONTROL_RTS) return STAT_INPUT_VALUE_RANGE_ERROR; cfg.enable_flow_control = (uint8_t)nv->value; - return _set_comm_helper(nv, XIO_XOFF, XIO_NOXOFF); + return _set_comm_helper(nv, USART_XOFF); } static stat_t get_rx(nvObj_t *nv) { - nv->value = (float)xio_get_usb_rx_free(); + nv->value = (float)usart_rx_space(); nv->valuetype = TYPE_INTEGER; return STAT_OK; } /* - * set_baud() - set USB baud rate + * set_baud() - set baud rate * - * See xio_usart.h for valid values. Works as a callback. + * See usart.h for valid values. Works as a callback. * The initial routine changes the baud config setting and sets a flag * Then it posts a user message indicating the new baud rate * Then it waits for the TX buffer to empty (so the message is sent) @@ -841,8 +837,8 @@ static stat_t set_baud(nvObj_t *nv) { return STAT_INPUT_VALUE_RANGE_ERROR; } - cfg.usb_baud_rate = baud; - cfg.usb_baud_flag = true; + cfg.baud_rate = baud; + cfg.baud_flag = true; char_t message[NV_MESSAGE_LEN]; sprintf_P(message, PSTR("*** NOTICE *** Resetting baud rate to %s"),GET_TEXT_ITEM(msg_baud, baud)); nv_add_conditional_message(message); @@ -852,9 +848,9 @@ static stat_t set_baud(nvObj_t *nv) { stat_t set_baud_callback() { - if (cfg.usb_baud_flag == false) return STAT_NOOP; - cfg.usb_baud_flag = false; - xio_set_baud(XIO_DEV_USB, cfg.usb_baud_rate); + if (cfg.baud_flag == false) return STAT_NOOP; + cfg.baud_flag = false; + usart_set_baud(cfg.baud_rate); return STAT_OK; } @@ -870,14 +866,12 @@ static const char fmt_ec[] PROGMEM = "[ec] expand LF to CRLF on TX%6d [0=off,1= static const char fmt_ee[] PROGMEM = "[ee] enable echo%18d [0=off,1=on]\n"; static const char fmt_ex[] PROGMEM = "[ex] enable flow control%10d [0=off,1=XON/XOFF, 2=RTS/CTS]\n"; static const char fmt_baud[] PROGMEM = "[baud] Baud rate%15d [1=9600,2=19200,3=38400,4=57600,5=115200,6=230400]\n"; -static const char fmt_net[] PROGMEM = "[net] network mode%17d [0=master]\n"; static const char fmt_rx[] PROGMEM = "rx:%d\n"; void cfg_print_ec(nvObj_t *nv) {text_print_ui8(nv, fmt_ec);} void cfg_print_ee(nvObj_t *nv) {text_print_ui8(nv, fmt_ee);} void cfg_print_ex(nvObj_t *nv) {text_print_ui8(nv, fmt_ex);} void cfg_print_baud(nvObj_t *nv) {text_print_ui8(nv, fmt_baud);} -void cfg_print_net(nvObj_t *nv) {text_print_ui8(nv, fmt_net);} void cfg_print_rx(nvObj_t *nv) {text_print_ui8(nv, fmt_rx);} #endif // __TEXT_MODE diff --git a/src/config_app.h b/src/config_app.h index 5b738e4..275f0b4 100755 --- a/src/config_app.h +++ b/src/config_app.h @@ -39,17 +39,16 @@ enum nvType { // classification of commands ***********************************************************************************/ typedef struct cfgParameters { // mostly communications variables at this point - uint16_t magic_start; // magic number to test memory integrity + uint16_t magic_start; // magic number to test memory integrity // communications settings uint8_t comm_mode; // TG_TEXT_MODE or TG_JSON_MODE uint8_t enable_cr; // enable CR in CRFL expansion on TX - uint8_t enable_echo; // enable text-mode echo - uint8_t enable_flow_control; // enable XON/XOFF or RTS/CTS flow control -// uint8_t ignore_crlf; // ignore CR or LF on RX --- these 4 are shadow settings for XIO cntrl bits + uint8_t enable_echo; // enable text-mode echo + uint8_t enable_flow_control; // enable XON/XOFF or RTS/CTS flow control - uint8_t usb_baud_rate; // see xio_usart.h for XIO_BAUD values - uint8_t usb_baud_flag; // technically this belongs in the controller singleton + uint8_t baud_rate; // see usart.h for BAUD values + uint8_t baud_flag; // technically this belongs in the controller singleton // user-defined data groups uint32_t user_data_a[4]; diff --git a/src/controller.c b/src/controller.c index 37d9123..33cec6f 100755 --- a/src/controller.c +++ b/src/controller.c @@ -44,7 +44,7 @@ #include "report.h" #include "help.h" #include "util.h" -#include "xio/xio.h" +#include "usart.h" controller_t cs; // controller state structure @@ -61,7 +61,7 @@ stat_t hardware_hard_reset_handler(); stat_t hardware_bootloader_handler(); -void controller_init(uint8_t std_in, uint8_t std_out, uint8_t std_err) { +void controller_init() { memset(&cs, 0, sizeof(controller_t)); // clear all values, job_id's, pointers and status controller_init_assertions(); @@ -70,11 +70,6 @@ void controller_init(uint8_t std_in, uint8_t std_out, uint8_t std_err) { cs.hw_platform = TINYG_HARDWARE_PLATFORM; // NB: HW version is set from EEPROM cs.state = CONTROLLER_STARTUP; // ready to run startup lines - xio_set_stdin(std_in); - xio_set_stdout(std_out); - xio_set_stderr(std_err); - cs.default_src = std_in; - tg_set_primary_source(cs.default_src); } @@ -151,43 +146,33 @@ void controller_run() { * * Reads next command line and dispatches to relevant parser or action * Accepts commands if the move queue has room - EAGAINS if it doesn't - * Manages cutback to serial input from file devices (EOF) * Also responsible for prompts and for flow control */ static stat_t _command_dispatch() { stat_t status; // read input line or return if not a completed line - // xio_gets() is a non-blocking workalike of fgets() + // usart_gets() is a non-blocking workalike of fgets() while (true) { - if ((status = xio_gets(cs.primary_src, cs.in_buf, sizeof(cs.in_buf))) == STAT_OK) { + if ((status = usart_gets(cs.in_buf, sizeof(cs.in_buf))) == STAT_OK) { cs.bufp = cs.in_buf; break; } - // handle end-of-file from file devices - if (status == STAT_EOF) { // EOF can come from file devices only - if (cfg.comm_mode == TEXT_MODE) - fprintf_P(stderr, PSTR("End of command file\n")); - else rpt_exception(STAT_EOF); // not really an exception - - tg_reset_source(); // reset to default source - } - return status; // Note: STAT_EAGAIN, errors, etc. will drop through } // set up the buffers - cs.linelen = strlen(cs.in_buf)+1; // linelen only tracks primary input - strncpy(cs.saved_buf, cs.bufp, SAVED_BUFFER_LEN-1); // save input buffer for reporting + cs.linelen = strlen(cs.in_buf) + 1; // linelen only tracks primary input + strncpy(cs.saved_buf, cs.bufp, SAVED_BUFFER_LEN - 1); // save input buffer for reporting // dispatch the new text line switch (toupper(*cs.bufp)) { // first char - case '!': cm_request_feedhold(); break; // include for diagnostics + case '!': cm_request_feedhold(); break; // include for diagnostics case '%': cm_request_queue_flush(); break; case '~': cm_request_cycle_start(); break; - case 0: // blank line (just a CR) + case 0: // blank line (just a CR) if (cfg.comm_mode != JSON_MODE) text_response(STAT_OK, cs.saved_buf); break; @@ -242,28 +227,13 @@ static stat_t _normal_idler() { return STAT_OK; } -/* - * tg_reset_source() - reset source to default input device (see note) - * tg_set_primary_source() - set current primary input source - * tg_set_secondary_source() - set current primary input source - * - * Note: Once multiple serial devices are supported reset_source() should - * be expanded to also set the stdout/stderr console device so the prompt - * and other messages are sent to the active device. - */ - -void tg_reset_source() {tg_set_primary_source(cs.default_src);} -void tg_set_primary_source(uint8_t dev) {cs.primary_src = dev;} -void tg_set_secondary_source(uint8_t dev) {cs.secondary_src = dev;} - /* * _sync_to_tx_buffer() - return eagain if TX queue is backed up * _sync_to_planner() - return eagain if planner is not ready for a new command * _sync_to_time() - return eagain if planner is not ready for a new command */ static stat_t _sync_to_tx_buffer() { - if ((xio_get_tx_bufcount_usart(ds[XIO_DEV_USB].x) >= XOFF_TX_LO_WATER_MARK)) - return STAT_EAGAIN; + if (usart_tx_full()) return STAT_EAGAIN; return STAT_OK; } @@ -296,7 +266,6 @@ stat_t _system_assertions() { system_assert(planner_test_assertions()); system_assert(stepper_test_assertions()); system_assert(encoder_test_assertions()); - system_assert(xio_test_assertions()); return STAT_OK; } diff --git a/src/controller.h b/src/controller.h index dfe0e7d..9a85b53 100755 --- a/src/controller.h +++ b/src/controller.h @@ -45,26 +45,17 @@ typedef struct controllerSingleton { // main TG controller struct float hw_platform; // tinyg hardware compatibility - platform type float hw_version; // tinyg hardware compatibility - platform revision - // communications state variables - uint8_t primary_src; // primary input source device - uint8_t secondary_src; // secondary input source device - uint8_t default_src; // default source device - uint8_t network_mode; // 0=master, 1=repeater, 2=slave - uint16_t linelen; // length of currently processing line uint16_t read_index; // length of line being read // system state variables uint8_t led_state; // LEGACY // 0=off, 1=on - int32_t led_counter; // LEGACY // a convenience for flashing an LED + int32_t led_counter; // LEGACY // a convenience for flashing an LED uint32_t led_timer; // used by idlers to flash indicator LED - uint8_t hard_reset_requested; // flag to perform a hard reset - uint8_t bootloader_requested; // flag to enter the bootloader + uint8_t hard_reset_requested; // flag to perform a hard reset + uint8_t bootloader_requested; // flag to enter the bootloader uint8_t shared_buf_overrun; // flag for shared string buffer overrun condition -// uint8_t sync_to_time_state; -// uint32_t sync_to_time_time; - int32_t job_id[4]; // uuid to identify the job // controller serial buffers @@ -85,16 +76,10 @@ enum cmControllerState { // manages startup lines CONTROLLER_READY // controller is active and ready for use }; -/**** function prototypes ****/ -void controller_init(uint8_t std_in, uint8_t std_out, uint8_t std_err); +void controller_init(); void controller_init_assertions(); stat_t controller_test_assertions(); void controller_run(); -//void controller_reset(); - -void tg_reset_source(); -void tg_set_primary_source(uint8_t dev); -void tg_set_secondary_source(uint8_t dev); #endif // End of include guard: CONTROLLER_H_ONCE diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 2ebf8f4..6bbfb13 100755 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -23,7 +23,6 @@ #include "canonical_machine.h" #include "spindle.h" #include "util.h" -#include "xio/xio.h" // for char definitions struct gcodeParserSingleton { // struct to manage globals uint8_t modals[MODAL_GROUP_COUNT];// collects modal groups in a block diff --git a/src/gpio.c b/src/gpio.c index 21876d2..e40df21 100755 --- a/src/gpio.c +++ b/src/gpio.c @@ -55,7 +55,6 @@ #include "hardware.h" #include "gpio.h" #include "canonical_machine.h" -#include "xio/xio.h" // signals //======================== Parallel IO Functions =============================== diff --git a/src/hardware.h b/src/hardware.h index 51514c1..0061ddf 100755 --- a/src/hardware.h +++ b/src/hardware.h @@ -27,21 +27,22 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ + /* * INTERRUPT USAGE - TinyG uses a lot of them all over the place * - * HI Stepper DDA pulse generation (set in stepper.h) + * HI Stepper DDA pulse generation (set in stepper.h) * HI Stepper load routine SW interrupt (set in stepper.h) - * HI Dwell timer counter (set in stepper.h) - * LO Segment execution SW interrupt (set in stepper.h) - * MED GPIO1 switch port (set in gpio.h) - * MED Serial RX for USB & RS-485 (set in xio_usart.h) - * MED Serial TX for USB & RS-485 (set in xio_usart.h) (* see note) + * HI Dwell timer counter (set in stepper.h) + * LO Segment execution SW interrupt (set in stepper.h) + * MED GPIO1 switch port (set in gpio.h) + * MED Serial RX for USB & RS-485 (set in usart.c) + * MED Serial TX for USB & RS-485 (set in usart.c) (* see note) * LO Real time clock interrupt (set in xmega_rtc.h) * * (*) The TX cannot run at LO level or exception reports and other prints * called from a LO interrupt (as in prep_line()) will kill the system in a - * permanent sleep_mode() call in xio_putc_usb() (xio.usb.c) as no interrupt + * permanent sleep_mode() call in usart_putc() (usart.c) as no interrupt * can release the sleep mode. */ diff --git a/src/json_parser.c b/src/json_parser.c index d069053..df53ebc 100755 --- a/src/json_parser.c +++ b/src/json_parser.c @@ -33,14 +33,9 @@ #include "canonical_machine.h" #include "report.h" #include "util.h" -#include "xio/xio.h" // for char definitions - -/**** Allocation ****/ jsSingleton_t js; -/**** local scope stuff ****/ - static stat_t _json_parser_kernal(char_t *str); static stat_t _get_nv_pair(nvObj_t *nv, char_t **pstr, int8_t *depth); static stat_t _normalize_json_string(char_t *str, uint16_t size); @@ -156,7 +151,7 @@ static stat_t _normalize_json_string(char_t *str, uint16_t size) for (wr = str; *str != 0; str++) { if (!in_comment) { // normal processing if (*str == '(') in_comment = true; - if ((*str <= ' ') || (*str == DEL)) continue; // toss ctrls, WS & DEL + if ((*str <= ' ') || (*str == 0x7f)) continue; // toss ctrls, WS & DEL *wr++ = tolower(*str); } else { // Gcode comment processing if (*str == ')') in_comment = false; diff --git a/src/main.c b/src/main.c index 2d6a379..b7610b7 100755 --- a/src/main.c +++ b/src/main.c @@ -28,11 +28,10 @@ #include "planner.h" #include "stepper.h" #include "encoder.h" -#include "network.h" #include "switch.h" #include "test.h" #include "pwm.h" -#include "xio/xio.h" +#include "usart.h" #include #include "xmega/xmega_interrupts.h" @@ -48,7 +47,7 @@ static void init() { hardware_init(); // system hardware setup - must be first persistence_init(); // set up EEPROM or other NVM - must be second rtc_init(); // real time counter - xio_init(); // eXtended IO subsystem + usart_init(); // serial port // do these next stepper_init(); // stepper subsystem @@ -56,9 +55,8 @@ static void init() { switch_init(); // switches pwm_init(); // pulse width modulation drivers - controller_init(STD_IN, STD_OUT, STD_ERR);// must be first app init; reqs xio_init() + controller_init(); // must be first app init config_init(); // config records from eeprom - must be next app init - network_init(); // reset std devices if required - must follow config_init() planner_init(); // motion planning subsystem canonical_machine_init(); // canonical machine - must follow config_init() diff --git a/src/network.c b/src/network.c deleted file mode 100755 index ac2ba7d..0000000 --- a/src/network.c +++ /dev/null @@ -1,114 +0,0 @@ -/* - * network.c - tinyg networking protocol - * Part of TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* This module is really nothing mre than a placeholder at this time. - * "Networking" refers to a planned RS485 broadcast network to support - * multi-board configs and external RS485 devices such as extruders. - * Basic operation of RS485 on the TinyG hardware has been verified - * using what's in this file, but you won;t find much more. - */ - -#include // for tests - -#include "tinyg.h" -#include "network.h" -#include "controller.h" -#include "gpio.h" -#include "hardware.h" -#include "xio/xio.h" - -/* - * Local Scope Functions and Data - */ - -/* - * network_init() - */ -void network_init() -{ - // re-point IO if in slave mode - if (cs.network_mode == NETWORK_SLAVE) { - controller_init(XIO_DEV_RS485, XIO_DEV_USB, XIO_DEV_USB); - tg_set_secondary_source(XIO_DEV_USB); - } - xio_enable_rs485_rx(); // needed for clean start for RS-485; -} - -void net_forward(unsigned char c) -{ - xio_putc(XIO_DEV_RS485, c); // write to RS485 port -} - -/* - * net_test_rxtx() - test transmission from master to slave - * net_test_loopback() - test transmission from master to slave and looping back - */ - -uint8_t net_test_rxtx(uint8_t c) -{ - int d; - - // master operation - if (cs.network_mode == NETWORK_MASTER) { - if ((c < 0x20) || (c >= 0x7F)) { c = 0x20; } - c++; - xio_putc(XIO_DEV_RS485, c); // write to RS485 port - xio_putc(XIO_DEV_USB, c); // write to USB port - _delay_ms(2); - - // slave operation - } else { - if ((d = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { - xio_putc(XIO_DEV_USB, d); - } - } - return c; -} - -uint8_t net_test_loopback(uint8_t c) -{ - if (cs.network_mode == NETWORK_MASTER) { - // send a character - if ((c < 0x20) || (c >= 0x7F)) { c = 0x20; } - c++; - xio_putc(XIO_DEV_RS485, c); // write to RS485 port - - // wait for loopback character - while (true) { - if ((c = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { - xio_putc(XIO_DEV_USB, c); // write to USB port - } - } - } else { - if ((c = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { - xio_putc(XIO_DEV_RS485, c); // write back to master - xio_putc(XIO_DEV_USB, c); // write to slave USB - } - } - _delay_ms(2); - return c; -} - diff --git a/src/network.h b/src/network.h deleted file mode 100755 index 9ef2c4f..0000000 --- a/src/network.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * network.h - tinyg networking protocol - * Part of TinyG project - * - * Copyright (c) 2011 - 2012 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -#ifndef network_h -#define network_h - -/* - * Global Scope Functions - */ - -enum networkMode { - NETWORK_STANDALONE = 0, - NETWORK_MASTER, - NETWORK_SLAVE -}; - -void network_init(); -void net_forward(unsigned char c); -uint8_t net_test_rxtx(uint8_t c); -uint8_t net_test_loopback(uint8_t c); - -#define XIO_DEV_NET XIO_DEV_RS485 // define the network channel -//#define net_forward(c) (xio_putc(XIO_DEV_NET,c)) - -#endif diff --git a/src/report.c b/src/report.c index 72b961d..96dc2ff 100755 --- a/src/report.c +++ b/src/report.c @@ -34,7 +34,7 @@ #include "planner.h" #include "settings.h" #include "util.h" -#include "xio/xio.h" +#include "usart.h" /**** Allocation ****/ @@ -504,17 +504,20 @@ stat_t qr_queue_report_callback() // called by controller dispatcher } } qr_init_queue_report(); + return STAT_OK; } + /* * rx_request_rx_report() - request an update on usb serial buffer space available */ void rx_request_rx_report() { rx.rx_report_requested = true; - rx.space_available = xio_get_usb_rx_free(); + rx.space_available = usart_rx_space(); } + /* * rx_report_callback() - send rx report if one has been requested */ @@ -528,21 +531,6 @@ stat_t rx_report_callback() { return STAT_OK; } -/* Alternate Formulation for a Single report - using nvObj list - - // get a clean nv object -// nvObj_t *nv = nv_reset_nv_list(); // normally you do a list reset but the following is more time efficient - nvObj_t *nv = nv_body; - nv_reset_nv(nv); - nv->nx = 0; // terminate the list - - // make a qr object and print it - sprintf_P(nv->token, PSTR("qr")); - nv->value = qr.buffers_available; - nv->valuetype = TYPE_INTEGER; - nv_print_list(STAT_OK, TEXT_INLINE_PAIRS, JSON_OBJECT_FORMAT); - return STAT_OK; -*/ /* * Wrappers and Setters - for calling from cfgArray table @@ -551,29 +539,29 @@ stat_t rx_report_callback() { * qi_get() - run a queue report - buffers in * qo_get() - run a queue report - buffers out */ -stat_t qr_get(nvObj_t *nv) -{ +stat_t qr_get(nvObj_t *nv) { nv->value = (float)mp_get_planner_buffers_available(); // ensure that manually requested QR count is always up to date nv->valuetype = TYPE_INTEGER; return STAT_OK; } -stat_t qi_get(nvObj_t *nv) -{ + +stat_t qi_get(nvObj_t *nv) { nv->value = (float)qr.buffers_added; nv->valuetype = TYPE_INTEGER; qr.buffers_added = 0; // reset it return STAT_OK; } -stat_t qo_get(nvObj_t *nv) -{ + +stat_t qo_get(nvObj_t *nv) { nv->value = (float)qr.buffers_removed; nv->valuetype = TYPE_INTEGER; qr.buffers_removed = 0; // reset it return STAT_OK; } + /***************************************************************************** * JOB ID REPORTS * @@ -584,8 +572,7 @@ stat_t qo_get(nvObj_t *nv) * job_set() * job_print_job() */ -stat_t job_populate_job_report() -{ +stat_t job_populate_job_report() { const char_t job_str[] = "job"; char_t tmp[TOKEN_LEN+1]; nvObj_t *nv = nv_reset_nv_list(); // sets *nv to the start of the body diff --git a/src/ringbuf.def b/src/ringbuf.def new file mode 100644 index 0000000..b99cc5f --- /dev/null +++ b/src/ringbuf.def @@ -0,0 +1,144 @@ +/******************************************************************************\ + + Siren Operated Sensor Firmware + Version IX + + Joseph Coffland + Cauldron Development LLC + + November, 2013 - February 2014 + +\******************************************************************************/ + +/* + This file defines an X-Macro ring buffer. It can be used like this: + + #define RING_BUF_NAME tx_buf + #define RING_BUF_SIZE 256 + #include "ringbuf.def" + + This will define the following functions: + + void _init(); + int _empty(); + int _full(); + _peek(); + void _pop(); + void _push( data); + + Where is defined by RING_BUF_NAME and by RING_BUF_TYPE. + RING_BUF_SIZE defines the length of the ring buffer and must be a power of 2. + + The data type and index type both default to uint8_t but can be changed by + defining RING_BUF_TYPE and RING_BUF_INDEX_TYPE respectively. + + By default these functions are declared static inline but this can be changed + by defining RING_BUF_FUNC. +*/ + +#include + +#ifndef RING_BUF_NAME +#error Must define RING_BUF_NAME +#endif + +#ifndef RING_BUF_SIZE +#error Must define RING_BUF_SIZE +#endif + +#ifndef RING_BUF_TYPE +#define RING_BUF_TYPE uint8_t +#endif + +#ifndef RING_BUF_INDEX_TYPE +#define RING_BUF_INDEX_TYPE uint8_t +#endif + +#ifndef RING_BUF_FUNC +#define RING_BUF_FUNC static inline +#endif + +#define RING_BUF_MASK (RING_BUF_SIZE - 1) +#if (RING_BUF_SIZE & RING_BUF_MASK) +#error RING_BUF_SIZE is not a power of 2 +#endif + +#ifndef CONCAT +#define _CONCAT(prefix, name) prefix##name +#define CONCAT(prefix, name) _CONCAT(prefix, name) +#endif + +#define RING_BUF_STRUCT CONCAT(RING_BUF_NAME, _ring_buf_t) +#define RING_BUF CONCAT(RING_BUF_NAME, _ring_buf) + +typedef struct { + RING_BUF_TYPE buf[RING_BUF_SIZE]; + volatile RING_BUF_INDEX_TYPE head; + volatile RING_BUF_INDEX_TYPE tail; +} RING_BUF_STRUCT; + +static RING_BUF_STRUCT RING_BUF; + + +RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _init)() { + RING_BUF.head = RING_BUF.tail = 0; +} + + +#define RING_BUF_INC CONCAT(RING_BUF_NAME, _inc) +RING_BUF_FUNC RING_BUF_INDEX_TYPE RING_BUF_INC(RING_BUF_INDEX_TYPE x) { + return (x + 1) & RING_BUF_MASK; +} + + +RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _empty)() { + return RING_BUF.head == RING_BUF.tail; +} + + +RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _full)() { + return RING_BUF.head == RING_BUF_INC(RING_BUF.tail); +} + + +RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _fill)() { + return (RING_BUF.tail - RING_BUF.head) & RING_BUF_MASK; +} + + +RING_BUF_FUNC int CONCAT(RING_BUF_NAME, _space)() { + return RING_BUF_SIZE - CONCAT(RING_BUF_NAME, _fill)(); +} + + +RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _peek)() { + return RING_BUF.buf[RING_BUF.head]; +} + + +RING_BUF_FUNC RING_BUF_TYPE CONCAT(RING_BUF_NAME, _get)(int offset) { + return RING_BUF.buf[(RING_BUF.head + offset) & RING_BUF_MASK]; +} + + +RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _pop)() { + RING_BUF.head = RING_BUF_INC(RING_BUF.head); +} + + +RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _push)(RING_BUF_TYPE data) { + RING_BUF.buf[RING_BUF.tail] = data; + RING_BUF.tail = RING_BUF_INC(RING_BUF.tail); +} + + +#undef RING_BUF +#undef RING_BUF_STRUCT +#undef RING_BUF_INC +#undef RING_BUF_MASK + +#undef RING_BUF_NAME +#undef RING_BUF_SIZE +#undef RING_BUF_TYPE +#undef RING_BUF_INDEX_TYPE +#undef RING_BUF_FUNC diff --git a/src/system.h b/src/system.h index b3a1357..cf8022d 100755 --- a/src/system.h +++ b/src/system.h @@ -27,13 +27,13 @@ /* * INTERRUPT USAGE - TinyG uses a lot of them all over the place * - * HI Stepper DDA pulse generation (set in stepper.h) + * HI Stepper DDA pulse generation (set in stepper.h) * HI Stepper load routine SW interrupt (set in stepper.h) - * HI Dwell timer counter (set in stepper.h) - * LO Segment execution SW interrupt (set in stepper.h) - * MED GPIO1 switch port (set in gpio.h) - * MED Serial RX for USB & RS-485 (set in xio_usart.h) - * LO Serial TX for USB & RS-485 (set in xio_usart.h) + * HI Dwell timer counter (set in stepper.h) + * LO Segment execution SW interrupt (set in stepper.h) + * MED GPIO1 switch port (set in gpio.h) + * MED Serial RX (set in usart.c) + * LO Serial TX (set in usart.c) * LO Real time clock interrupt (set in xmega_rtc.h) */ #ifndef system_h diff --git a/src/test.c b/src/test.c index 6fb16f3..cea709a 100755 --- a/src/test.c +++ b/src/test.c @@ -17,13 +17,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 +#include "tinyg.h" // #1 #include "config.h" // #2 #include "controller.h" #include "planner.h" #include "test.h" #include "util.h" -#include "xio/xio.h" // regression test files #ifdef __CANNED_TESTS @@ -61,32 +60,30 @@ uint8_t run_test(nvObj_t *nv) { switch ((uint8_t)nv->value) { case 0: return STAT_OK; #ifdef __CANNED_TESTS - case 1: xio_open(XIO_DEV_PGM, PGMFILE(&test_smoke),PGM_FLAGS); break; - case 2: xio_open(XIO_DEV_PGM, PGMFILE(&test_homing),PGM_FLAGS); break; - case 3: xio_open(XIO_DEV_PGM, PGMFILE(&test_squares),PGM_FLAGS); break; - case 4: xio_open(XIO_DEV_PGM, PGMFILE(&test_arcs),PGM_FLAGS); break; - case 5: xio_open(XIO_DEV_PGM, PGMFILE(&test_dwell),PGM_FLAGS); break; - case 6: xio_open(XIO_DEV_PGM, PGMFILE(&test_feedhold),PGM_FLAGS); break; - case 7: xio_open(XIO_DEV_PGM, PGMFILE(&test_Mcodes),PGM_FLAGS); break; - case 8: xio_open(XIO_DEV_PGM, PGMFILE(&test_json),PGM_FLAGS); break; - case 9: xio_open(XIO_DEV_PGM, PGMFILE(&test_inverse_time),PGM_FLAGS); break; - case 10: xio_open(XIO_DEV_PGM, PGMFILE(&test_rotary),PGM_FLAGS); break; - case 11: xio_open(XIO_DEV_PGM, PGMFILE(&test_small_moves),PGM_FLAGS); break; - case 12: xio_open(XIO_DEV_PGM, PGMFILE(&test_slow_moves),PGM_FLAGS); break; - case 13: xio_open(XIO_DEV_PGM, PGMFILE(&test_coordinate_offsets),PGM_FLAGS); break; - case 14: xio_open(XIO_DEV_PGM, PGMFILE(&test_microsteps),PGM_FLAGS); break; - case 50: xio_open(XIO_DEV_PGM, PGMFILE(&test_mudflap),PGM_FLAGS); break; - case 51: xio_open(XIO_DEV_PGM, PGMFILE(&test_braid),PGM_FLAGS); break; + case 1: test_open(PGMFILE(&test_smoke),PGM_FLAGS); break; + case 2: test_open(PGMFILE(&test_homing),PGM_FLAGS); break; + case 3: test_open(PGMFILE(&test_squares),PGM_FLAGS); break; + case 4: test_open(PGMFILE(&test_arcs),PGM_FLAGS); break; + case 5: test_open(PGMFILE(&test_dwell),PGM_FLAGS); break; + case 6: test_open(PGMFILE(&test_feedhold),PGM_FLAGS); break; + case 7: test_open(PGMFILE(&test_Mcodes),PGM_FLAGS); break; + case 8: test_open(PGMFILE(&test_json),PGM_FLAGS); break; + case 9: test_open(PGMFILE(&test_inverse_time),PGM_FLAGS); break; + case 10: test_open(PGMFILE(&test_rotary),PGM_FLAGS); break; + case 11: test_open(PGMFILE(&test_small_moves),PGM_FLAGS); break; + case 12: test_open(PGMFILE(&test_slow_moves),PGM_FLAGS); break; + case 13: test_open(PGMFILE(&test_coordinate_offsets),PGM_FLAGS); break; + case 14: test_open(PGMFILE(&test_microsteps),PGM_FLAGS); break; + case 50: test_open(PGMFILE(&test_mudflap),PGM_FLAGS); break; + case 51: test_open(PGMFILE(&test_braid),PGM_FLAGS); break; #endif #ifdef __TEST_99 - case 99: xio_open(XIO_DEV_PGM, PGMFILE(&test_99),PGM_FLAGS); break; + case 99: test_open(PGMFILE(&test_99),PGM_FLAGS); break; #endif default: fprintf_P(stderr,PSTR("Test #%d not found\n"),(uint8_t)nv->value); return STAT_ERROR; } - tg_set_primary_source(XIO_DEV_PGM); - return STAT_OK; } diff --git a/src/text_parser.c b/src/text_parser.c index 92d0cc7..d9cf2ad 100755 --- a/src/text_parser.c +++ b/src/text_parser.c @@ -33,7 +33,6 @@ #include "report.h" #include "help.h" #include "util.h" -#include "xio/xio.h" // for ASCII char definitions txtSingleton_t txt; // declare the singleton for either __TEXT_MODE setting @@ -59,222 +58,212 @@ static stat_t _text_parser_kernal(char_t *str, nvObj_t *nv); * - $x display a group * - ? generate a status report (multiline format) */ -stat_t text_parser(char_t *str) -{ - nvObj_t *nv = nv_reset_nv_list(); // returns first object in the body - stat_t status = STAT_OK; - - // trap special displays - if (str[0] == '?') { // handle status report case - sr_run_text_status_report(); - return STAT_OK; - } - if (str[0] == 'H') { // print help screens - help_general((nvObj_t *)0); - return STAT_OK; - } +stat_t text_parser(char_t *str) { + nvObj_t *nv = nv_reset_nv_list(); // returns first object in the body + stat_t status = STAT_OK; - // pre-process the command - if ((str[0] == '$') && (str[1] == 0)) { // treat a lone $ as a sys request - strcat(str,"sys"); - } + // trap special displays + if (str[0] == '?') { // handle status report case + sr_run_text_status_report(); + return STAT_OK; + } - // parse and execute the command (only processes 1 command per line) - ritorno(_text_parser_kernal(str, nv)); // run the parser to decode the command - if ((nv->valuetype == TYPE_0) || (nv->valuetype == TYPE_PARENT)) { - if (nv_get(nv) == STAT_COMPLETE){ // populate value, group values, or run uber-group displays - return STAT_OK; // return for uber-group displays so they don't print twice - } - } else { // process SET and RUN commands - if (cm.machine_state == MACHINE_ALARM) - return STAT_MACHINE_ALARMED; - status = nv_set(nv); // set (or run) single value - if (status == STAT_OK) { - nv_persist(nv); // conditionally persist depending on flags in array - } - } - nv_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); // print the results - return status; + if (str[0] == 'H') { // print help screens + help_general((nvObj_t *)0); + return STAT_OK; + } + + // pre-process the command + if ((str[0] == '$') && (str[1] == 0)) // treat a lone $ as a sys request + strcat(str,"sys"); + + // parse and execute the command (only processes 1 command per line) + ritorno(_text_parser_kernal(str, nv)); // run the parser to decode the command + if ((nv->valuetype == TYPE_0) || (nv->valuetype == TYPE_PARENT)) { + if (nv_get(nv) == STAT_COMPLETE) // populate value, group values, or run uber-group displays + return STAT_OK; // return for uber-group displays so they don't print twice + + } else { // process SET and RUN commands + if (cm.machine_state == MACHINE_ALARM) return STAT_MACHINE_ALARMED; + status = nv_set(nv); // set (or run) single value + if (status == STAT_OK) nv_persist(nv); // conditionally persist depending on flags in array + } + nv_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); // print the results + return status; } -static stat_t _text_parser_kernal(char_t *str, nvObj_t *nv) -{ - char_t *rd, *wr; // read and write pointers -// char_t separators[] = {"="}; // STRICT: only separator allowed is = sign - char_t separators[] = {" =:|\t"}; // RELAXED: any separator someone might use - - // pre-process and normalize the string -// nv_reset_nv(nv); // initialize config object - nv_copy_string(nv, str); // make a copy for eventual reporting - if (*str == '$') str++; // ignore leading $ - for (rd = wr = str; *rd != 0; rd++, wr++) { - *wr = tolower(*rd); // convert string to lower case - if (*rd == ',') { *wr = *(++rd);} // skip over commas - } - *wr = 0; // terminate the string - - // parse fields into the nv struct - nv->valuetype = TYPE_0; - if ((rd = strpbrk(str, separators)) == 0) { // no value part - strncpy(nv->token, str, TOKEN_LEN); - } else { - *rd = 0; // terminate at end of name - strncpy(nv->token, str, TOKEN_LEN); - str = ++rd; - nv->value = strtof(str, &rd); // rd used as end pointer - if (rd != str) { - nv->valuetype = TYPE_FLOAT; - } - } - // validate and post-process the token - if ((nv->index = nv_get_index((const char_t *)"", nv->token)) == NO_MATCH) { // get index or fail it - return STAT_UNRECOGNIZED_NAME; - } - strcpy_P(nv->group, cfgArray[nv->index].group); // capture the group string if there is one - - // see if you need to strip the token - if (nv->group[0] != 0) { - wr = nv->token; - rd = nv->token + strlen(nv->group); - while (*rd != 0) { *(wr)++ = *(rd)++;} - *wr = 0; - } - return STAT_OK; +static stat_t _text_parser_kernal(char_t *str, nvObj_t *nv) { + char_t *rd, *wr; // read and write pointers + char_t separators[] = {" =:|\t"}; // RELAXED: any separator someone might use + + // pre-process and normalize the string + nv_copy_string(nv, str); // make a copy for eventual reporting + if (*str == '$') str++; // ignore leading $ + + for (rd = wr = str; *rd != 0; rd++, wr++) { + *wr = tolower(*rd); // convert string to lower case + if (*rd == ',') { *wr = *(++rd);} // skip over commas + } + *wr = 0; // terminate the string + + // parse fields into the nv struct + nv->valuetype = TYPE_0; + if ((rd = strpbrk(str, separators)) == 0) { // no value part + strncpy(nv->token, str, TOKEN_LEN); + } else { + *rd = 0; // terminate at end of name + strncpy(nv->token, str, TOKEN_LEN); + str = ++rd; + nv->value = strtof(str, &rd); // rd used as end pointer + if (rd != str) nv->valuetype = TYPE_FLOAT; + } + + // validate and post-process the token + if ((nv->index = nv_get_index((const char_t *)"", nv->token)) == NO_MATCH) // get index or fail it + return STAT_UNRECOGNIZED_NAME; + + strcpy_P(nv->group, cfgArray[nv->index].group); // capture the group string if there is one + + // see if you need to strip the token + if (nv->group[0] != 0) { + wr = nv->token; + rd = nv->token + strlen(nv->group); + while (*rd != 0) *(wr)++ = *(rd)++; + *wr = 0; + } + + return STAT_OK; } -/************************************************************************************ - * text_response() - text mode responses - */ + +/// text mode responses static const char prompt_ok[] PROGMEM = "tinyg [%s] ok> "; static const char prompt_err[] PROGMEM = "tinyg [%s] err: %s: %s "; -void text_response(const stat_t status, char_t *buf) -{ - if (txt.text_verbosity == TV_SILENT) return; // skip all this +void text_response(const stat_t status, char_t *buf) { + if (txt.text_verbosity == TV_SILENT) return; // skip all this - char units[] = "inch"; - if (cm_get_units_mode(MODEL) != INCHES) { strcpy(units, "mm"); } + char units[] = "inch"; + if (cm_get_units_mode(MODEL) != INCHES) strcpy(units, "mm"); - if ((status == STAT_OK) || (status == STAT_EAGAIN) || (status == STAT_NOOP)) { - fprintf_P(stderr, prompt_ok, units); - } else { - fprintf_P(stderr, prompt_err, units, get_status_message(status), buf); - } - nvObj_t *nv = nv_body+1; + if ((status == STAT_OK) || (status == STAT_EAGAIN) || (status == STAT_NOOP)) + fprintf_P(stderr, prompt_ok, units); + else fprintf_P(stderr, prompt_err, units, get_status_message(status), buf); - if (nv_get_type(nv) == NV_TYPE_MESSAGE) { - fprintf(stderr, (char *)*nv->stringp); - } - fprintf(stderr, "\n"); + nvObj_t *nv = nv_body + 1; + + if (nv_get_type(nv) == NV_TYPE_MESSAGE) fprintf(stderr, (char *)*nv->stringp); + + fprintf(stderr, "\n"); } + /***** PRINT FUNCTIONS ******************************************************** * json_print_list() - command to select and produce a JSON formatted output * text_print_inline_pairs() * text_print_inline_values() * text_print_multiline_formatted() */ - -void text_print_list(stat_t status, uint8_t flags) -{ - switch (flags) { - case TEXT_NO_PRINT: { break; } - case TEXT_INLINE_PAIRS: { text_print_inline_pairs(nv_body); break; } - case TEXT_INLINE_VALUES: { text_print_inline_values(nv_body); break; } - case TEXT_MULTILINE_FORMATTED: { text_print_multiline_formatted(nv_body);} - } +void text_print_list(stat_t status, uint8_t flags) { + switch (flags) { + case TEXT_NO_PRINT: break; + case TEXT_INLINE_PAIRS: text_print_inline_pairs(nv_body); break; + case TEXT_INLINE_VALUES: text_print_inline_values(nv_body); break; + case TEXT_MULTILINE_FORMATTED: text_print_multiline_formatted(nv_body); + } } -void text_print_inline_pairs(nvObj_t *nv) -{ - uint32_t *v = (uint32_t*)&nv->value; - for (uint8_t i=0; ivaluetype) { - case TYPE_PARENT: { if ((nv = nv->nx) == 0) return; continue;} // 0 means parent with no child - case TYPE_FLOAT: { preprocess_float(nv); - fntoa(global_string_buf, nv->value, nv->precision); - fprintf_P(stderr,PSTR("%s:%s"), nv->token, global_string_buf) ; break; - } - case TYPE_INTEGER: { fprintf_P(stderr,PSTR("%s:%1.0f"), nv->token, nv->value); break;} - case TYPE_DATA: { fprintf_P(stderr,PSTR("%s:%lu"), nv->token, *v); break;} - case TYPE_STRING: { fprintf_P(stderr,PSTR("%s:%s"), nv->token, *nv->stringp); break;} - case TYPE_EMPTY: { fprintf_P(stderr,PSTR("\n")); return; } - } - if ((nv = nv->nx) == 0) return; - if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} + +void text_print_inline_pairs(nvObj_t *nv) { + uint32_t *v = (uint32_t*)&nv->value; + for (uint8_t i=0; ivaluetype) { + case TYPE_PARENT: if ((nv = nv->nx) == 0) return; continue; // 0 means parent with no child + case TYPE_FLOAT: + preprocess_float(nv); + fntoa(global_string_buf, nv->value, nv->precision); + fprintf_P(stderr, PSTR("%s:%s"), nv->token, global_string_buf); + break; + case TYPE_INTEGER: fprintf_P(stderr,PSTR("%s:%1.0f"), nv->token, nv->value); break; + case TYPE_DATA: fprintf_P(stderr,PSTR("%s:%lu"), nv->token, *v); break; + case TYPE_STRING: fprintf_P(stderr,PSTR("%s:%s"), nv->token, *nv->stringp); break; + case TYPE_EMPTY: fprintf_P(stderr,PSTR("\n")); return; } + if ((nv = nv->nx) == 0) return; + if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} + } } -void text_print_inline_values(nvObj_t *nv) -{ - uint32_t *v = (uint32_t*)&nv->value; - for (uint8_t i=0; ivaluetype) { - case TYPE_PARENT: { if ((nv = nv->nx) == 0) return; continue;} // 0 means parent with no child - case TYPE_FLOAT: { preprocess_float(nv); - fntoa(global_string_buf, nv->value, nv->precision); - fprintf_P(stderr,PSTR("%s"), global_string_buf) ; break; - } - case TYPE_INTEGER: { fprintf_P(stderr,PSTR("%1.0f"), nv->value); break;} - case TYPE_DATA: { fprintf_P(stderr,PSTR("%lu"), *v); break;} - case TYPE_STRING: { fprintf_P(stderr,PSTR("%s"), *nv->stringp); break;} - case TYPE_EMPTY: { fprintf_P(stderr,PSTR("\n")); return; } - } - if ((nv = nv->nx) == 0) return; - if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} + +void text_print_inline_values(nvObj_t *nv) { + uint32_t *v = (uint32_t*)&nv->value; + for (uint8_t i=0; ivaluetype) { + case TYPE_PARENT: if ((nv = nv->nx) == 0) return; continue; // 0 means parent with no child + case TYPE_FLOAT: + preprocess_float(nv); + fntoa(global_string_buf, nv->value, nv->precision); + fprintf_P(stderr,PSTR("%s"), global_string_buf); + break; + case TYPE_INTEGER: fprintf_P(stderr,PSTR("%1.0f"), nv->value); break; + case TYPE_DATA: fprintf_P(stderr,PSTR("%lu"), *v); break; + case TYPE_STRING: fprintf_P(stderr,PSTR("%s"), *nv->stringp); break; + case TYPE_EMPTY: fprintf_P(stderr,PSTR("\n")); return; } + + if ((nv = nv->nx) == 0) return; + if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} + } } -void text_print_multiline_formatted(nvObj_t *nv) -{ - for (uint8_t i=0; ivaluetype != TYPE_PARENT) { - preprocess_float(nv); - nv_print(nv); - } - if ((nv = nv->nx) == 0) return; - if (nv->valuetype == TYPE_EMPTY) break; + +void text_print_multiline_formatted(nvObj_t *nv) { + for (uint8_t i=0; ivaluetype != TYPE_PARENT) { + preprocess_float(nv); + nv_print(nv); } + + if ((nv = nv->nx) == 0) return; + if (nv->valuetype == TYPE_EMPTY) break; + } } -/* - * Text print primitives using generic formats - */ + +/// Text print primitives using generic formats static const char fmt_str[] PROGMEM = "%s\n"; // generic format for string message (with no formatting) static const char fmt_ui8[] PROGMEM = "%d\n"; // generic format for ui8s static const char fmt_int[] PROGMEM = "%lu\n"; // generic format for ui16's and ui32s static const char fmt_flt[] PROGMEM = "%f\n"; // generic format for floats void tx_print_nul(nvObj_t *nv) {} -void tx_print_str(nvObj_t *nv) { text_print_str(nv, fmt_str);} -void tx_print_ui8(nvObj_t *nv) { text_print_ui8(nv, fmt_ui8);} -void tx_print_int(nvObj_t *nv) { text_print_int(nv, fmt_int);} -void tx_print_flt(nvObj_t *nv) { text_print_flt(nv, fmt_flt);} +void tx_print_str(nvObj_t *nv) {text_print_str(nv, fmt_str);} +void tx_print_ui8(nvObj_t *nv) {text_print_ui8(nv, fmt_ui8);} +void tx_print_int(nvObj_t *nv) {text_print_int(nv, fmt_int);} +void tx_print_flt(nvObj_t *nv) {text_print_flt(nv, fmt_flt);} + /* * Text print primitives using external formats * * NOTE: format's are passed in as flash strings (PROGMEM) */ +void text_print_nul(nvObj_t *nv, const char *format) {fprintf_P(stderr, format);} // just print the format string +void text_print_str(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, *nv->stringp);} +void text_print_ui8(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, (uint8_t)nv->value);} +void text_print_int(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, (uint32_t)nv->value);} +void text_print_flt(nvObj_t *nv, const char *format) {fprintf_P(stderr, format, nv->value);} -void text_print_nul(nvObj_t *nv, const char *format) { fprintf_P(stderr, format);} // just print the format string -void text_print_str(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, *nv->stringp);} -void text_print_ui8(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, (uint8_t)nv->value);} -void text_print_int(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, (uint32_t)nv->value);} -void text_print_flt(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, nv->value);} -void text_print_flt_units(nvObj_t *nv, const char *format, const char *units) -{ - fprintf_P(stderr, format, nv->value, units); +void text_print_flt_units(nvObj_t *nv, const char *format, const char *units) { + fprintf_P(stderr, format, nv->value, units); } -/* - * Formatted print supporting the text parser - */ -static const char fmt_tv[] PROGMEM = "[tv] text verbosity%15d [0=silent,1=verbose]\n"; -void tx_print_tv(nvObj_t *nv) { text_print_ui8(nv, fmt_tv);} +/// Formatted print supporting the text parser +static const char fmt_tv[] PROGMEM = "[tv] text verbosity%15d [0=silent,1=verbose]\n"; +void tx_print_tv(nvObj_t *nv) {text_print_ui8(nv, fmt_tv);} #endif // __TEXT_MODE diff --git a/src/tinyg.h b/src/tinyg.h index 1a27405..13b9244 100755 --- a/src/tinyg.h +++ b/src/tinyg.h @@ -46,8 +46,8 @@ #define TINYG_FIRMWARE_BUILD 440.20 // arc test #endif -#define TINYG_FIRMWARE_VERSION 0.97 // firmware major version -#define TINYG_HARDWARE_PLATFORM HW_PLATFORM_TINYG_XMEGA // see hardware.h +#define TINYG_FIRMWARE_VERSION 0.97 // firmware major version +#define TINYG_HARDWARE_PLATFORM HW_PLATFORM_TINYG_XMEGA // see hardware.h #define TINYG_HARDWARE_VERSION HW_VERSION_TINYGV8 // see hardware.h #define TINYG_HARDWARE_VERSION_MAX TINYG_HARDWARE_VERSION @@ -87,11 +87,6 @@ typedef char char_t; // get units from array of strings in PGM and convert to RAM string #define GET_UNITS(a) strncpy_P(global_string_buf,(const char *)pgm_read_word(&msg_units[cm_get_units_mode(a)]), MESSAGE_LEN-1) -// IO settings -#define STD_IN XIO_DEV_USB // default IO settings -#define STD_OUT XIO_DEV_USB -#define STD_ERR XIO_DEV_USB - // String compatibility #define strtof strtod // strtof is not in the AVR lib @@ -100,16 +95,16 @@ typedef char char_t; ******************************************************************************/ typedef uint16_t magic_t; // magic number size -#define MAGICNUM 0x12EF // used for memory integrity assertions +#define MAGICNUM 0x12EF // used for memory integrity assertions /***** Axes, motors & PWM channels used by the application *****/ // Axes, motors & PWM channels must be defines (not enums) so #ifdef can be used -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (1-6) -#define PWMS 2 // number of supported PWM channels +#define AXES 6 // number of axes supported in this version +#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) +#define MOTORS 4 // number of motors on the board +#define COORDS 6 // number of supported coordinate systems (1-6) +#define PWMS 2 // number of supported PWM channels // Note: If you change COORDS you must adjust the entries in cfgArray table in config.c @@ -123,7 +118,7 @@ typedef uint16_t magic_t; // magic number size #define AXIS_V 7 // reserved #define AXIS_W 8 // reserved -#define MOTOR_1 0 // define motor numbers and array indexes +#define MOTOR_1 0 // define motor numbers and array indexes #define MOTOR_2 1 // must be defines. enums don't work #define MOTOR_3 2 #define MOTOR_4 3 @@ -136,19 +131,16 @@ typedef uint16_t magic_t; // magic number size /************************************************************************************ * STATUS CODES * - * The first code range (0-19) is aligned with the XIO codes and must be so. - * Please don't change them without checking the corresponding values in xio.h - * * Status codes are divided into ranges for clarity and extensibility. At some point * this may break down and the whole thing will get messy(er), but it's advised not * to change the values of existing status codes once they are in distribution. * * Ranges are: * - * 0 - 19 OS, communications and low-level status (must align with XIO_xxxx codes in xio.h) + * 0 - 19 OS, communications and low-level status * * 20 - 99 Generic internal and application errors. Internal errors start at 20 and work up, - * Assertion failures start at 99 and work down. + * Assertion failures start at 99 and work down. * * 100 - 129 Generic data and input errors - not specific to Gcode or TinyG * @@ -175,163 +167,162 @@ char *get_status_message(stat_t status); // It returns only if an error occurred. (ritorno is Italian for return) #define ritorno(a) if((status_code=a) != STAT_OK) { return(status_code); } -// OS, communications and low-level status (must align with XIO_xxxx codes in xio.h) -#define STAT_OK 0 // function completed OK -#define STAT_ERROR 1 // generic error return EPERM -#define STAT_EAGAIN 2 // function would block here (call again) -#define STAT_NOOP 3 // function had no-operation -#define STAT_COMPLETE 4 // operation is complete +// OS, communications and low-level status +#define STAT_OK 0 // function completed OK +#define STAT_ERROR 1 // generic error return EPERM +#define STAT_EAGAIN 2 // function would block here (call again) +#define STAT_NOOP 3 // function had no-operation +#define STAT_COMPLETE 4 // operation is complete #define STAT_TERMINATE 5 // operation terminated (gracefully) #define STAT_RESET 6 // operation was hard reset (sig kill) -#define STAT_EOL 7 // function returned end-of-line -#define STAT_EOF 8 // function returned end-of-file -#define STAT_FILE_NOT_OPEN 9 -#define STAT_FILE_SIZE_EXCEEDED 10 -#define STAT_NO_SUCH_DEVICE 11 -#define STAT_BUFFER_EMPTY 12 -#define STAT_BUFFER_FULL 13 -#define STAT_BUFFER_FULL_FATAL 14 -#define STAT_INITIALIZING 15 // initializing - not ready for use -#define STAT_ENTERING_BOOT_LOADER 16 // this code actually emitted from boot loader, not TinyG -#define STAT_FUNCTION_IS_STUBBED 17 -#define STAT_ERROR_18 18 -#define STAT_ERROR_19 19 // NOTE: XIO codes align to here +#define STAT_EOL 7 // function returned end-of-line +#define STAT_EOF 8 // function returned end-of-file +#define STAT_FILE_NOT_OPEN 9 +#define STAT_FILE_SIZE_EXCEEDED 10 +#define STAT_NO_SUCH_DEVICE 11 +#define STAT_BUFFER_EMPTY 12 +#define STAT_BUFFER_FULL 13 +#define STAT_BUFFER_FULL_FATAL 14 +#define STAT_INITIALIZING 15 // initializing - not ready for use +#define STAT_ENTERING_BOOT_LOADER 16 // this code actually emitted from boot loader, not TinyG +#define STAT_FUNCTION_IS_STUBBED 17 +#define STAT_ERROR_18 18 +#define STAT_ERROR_19 19 // Internal errors and startup messages -#define STAT_INTERNAL_ERROR 20 // unrecoverable internal error -#define STAT_INTERNAL_RANGE_ERROR 21 // number range other than by user input -#define STAT_FLOATING_POINT_ERROR 22 // number conversion error -#define STAT_DIVIDE_BY_ZERO 23 -#define STAT_INVALID_ADDRESS 24 -#define STAT_READ_ONLY_ADDRESS 25 -#define STAT_INIT_FAIL 26 -#define STAT_ALARMED 27 -#define STAT_FAILED_TO_GET_PLANNER_BUFFER 28 +#define STAT_INTERNAL_ERROR 20 // unrecoverable internal error +#define STAT_INTERNAL_RANGE_ERROR 21 // number range other than by user input +#define STAT_FLOATING_POINT_ERROR 22 // number conversion error +#define STAT_DIVIDE_BY_ZERO 23 +#define STAT_INVALID_ADDRESS 24 +#define STAT_READ_ONLY_ADDRESS 25 +#define STAT_INIT_FAIL 26 +#define STAT_ALARMED 27 +#define STAT_FAILED_TO_GET_PLANNER_BUFFER 28 #define STAT_GENERIC_EXCEPTION_REPORT 29 // used for test -#define STAT_PREP_LINE_MOVE_TIME_IS_INFINITE 30 -#define STAT_PREP_LINE_MOVE_TIME_IS_NAN 31 -#define STAT_FLOAT_IS_INFINITE 32 -#define STAT_FLOAT_IS_NAN 33 -#define STAT_PERSISTENCE_ERROR 34 -#define STAT_BAD_STATUS_REPORT_SETTING 35 -#define STAT_ERROR_36 36 -#define STAT_ERROR_37 37 -#define STAT_ERROR_38 38 -#define STAT_ERROR_39 39 - -#define STAT_ERROR_40 40 -#define STAT_ERROR_41 41 -#define STAT_ERROR_42 42 -#define STAT_ERROR_43 43 -#define STAT_ERROR_44 44 -#define STAT_ERROR_45 45 -#define STAT_ERROR_46 46 -#define STAT_ERROR_47 47 -#define STAT_ERROR_48 48 -#define STAT_ERROR_49 49 - -#define STAT_ERROR_50 50 -#define STAT_ERROR_51 51 -#define STAT_ERROR_52 52 -#define STAT_ERROR_53 53 -#define STAT_ERROR_54 54 -#define STAT_ERROR_55 55 -#define STAT_ERROR_56 56 -#define STAT_ERROR_57 57 -#define STAT_ERROR_58 58 -#define STAT_ERROR_59 59 - -#define STAT_ERROR_60 60 -#define STAT_ERROR_61 61 -#define STAT_ERROR_62 62 -#define STAT_ERROR_63 63 -#define STAT_ERROR_64 64 -#define STAT_ERROR_65 65 -#define STAT_ERROR_66 66 -#define STAT_ERROR_67 67 -#define STAT_ERROR_68 68 -#define STAT_ERROR_69 69 - -#define STAT_ERROR_70 70 -#define STAT_ERROR_71 71 -#define STAT_ERROR_72 72 -#define STAT_ERROR_73 73 -#define STAT_ERROR_74 74 -#define STAT_ERROR_75 75 -#define STAT_ERROR_76 76 -#define STAT_ERROR_77 77 -#define STAT_ERROR_78 78 -#define STAT_ERROR_79 79 - -#define STAT_ERROR_80 80 -#define STAT_ERROR_81 81 -#define STAT_ERROR_82 82 -#define STAT_ERROR_83 83 -#define STAT_ERROR_84 84 -#define STAT_ERROR_85 85 -#define STAT_ERROR_86 86 -#define STAT_ERROR_87 87 -#define STAT_ERROR_88 88 -#define STAT_ERROR_89 89 +#define STAT_PREP_LINE_MOVE_TIME_IS_INFINITE 30 +#define STAT_PREP_LINE_MOVE_TIME_IS_NAN 31 +#define STAT_FLOAT_IS_INFINITE 32 +#define STAT_FLOAT_IS_NAN 33 +#define STAT_PERSISTENCE_ERROR 34 +#define STAT_BAD_STATUS_REPORT_SETTING 35 +#define STAT_ERROR_36 36 +#define STAT_ERROR_37 37 +#define STAT_ERROR_38 38 +#define STAT_ERROR_39 39 + +#define STAT_ERROR_40 40 +#define STAT_ERROR_41 41 +#define STAT_ERROR_42 42 +#define STAT_ERROR_43 43 +#define STAT_ERROR_44 44 +#define STAT_ERROR_45 45 +#define STAT_ERROR_46 46 +#define STAT_ERROR_47 47 +#define STAT_ERROR_48 48 +#define STAT_ERROR_49 49 + +#define STAT_ERROR_50 50 +#define STAT_ERROR_51 51 +#define STAT_ERROR_52 52 +#define STAT_ERROR_53 53 +#define STAT_ERROR_54 54 +#define STAT_ERROR_55 55 +#define STAT_ERROR_56 56 +#define STAT_ERROR_57 57 +#define STAT_ERROR_58 58 +#define STAT_ERROR_59 59 + +#define STAT_ERROR_60 60 +#define STAT_ERROR_61 61 +#define STAT_ERROR_62 62 +#define STAT_ERROR_63 63 +#define STAT_ERROR_64 64 +#define STAT_ERROR_65 65 +#define STAT_ERROR_66 66 +#define STAT_ERROR_67 67 +#define STAT_ERROR_68 68 +#define STAT_ERROR_69 69 + +#define STAT_ERROR_70 70 +#define STAT_ERROR_71 71 +#define STAT_ERROR_72 72 +#define STAT_ERROR_73 73 +#define STAT_ERROR_74 74 +#define STAT_ERROR_75 75 +#define STAT_ERROR_76 76 +#define STAT_ERROR_77 77 +#define STAT_ERROR_78 78 +#define STAT_ERROR_79 79 + +#define STAT_ERROR_80 80 +#define STAT_ERROR_81 81 +#define STAT_ERROR_82 82 +#define STAT_ERROR_83 83 +#define STAT_ERROR_84 84 +#define STAT_ERROR_85 85 +#define STAT_ERROR_86 86 +#define STAT_ERROR_87 87 +#define STAT_ERROR_88 88 +#define STAT_ERROR_89 89 // Assertion failures - build down from 99 until they meet the system internal errors -#define STAT_CONFIG_ASSERTION_FAILURE 90 -#define STAT_XIO_ASSERTION_FAILURE 91 -#define STAT_ENCODER_ASSERTION_FAILURE 92 -#define STAT_STEPPER_ASSERTION_FAILURE 93 -#define STAT_PLANNER_ASSERTION_FAILURE 94 -#define STAT_CANONICAL_MACHINE_ASSERTION_FAILURE 95 -#define STAT_CONTROLLER_ASSERTION_FAILURE 96 -#define STAT_STACK_OVERFLOW 97 -#define STAT_MEMORY_FAULT 98 // generic memory corruption detected by magic numbers -#define STAT_GENERIC_ASSERTION_FAILURE 99 // generic assertion failure - unclassified +#define STAT_CONFIG_ASSERTION_FAILURE 90 +#define STAT_ENCODER_ASSERTION_FAILURE 92 +#define STAT_STEPPER_ASSERTION_FAILURE 93 +#define STAT_PLANNER_ASSERTION_FAILURE 94 +#define STAT_CANONICAL_MACHINE_ASSERTION_FAILURE 95 +#define STAT_CONTROLLER_ASSERTION_FAILURE 96 +#define STAT_STACK_OVERFLOW 97 +#define STAT_MEMORY_FAULT 98 // generic memory corruption detected by magic numbers +#define STAT_GENERIC_ASSERTION_FAILURE 99 // generic assertion failure - unclassified // Application and data input errors // Generic data input errors -#define STAT_UNRECOGNIZED_NAME 100 // parser didn't recognize the name -#define STAT_INVALID_OR_MALFORMED_COMMAND 101 // malformed line to parser -#define STAT_BAD_NUMBER_FORMAT 102 // number format error -#define STAT_UNSUPPORTED_TYPE 103 // An otherwise valid number or JSON type is not supported -#define STAT_PARAMETER_IS_READ_ONLY 104 // input error: parameter cannot be set -#define STAT_PARAMETER_CANNOT_BE_READ 105 // input error: parameter cannot be set -#define STAT_COMMAND_NOT_ACCEPTED 106 // command cannot be accepted at this time -#define STAT_INPUT_EXCEEDS_MAX_LENGTH 107 // input string is too long -#define STAT_INPUT_LESS_THAN_MIN_VALUE 108 // input error: value is under minimum -#define STAT_INPUT_EXCEEDS_MAX_VALUE 109 // input error: value is over maximum - -#define STAT_INPUT_VALUE_RANGE_ERROR 110 // input error: value is out-of-range -#define STAT_JSON_SYNTAX_ERROR 111 // JSON input string is not well formed -#define STAT_JSON_TOO_MANY_PAIRS 112 // JSON input string has too many JSON pairs -#define STAT_JSON_TOO_LONG 113 // JSON input or output exceeds buffer size -#define STAT_ERROR_114 114 -#define STAT_ERROR_115 115 -#define STAT_ERROR_116 116 -#define STAT_ERROR_117 117 -#define STAT_ERROR_118 118 -#define STAT_ERROR_119 119 - -#define STAT_ERROR_120 120 -#define STAT_ERROR_121 121 -#define STAT_ERROR_122 122 -#define STAT_ERROR_123 123 -#define STAT_ERROR_124 124 -#define STAT_ERROR_125 125 -#define STAT_ERROR_126 126 -#define STAT_ERROR_127 127 -#define STAT_ERROR_128 128 -#define STAT_ERROR_129 129 +#define STAT_UNRECOGNIZED_NAME 100 // parser didn't recognize the name +#define STAT_INVALID_OR_MALFORMED_COMMAND 101 // malformed line to parser +#define STAT_BAD_NUMBER_FORMAT 102 // number format error +#define STAT_UNSUPPORTED_TYPE 103 // An otherwise valid number or JSON type is not supported +#define STAT_PARAMETER_IS_READ_ONLY 104 // input error: parameter cannot be set +#define STAT_PARAMETER_CANNOT_BE_READ 105 // input error: parameter cannot be set +#define STAT_COMMAND_NOT_ACCEPTED 106 // command cannot be accepted at this time +#define STAT_INPUT_EXCEEDS_MAX_LENGTH 107 // input string is too long +#define STAT_INPUT_LESS_THAN_MIN_VALUE 108 // input error: value is under minimum +#define STAT_INPUT_EXCEEDS_MAX_VALUE 109 // input error: value is over maximum + +#define STAT_INPUT_VALUE_RANGE_ERROR 110 // input error: value is out-of-range +#define STAT_JSON_SYNTAX_ERROR 111 // JSON input string is not well formed +#define STAT_JSON_TOO_MANY_PAIRS 112 // JSON input string has too many JSON pairs +#define STAT_JSON_TOO_LONG 113 // JSON input or output exceeds buffer size +#define STAT_ERROR_114 114 +#define STAT_ERROR_115 115 +#define STAT_ERROR_116 116 +#define STAT_ERROR_117 117 +#define STAT_ERROR_118 118 +#define STAT_ERROR_119 119 + +#define STAT_ERROR_120 120 +#define STAT_ERROR_121 121 +#define STAT_ERROR_122 122 +#define STAT_ERROR_123 123 +#define STAT_ERROR_124 124 +#define STAT_ERROR_125 125 +#define STAT_ERROR_126 126 +#define STAT_ERROR_127 127 +#define STAT_ERROR_128 128 +#define STAT_ERROR_129 129 // Gcode errors and warnings (Most originate from NIST - by concept, not number) // Fascinating: http://www.cncalarms.com/ -#define STAT_GCODE_GENERIC_INPUT_ERROR 130 // generic error for gcode input -#define STAT_GCODE_COMMAND_UNSUPPORTED 131 // G command is not supported -#define STAT_MCODE_COMMAND_UNSUPPORTED 132 // M command is not supported -#define STAT_GCODE_MODAL_GROUP_VIOLATION 133 // gcode modal group error -#define STAT_GCODE_AXIS_IS_MISSING 134 // command requires at least one axis present +#define STAT_GCODE_GENERIC_INPUT_ERROR 130 // generic error for gcode input +#define STAT_GCODE_COMMAND_UNSUPPORTED 131 // G command is not supported +#define STAT_MCODE_COMMAND_UNSUPPORTED 132 // M command is not supported +#define STAT_GCODE_MODAL_GROUP_VIOLATION 133 // gcode modal group error +#define STAT_GCODE_AXIS_IS_MISSING 134 // command requires at least one axis present #define STAT_GCODE_AXIS_CANNOT_BE_PRESENT 135 // error if G80 has axis words #define STAT_GCODE_AXIS_IS_INVALID 136 // an axis is specified that is illegal for the command #define STAT_GCODE_AXIS_IS_NOT_CONFIGURED 137 // WARNING: attempt to program an axis that is disabled @@ -340,21 +331,21 @@ char *get_status_message(stat_t status); #define STAT_GCODE_ACTIVE_PLANE_IS_MISSING 140 // active plane is not programmed #define STAT_GCODE_ACTIVE_PLANE_IS_INVALID 141 // active plane selected is not valid for this command -#define STAT_GCODE_FEEDRATE_NOT_SPECIFIED 142 // move has no feedrate +#define STAT_GCODE_FEEDRATE_NOT_SPECIFIED 142 // move has no feedrate #define STAT_GCODE_INVERSE_TIME_MODE_CANNOT_BE_USED 143 // G38.2 and some canned cycles cannot accept inverse time mode #define STAT_GCODE_ROTARY_AXIS_CANNOT_BE_USED 144 // G38.2 and some other commands cannot have rotary axes #define STAT_GCODE_G53_WITHOUT_G0_OR_G1 145 // G0 or G1 must be active for G53 #define STAT_REQUESTED_VELOCITY_EXCEEDS_LIMITS 146 #define STAT_CUTTER_COMPENSATION_CANNOT_BE_ENABLED 147 #define STAT_PROGRAMMED_POINT_SAME_AS_CURRENT_POINT 148 -#define STAT_SPINDLE_SPEED_BELOW_MINIMUM 149 - -#define STAT_SPINDLE_SPEED_MAX_EXCEEDED 150 -#define STAT_S_WORD_IS_MISSING 151 -#define STAT_S_WORD_IS_INVALID 152 -#define STAT_SPINDLE_MUST_BE_OFF 153 -#define STAT_SPINDLE_MUST_BE_TURNING 154 // some canned cycles require spindle to be turning when called -#define STAT_ARC_SPECIFICATION_ERROR 155 // generic arc specification error +#define STAT_SPINDLE_SPEED_BELOW_MINIMUM 149 + +#define STAT_SPINDLE_SPEED_MAX_EXCEEDED 150 +#define STAT_S_WORD_IS_MISSING 151 +#define STAT_S_WORD_IS_INVALID 152 +#define STAT_SPINDLE_MUST_BE_OFF 153 +#define STAT_SPINDLE_MUST_BE_TURNING 154 // some canned cycles require spindle to be turning when called +#define STAT_ARC_SPECIFICATION_ERROR 155 // generic arc specification error #define STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE 156 // arc is missing axis (axes) required by selected plane #define STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE 157 // one or both offsets are not specified #define STAT_ARC_RADIUS_OUT_OF_TOLERANCE 158 // WARNING - radius arc is too small or too large - accuracy in question @@ -382,88 +373,88 @@ char *get_status_message(stat_t status); #define STAT_T_WORD_IS_MISSING 178 #define STAT_T_WORD_IS_INVALID 179 -#define STAT_ERROR_180 180 // reserved for Gcode errors -#define STAT_ERROR_181 181 -#define STAT_ERROR_182 182 -#define STAT_ERROR_183 183 -#define STAT_ERROR_184 184 -#define STAT_ERROR_185 185 -#define STAT_ERROR_186 186 -#define STAT_ERROR_187 187 -#define STAT_ERROR_188 188 -#define STAT_ERROR_189 189 - -#define STAT_ERROR_190 190 -#define STAT_ERROR_191 191 -#define STAT_ERROR_192 192 -#define STAT_ERROR_193 193 -#define STAT_ERROR_194 194 -#define STAT_ERROR_195 195 -#define STAT_ERROR_196 196 -#define STAT_ERROR_197 197 -#define STAT_ERROR_198 198 -#define STAT_ERROR_199 199 +#define STAT_ERROR_180 180 // reserved for Gcode errors +#define STAT_ERROR_181 181 +#define STAT_ERROR_182 182 +#define STAT_ERROR_183 183 +#define STAT_ERROR_184 184 +#define STAT_ERROR_185 185 +#define STAT_ERROR_186 186 +#define STAT_ERROR_187 187 +#define STAT_ERROR_188 188 +#define STAT_ERROR_189 189 + +#define STAT_ERROR_190 190 +#define STAT_ERROR_191 191 +#define STAT_ERROR_192 192 +#define STAT_ERROR_193 193 +#define STAT_ERROR_194 194 +#define STAT_ERROR_195 195 +#define STAT_ERROR_196 196 +#define STAT_ERROR_197 197 +#define STAT_ERROR_198 198 +#define STAT_ERROR_199 199 // TinyG errors and warnings #define STAT_GENERIC_ERROR 200 -#define STAT_MINIMUM_LENGTH_MOVE 201 // move is less than minimum length -#define STAT_MINIMUM_TIME_MOVE 202 // move is less than minimum time -#define STAT_MACHINE_ALARMED 203 // machine is alarmed. Command not processed -#define STAT_LIMIT_SWITCH_HIT 204 // a limit switch was hit causing shutdown -#define STAT_PLANNER_FAILED_TO_CONVERGE 205 // trapezoid generator can through this exception -#define STAT_ERROR_206 206 -#define STAT_ERROR_207 207 -#define STAT_ERROR_208 208 -#define STAT_ERROR_209 209 - -#define STAT_ERROR_210 210 -#define STAT_ERROR_211 211 -#define STAT_ERROR_212 212 -#define STAT_ERROR_213 213 -#define STAT_ERROR_214 214 -#define STAT_ERROR_215 215 -#define STAT_ERROR_216 216 -#define STAT_ERROR_217 217 -#define STAT_ERROR_218 218 -#define STAT_ERROR_219 219 - -#define STAT_SOFT_LIMIT_EXCEEDED 220 // soft limit error - axis unspecified -#define STAT_SOFT_LIMIT_EXCEEDED_XMIN 221 // soft limit error - X minimum -#define STAT_SOFT_LIMIT_EXCEEDED_XMAX 222 // soft limit error - X maximum -#define STAT_SOFT_LIMIT_EXCEEDED_YMIN 223 // soft limit error - Y minimum -#define STAT_SOFT_LIMIT_EXCEEDED_YMAX 224 // soft limit error - Y maximum -#define STAT_SOFT_LIMIT_EXCEEDED_ZMIN 225 // soft limit error - Z minimum -#define STAT_SOFT_LIMIT_EXCEEDED_ZMAX 226 // soft limit error - Z maximum -#define STAT_SOFT_LIMIT_EXCEEDED_AMIN 227 // soft limit error - A minimum -#define STAT_SOFT_LIMIT_EXCEEDED_AMAX 228 // soft limit error - A maximum -#define STAT_SOFT_LIMIT_EXCEEDED_BMIN 229 // soft limit error - B minimum - -#define STAT_SOFT_LIMIT_EXCEEDED_BMAX 220 // soft limit error - B maximum -#define STAT_SOFT_LIMIT_EXCEEDED_CMIN 231 // soft limit error - C minimum -#define STAT_SOFT_LIMIT_EXCEEDED_CMAX 232 // soft limit error - C maximum -#define STAT_ERROR_233 233 -#define STAT_ERROR_234 234 -#define STAT_ERROR_235 235 -#define STAT_ERROR_236 236 -#define STAT_ERROR_237 237 -#define STAT_ERROR_238 238 -#define STAT_ERROR_239 239 - -#define STAT_HOMING_CYCLE_FAILED 240 // homing cycle did not complete -#define STAT_HOMING_ERROR_BAD_OR_NO_AXIS 241 -#define STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY 242 -#define STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY 243 -#define STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL 244 -#define STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF 245 -#define STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION 246 -#define STAT_ERROR_247 247 -#define STAT_ERROR_248 248 -#define STAT_ERROR_249 249 - -#define STAT_PROBE_CYCLE_FAILED 250 // probing cycle did not complete +#define STAT_MINIMUM_LENGTH_MOVE 201 // move is less than minimum length +#define STAT_MINIMUM_TIME_MOVE 202 // move is less than minimum time +#define STAT_MACHINE_ALARMED 203 // machine is alarmed. Command not processed +#define STAT_LIMIT_SWITCH_HIT 204 // a limit switch was hit causing shutdown +#define STAT_PLANNER_FAILED_TO_CONVERGE 205 // trapezoid generator can through this exception +#define STAT_ERROR_206 206 +#define STAT_ERROR_207 207 +#define STAT_ERROR_208 208 +#define STAT_ERROR_209 209 + +#define STAT_ERROR_210 210 +#define STAT_ERROR_211 211 +#define STAT_ERROR_212 212 +#define STAT_ERROR_213 213 +#define STAT_ERROR_214 214 +#define STAT_ERROR_215 215 +#define STAT_ERROR_216 216 +#define STAT_ERROR_217 217 +#define STAT_ERROR_218 218 +#define STAT_ERROR_219 219 + +#define STAT_SOFT_LIMIT_EXCEEDED 220 // soft limit error - axis unspecified +#define STAT_SOFT_LIMIT_EXCEEDED_XMIN 221 // soft limit error - X minimum +#define STAT_SOFT_LIMIT_EXCEEDED_XMAX 222 // soft limit error - X maximum +#define STAT_SOFT_LIMIT_EXCEEDED_YMIN 223 // soft limit error - Y minimum +#define STAT_SOFT_LIMIT_EXCEEDED_YMAX 224 // soft limit error - Y maximum +#define STAT_SOFT_LIMIT_EXCEEDED_ZMIN 225 // soft limit error - Z minimum +#define STAT_SOFT_LIMIT_EXCEEDED_ZMAX 226 // soft limit error - Z maximum +#define STAT_SOFT_LIMIT_EXCEEDED_AMIN 227 // soft limit error - A minimum +#define STAT_SOFT_LIMIT_EXCEEDED_AMAX 228 // soft limit error - A maximum +#define STAT_SOFT_LIMIT_EXCEEDED_BMIN 229 // soft limit error - B minimum + +#define STAT_SOFT_LIMIT_EXCEEDED_BMAX 220 // soft limit error - B maximum +#define STAT_SOFT_LIMIT_EXCEEDED_CMIN 231 // soft limit error - C minimum +#define STAT_SOFT_LIMIT_EXCEEDED_CMAX 232 // soft limit error - C maximum +#define STAT_ERROR_233 233 +#define STAT_ERROR_234 234 +#define STAT_ERROR_235 235 +#define STAT_ERROR_236 236 +#define STAT_ERROR_237 237 +#define STAT_ERROR_238 238 +#define STAT_ERROR_239 239 + +#define STAT_HOMING_CYCLE_FAILED 240 // homing cycle did not complete +#define STAT_HOMING_ERROR_BAD_OR_NO_AXIS 241 +#define STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY 242 +#define STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY 243 +#define STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL 244 +#define STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF 245 +#define STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION 246 +#define STAT_ERROR_247 247 +#define STAT_ERROR_248 248 +#define STAT_ERROR_249 249 + +#define STAT_PROBE_CYCLE_FAILED 250 // probing cycle did not complete #define STAT_PROBE_ENDPOINT_IS_STARTING_POINT 251 -#define STAT_JOGGING_CYCLE_FAILED 252 // jogging cycle did not complete +#define STAT_JOGGING_CYCLE_FAILED 252 // jogging cycle did not complete // !!! Do not exceed 255 without also changing stat_t typedef diff --git a/src/usart.c b/src/usart.c new file mode 100644 index 0000000..cbb6024 --- /dev/null +++ b/src/usart.c @@ -0,0 +1,246 @@ +/******************************************************************************\ + + This file is part of the TinyG firmware. + + Copyright (c) 2015, Buildbotics LLC + All rights reserved. + + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + + The C! library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the C! library. If not, see + . + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +#include "usart.h" + +#include +#include +#include + +#include + +// Ring buffers +#define RING_BUF_NAME tx_buf +#define RING_BUF_SIZE USART_TX_RING_BUF_SIZE +#include "ringbuf.def" + +#define RING_BUF_NAME rx_buf +#define RING_BUF_SIZE USART_RX_RING_BUF_SIZE +#include "ringbuf.def" + +int usart_flags = USART_CRLF | USART_ECHO; + + +static void set_dre_interrupt(int enable) { + if (enable) USARTC0.CTRLA |= USART_DREINTLVL_HI_gc; + else USARTC0.CTRLA &= ~USART_DREINTLVL_HI_gc; +} + + +static void set_rxc_interrupt(int enable) { + if (enable) USARTC0.CTRLA |= USART_RXCINTLVL_HI_gc; + else USARTC0.CTRLA &= ~USART_RXCINTLVL_HI_gc; +} + + +// Data register empty interrupt vector +ISR(USARTC0_DRE_vect) { + if (tx_buf_empty()) set_dre_interrupt(0); // Disable interrupt + else { + USARTC0.DATA = tx_buf_peek(); + tx_buf_pop(); + } +} + + +// Data received interrupt vector +ISR(USARTC0_RXC_vect) { + if (rx_buf_full()) set_rxc_interrupt(0); // Disable interrupt + + else { + uint8_t data = USARTC0.DATA; + rx_buf_push(data); + + if ((usart_flags & USART_ECHO) && !tx_buf_full()) { + set_dre_interrupt(1); // Enable interrupt + usart_putc(data); + } + } +} + + +static int usart_putchar(char c, FILE *f) { + usart_putc(c); + return 0; +} + +static FILE _stdout = FDEV_SETUP_STREAM(usart_putchar, 0, _FDEV_SETUP_WRITE); + + +void usart_init(void) { + // Setup ring buffer + tx_buf_init(); + rx_buf_init(); + + PR.PRPC &= ~PR_USART0_bm; // Disable power reduction + + // Setup pins + PORTC.OUTSET = 1 << 3; // High + PORTC.DIRSET = 1 << 3; // Output + PORTC.DIRCLR = 1 << 2; // Input + + // Set baud rate + usart_set_baud(USART_BAUD_115200); + + // No parity, 8 data bits, 1 stop bit + USARTC0.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | + USART_CHSIZE_8BIT_gc; + + // Configure receiver and transmitter + USARTC0.CTRLA = USART_RXCINTLVL_HI_gc; + USARTC0.CTRLB = USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm; + + PMIC.CTRL |= PMIC_HILVLEN_bm; // Lowlevel interrupt on + + // Connect IO + stdout = &_stdout; + stderr = &_stdout; +} + + +static void set_baud(uint8_t bsel, uint8_t bscale) { + USARTC0.BAUDCTRLB = (uint8_t)((bscale << 4) | (bsel >> 8)); + USARTC0.BAUDCTRLA = bsel; +} + + +void usart_set_baud(int baud) { + // The BSEL / BSCALE values provided below assume a 32 Mhz clock + // Assumes CTRLB CLK2X bit (0x04) is not enabled + + switch (baud) { + case USART_BAUD_9600: set_baud(207, 0); break; + case USART_BAUD_19200: set_baud(103, 0); break; + case USART_BAUD_38400: set_baud(51, 0); break; + case USART_BAUD_57600: set_baud(34, 0); break; + case USART_BAUD_115200: set_baud(33, -1 << 4); break; + case USART_BAUD_230400: set_baud(31, -2 << 4); break; + case USART_BAUD_460800: set_baud(27, -3 << 4); break; + case USART_BAUD_921600: set_baud(19, -4 << 4); break; + case USART_BAUD_500000: set_baud(1, 1 << 4); break; + case USART_BAUD_1000000: set_baud(1, 0); break; + } +} + + +void usart_ctrl(int flag, int enable) { + if (enable) usart_flags |= flag; + else usart_flags &= ~flag; +} + + +static void usart_sleep() { + cli(); + SLEEP.CTRL = SLEEP_SMODE_IDLE_gc | SLEEP_SEN_bm; + sei(); + sleep_cpu(); +} + + +void usart_putc(char c) { + while (tx_buf_full()) usart_sleep(); + + tx_buf_push(c); + + set_dre_interrupt(1); // Enable interrupt + + if ((usart_flags & USART_CRLF) && c == '\n') usart_putc('\r'); +} + + +void usart_puts(const char *s) { + while (*s) usart_putc(*s++); +} + + +int8_t usart_getc() { + while (rx_buf_empty()) usart_sleep(); + + uint8_t data = rx_buf_peek(); + rx_buf_pop(); + + set_rxc_interrupt(1); // Enable interrupt + + return data; +} + + +int usart_gets(char *buf, int size) { + int fill = rx_buf_fill(); + + for (int i = 0; i < fill; i++) { + uint8_t data = rx_buf_get(i); + + if (data == '\r' || data == '\n' || i == size) { + for (int j = 0; j < i; j++) { + buf[j] = rx_buf_peek(); + rx_buf_pop(); + } + + buf[i] = 0; + if (i != size) rx_buf_pop(); + set_rxc_interrupt(1); // Enable interrupt + return 0; // OK + } + } + + return 2; // EAGAIN +} + + +int16_t usart_peek() { + return rx_buf_empty() ? -1 : rx_buf_peek(); +} + + +void usart_rx_flush() { + rx_buf_init(); +} + + +int16_t usart_rx_space() { + return rx_buf_space(); +} + + +int16_t usart_rx_fill() { + return rx_buf_fill(); +} + + +int16_t usart_tx_space() { + return tx_buf_space(); +} + + +int16_t usart_tx_fill() { + return tx_buf_fill(); +} diff --git a/src/usart.h b/src/usart.h new file mode 100644 index 0000000..f4d2b1f --- /dev/null +++ b/src/usart.h @@ -0,0 +1,79 @@ +/******************************************************************************\ + + This file is part of the TinyG firmware. + + Copyright (c) 2015, Buildbotics LLC + All rights reserved. + + The C! library is free software: you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation, either version 2.1 of + the License, or (at your option) any later version. + + The C! library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the C! library. If not, see + . + + In addition, BSD licensing may be granted on a case by case basis + by written permission from at least one of the copyright holders. + You may request written permission by emailing the authors. + + For information regarding this software email: + Joseph Coffland + joseph@buildbotics.com + +\******************************************************************************/ + +#ifndef USART_H +#define USART_H + +#include + +#define USART_TX_RING_BUF_SIZE 256 +#define USART_RX_RING_BUF_SIZE 256 + +enum { + USART_BAUD_9600, + USART_BAUD_19200, + USART_BAUD_38400, + USART_BAUD_57600, + USART_BAUD_115200, + USART_BAUD_230400, + USART_BAUD_460800, + USART_BAUD_921600, + USART_BAUD_500000, + USART_BAUD_1000000 +}; + +enum { + USART_CRLF = (1 << 0), + USART_ECHO = (1 << 1), + USART_XOFF = (1 << 2), +}; + +void usart_init(); +void usart_set_baud(int baud); +void usart_ctrl(int flag, int enable); +void usart_putc(char c); +void usart_puts(const char *s); +int8_t usart_getc(); +int usart_gets(char *buf, int size); +int16_t usart_peek(); + +void usart_rx_flush(); +int16_t usart_rx_fill(); +int16_t usart_rx_space(); +inline int usart_rx_empty() {return !usart_rx_fill();} +inline int usart_rx_full() {return !usart_rx_space();} + +int16_t usart_tx_fill(); +int16_t usart_tx_space(); +inline int usart_tx_empty() {return !usart_tx_fill();} +inline int usart_tx_full() {return !usart_tx_space();} + +#endif // USART_H diff --git a/src/xio/xio.c b/src/xio/xio.c deleted file mode 100755 index 351a8b5..0000000 --- a/src/xio/xio.c +++ /dev/null @@ -1,304 +0,0 @@ -/* - * xio.c - Xmega IO devices - common code file - * Part of TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* ----- XIO - Xmega Device System ---- - * - * XIO provides common access to native and derived xmega devices (see table below) - * XIO devices are compatible with avr-gcc stdio and also provide some special functions - * that are not found in stdio. - * - * Stdio support: - * - http://www.nongnu.org/avr-libc/user-manual/group__avr__stdio.html - * - Stdio compatible putc() and getc() functions provided for each device - * - This enables fgets, printf, scanf, and other stdio functions - * - Full support for formatted printing is provided (including floats) - * - Assignment of a default device to stdin, stdout & stderr is provided - * - printf() and printf_P() send to stdout, so use fprintf() to stderr - * for things that should't go over RS485 in SLAVE mode - * - * Facilities provided beyond stdio: - * - Supported devices include: - * - USB (derived from USART) - * - RS485 (derived from USART) - * - SPI devices and slave channels - * - Program memory "files" (read only) - * - Stdio FILE streams are managed as bindings to the above devices - * - Additional functions provided include: - * - open() - initialize parameters, addresses and flags - * - gets() - non-blocking input line reader - extends fgets - * - ctrl() - ioctl-like knockoff for setting device parameters (flags) - * - signal handling: interrupt on: feedhold, cycle_start, ctrl-x software reset - * - interrupt buffered RX and TX functions - * - XON/XOFF software flow control - */ -/* ----- XIO - Some Internals ---- - * - * XIO layers are: (1) xio virtual device (root), (2) xio device type, (3) xio devices - * - * The virtual device has the following methods: - * xio_init() - initialize the entire xio system - * xio_open() - open a device indicated by the XIO_DEV number - * xio_ctrl() - set control flags for XIO_DEV device - * xio_gets() - get a string from the XIO_DEV device (non blocking line reader) - * xio_getc() - read a character from the XIO_DEV device (not stdio compatible) - * xio_putc() - write a character to the XIO_DEV device (not stdio compatible) - * xio_set_baud() - set baud rates for devices for which this is meaningful - * - * The device type layer currently knows about USARTS, SPI, and File devices. Methods are: - * xio_init_() - initializes the devices of that type - * - * The device layer currently supports: USB, RS485, SPI channels, PGM file reading. methods: - * xio_open() - set up the device for use or reset the device - * xio_ctrl() - change device flag controls - * xio_gets() - get a string from the device (non-blocking) - * xio_getc() - read a character from the device (stdio compatible) - * xio_putc() - write a character to the device (stdio compatible) - * - * The virtual level uses XIO_DEV_xxx numeric device IDs for reference. - * Lower layers are called using the device structure pointer xioDev_t *d - * The stdio compatible functions use pointers to the stdio FILE structs. - */ -#include // for memset() -#include // precursor for xio.h -#include // precursor for xio.h - -#include "xio.h" // all device includes are nested here -#include "../tinyg.h" // needed by init() for default source -#include "../config.h" // needed by init() for default source -#include "../controller.h" // needed by init() for default source - - -typedef struct xioSingleton { - FILE * stderr_shadow; // used for stack overflow / memory integrity checking -} xioSingleton_t; -xioSingleton_t xio; - -/******************************************************************************** - * XIO Initializations, Resets and Assertions - */ -/* - * xio_init() - initialize entire xio sub-system - */ -void xio_init() -{ - // set memory integrity check - xio_set_stderr(0); // set a bogus value; may be overwritten with a real value - - // setup device types - xio_init_usart(); - xio_init_spi(); - xio_init_file(); - - // open individual devices (file device opens occur at time-of-use) - xio_open(XIO_DEV_USB, 0, USB_FLAGS); - xio_open(XIO_DEV_RS485,0, RS485_FLAGS); - xio_open(XIO_DEV_SPI1, 0, SPI_FLAGS); - xio_open(XIO_DEV_SPI2, 0, SPI_FLAGS); - - xio_init_assertions(); -} - -/* - * xio_init_assertions() - * xio_test_assertions() - validate operating state - * - * NOTE: xio device assertions are set up as part of xio_open_generic() - * This system is kind of brittle right now because if a device is - * not set up then it will fail in the assertions test. Need to fix this. - */ - -void xio_init_assertions() {} - -uint8_t xio_test_assertions() -{ - if (ds[XIO_DEV_USB].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (ds[XIO_DEV_USB].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (ds[XIO_DEV_RS485].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (ds[XIO_DEV_RS485].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (ds[XIO_DEV_SPI1].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (ds[XIO_DEV_SPI1].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (ds[XIO_DEV_SPI2].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (ds[XIO_DEV_SPI2].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; - if (stderr != xio.stderr_shadow) return STAT_XIO_ASSERTION_FAILURE; - return STAT_OK; -} - -/* - * xio_isbusy() - return TRUE if XIO sub-system is busy - * - * This function is here so that the caller can detect that the serial system is active - * and therefore generating interrupts. This is a hack for the earlier AVRs that require - * interrupts to be disabled for EEPROM write so the caller can see if the XIO system is - * quiescent. This is used by the G10 deferred writeback persistence functions. - * - * Idle conditions: - * - The serial RX buffer is empty, indicating with some probability that data is not being sent - * - The serial TX buffers are empty - */ - -uint8_t xio_isbusy() -{ - if (xio_get_rx_bufcount_usart(&USBu) != 0) return false; - if (xio_get_tx_bufcount_usart(&USBu) != 0) return false; - return true; -} - -/* - * xio_reset_working_flags() - */ - -void xio_reset_working_flags(xioDev_t *d) -{ - d->signal = 0; - d->flag_in_line = 0; - d->flag_eol = 0; - d->flag_eof = 0; -} - -/* - * xio_init_device() - generic initialization function for any device - * - * This binds the main fucntions and sets up the stdio FILE structure - * udata is used to point back to the device struct so it can be gotten - * from getc() and putc() functions. - * - * Requires device open() to be run prior to using the device - */ -void xio_open_generic(uint8_t dev, x_open_t x_open, - x_ctrl_t x_ctrl, - x_gets_t x_gets, - x_getc_t x_getc, - x_putc_t x_putc, - x_flow_t x_flow) -{ - xioDev_t *d = &ds[dev]; - memset (d, 0, sizeof(xioDev_t)); - d->magic_start = MAGICNUM; - d->magic_end = MAGICNUM; - d->dev = dev; - - // bind functions to device structure - d->x_open = x_open; - d->x_ctrl = x_ctrl; - d->x_gets = x_gets; - d->x_getc = x_getc; // you don't need to bind getc & putc unless you are going to use them directly - d->x_putc = x_putc; // they are bound into the fdev stream struct - d->x_flow = x_flow; - - // setup the stdio FILE struct and link udata back to the device struct - fdev_setup_stream(&d->file, x_putc, x_getc, _FDEV_SETUP_RW); - fdev_set_udata(&d->file, d); // reference yourself for udata -} - -/******************************************************************************** - * PUBLIC ENTRY POINTS - access the functions via the XIO_DEV number - * xio_open() - open function - * xio_gets() - entry point for non-blocking get line function - * xio_getc() - entry point for getc (not stdio compatible) - * xio_putc() - entry point for putc (not stdio compatible) - * - * It might be prudent to run an assertion such as below, but we trust the callers: - * if (dev < XIO_DEV_COUNT) blah blah blah - * else return _FDEV_ERR; // XIO_NO_SUCH_DEVICE - */ -FILE *xio_open(uint8_t dev, const char *addr, flags_t flags) -{ - return ds[dev].x_open(dev, addr, flags); -} - -int xio_gets(const uint8_t dev, char *buf, const int size) -{ - return ds[dev].x_gets(&ds[dev], buf, size); -} - -int xio_getc(const uint8_t dev) -{ - return ds[dev].x_getc(&ds[dev].file); -} - -int xio_putc(const uint8_t dev, const char c) -{ - return ds[dev].x_putc(c, &ds[dev].file); -} - -/* - * xio_ctrl() - PUBLIC set control flags (top-level XIO_DEV access) - * xio_ctrl_generic() - PRIVATE but generic set-control-flags - */ -int xio_ctrl(const uint8_t dev, const flags_t flags) -{ - return xio_ctrl_generic(&ds[dev], flags); -} - -#define SETFLAG(t,f) if ((flags & t) != 0) { d->f = true; } -#define CLRFLAG(t,f) if ((flags & t) != 0) { d->f = false; } - -int xio_ctrl_generic(xioDev_t *d, const flags_t flags) -{ - SETFLAG(XIO_BLOCK, flag_block); - CLRFLAG(XIO_NOBLOCK, flag_block); - SETFLAG(XIO_XOFF, flag_xoff); - CLRFLAG(XIO_NOXOFF, flag_xoff); - SETFLAG(XIO_ECHO, flag_echo); - CLRFLAG(XIO_NOECHO, flag_echo); - SETFLAG(XIO_CRLF, flag_crlf); - CLRFLAG(XIO_NOCRLF, flag_crlf); - SETFLAG(XIO_IGNORECR, flag_ignorecr); - CLRFLAG(XIO_NOIGNORECR, flag_ignorecr); - SETFLAG(XIO_IGNORELF, flag_ignorelf); - CLRFLAG(XIO_NOIGNORELF, flag_ignorelf); - SETFLAG(XIO_LINEMODE, flag_linemode); - CLRFLAG(XIO_NOLINEMODE, flag_linemode); - return XIO_OK; -} - -/* - * xio_set_baud() - PUBLIC entry to set baud rate - * Currently this only works on USART devices - */ -int xio_set_baud(const uint8_t dev, const uint8_t baud) -{ - xioUsart_t *dx = (xioUsart_t *)&us[dev - XIO_DEV_USART_OFFSET]; - xio_set_baud_usart(dx, baud); - return XIO_OK; -} - -/* - * xio_fc_null() - flow control null function - */ -void xio_fc_null(xioDev_t *d) -{ - return; -} - -/* - * xio_set_stdin() - set stdin from device number - * xio_set_stdout() - set stdout from device number - * xio_set_stderr() - set stderr from device number - * - * stderr is defined in stdio as __iob[2]. Turns out stderr is the last RAM - * allocated by the linker for this project. We usae that to keep a shadow - * of __iob[2] for stack overflow detection and other memory corruption. - */ -void xio_set_stdin(const uint8_t dev) { stdin = &ds[dev].file; } -void xio_set_stdout(const uint8_t dev) { stdout = &ds[dev].file; } -void xio_set_stderr(const uint8_t dev) -{ - stderr = &ds[dev].file; - xio.stderr_shadow = stderr; // this is the last thing in RAM, so we use it as a memory corruption canary -} diff --git a/src/xio/xio.h b/src/xio/xio.h deleted file mode 100755 index 48c60e2..0000000 --- a/src/xio/xio.h +++ /dev/null @@ -1,382 +0,0 @@ -/* - * xio.h - Xmega IO devices - common header file - * Part of TinyG project - * - * Copyright (c) 2010 - 2014 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -/* XIO devices are compatible with avr-gcc stdio, so formatted printing - * is supported. To use this sub-system outside of TinyG you may need - * some defines in tinyg.h. See notes at end of this file for more details. - */ -/* Note: anything that includes xio.h first needs the following: - * #include // needed for FILE def'n - * #include // needed for true and false - * #include // defines prog_char, PSTR - */ -/* Note: This file contains load of sub-includes near the middle - * #include "xio_file.h" - * #include "xio_usart.h" - * #include "xio_spi.h" - * #include "xio_signals.h" - * (possibly more) - */ -/* - * CAVEAT EMPTOR: File under "watch your ass": - * - * - Short story: Do not call ANYTHING that can print (i.e. send chars to the TX - * buffer) from a medium or hi interrupt. This obviously includes any printf() - * function, but also exception reports, cm_soft_alarm(), cm_hard_alarm() and a - * few other functions that call stdio print functions. - * - * - Longer Story: The stdio printf() functions use character drivers provided by - * tinyg to access the low-level Xmega devices. Specifically xio_putc_usb() in xio_usb.c, - * and xio_putc_rs485() in xio_rs485.c. Since stdio does not understand non-blocking - * IO these functions must block if there is no space in the TX buffer. Blocking is - * accomplished using sleep_mode(). The IO system is the only place where sleep_mode() - * is used. Everything else in TinyG is non-blocking. Sleep is woken (exited) whenever - * any interrupt fires. So there must always be a viable interrupt source running when - * you enter a sleep or the system will hang (lock up). In the IO functions this is the - * TX interupts, which fire when space becomes available in the USART for a TX char. This - * Means you cannot call a print function at or above the level of the TX interrupts, - * which are set to medium. - */ -#ifndef XIO_H_ONCE -#define XIO_H_ONCE - -// Pre-allocated XIO devices (configured devices) -// Unused devices are commented out. All this needs to line up. - -enum xioDevNum_t { // TYPE: DEVICE: - XIO_DEV_USB, // USART USB device - XIO_DEV_RS485, // USART RS485 device - XIO_DEV_SPI1, // SPI SPI channel #1 - XIO_DEV_SPI2, // SPI SPI channel #2 - XIO_DEV_PGM, // FILE Program memory file (read only) - XIO_DEV_COUNT // total device count (must be last entry) -}; -// If your change these ^, check these v - -#define XIO_DEV_USART_COUNT 2 // # of USART devices -#define XIO_DEV_USART_OFFSET 0 // offset for computing indices - -#define XIO_DEV_SPI_COUNT 2 // # of SPI devices -#define XIO_DEV_SPI_OFFSET XIO_DEV_USART_COUNT // offset for computing indicies - -#define XIO_DEV_FILE_COUNT 1 // # of FILE devices -#define XIO_DEV_FILE_OFFSET (XIO_DEV_USART_COUNT + XIO_DEV_SPI_COUNT) // index into FILES - -// Fast accessors -#define USB ds[XIO_DEV_USB] -#define USBu us[XIO_DEV_USB - XIO_DEV_USART_OFFSET] - -/****************************************************************************** - * Device structures - * - * Each device has 3 structs. The generic device struct is declared below. - * It embeds a stdio stream struct "FILE". The FILE struct uses the udata - * field to back-reference the generic struct so getc & putc can get at it. - * Lastly there's an 'x' struct which contains data specific to each dev type. - * - * The generic open() function sets up the generic struct and the FILE stream. - * the device opens() set up the extended struct and bind it ot the generic. - ******************************************************************************/ -// NOTE" "FILE *" is another way of saying "struct __file *" -// NOTE: using the "x_" prefix fo avoid collisions with stdio defined getc, putc, etc - -#define flags_t uint16_t - -typedef struct xioDEVICE { // common device struct (one per dev) - // references and self references - uint16_t magic_start; // memory integrity check - uint8_t dev; // self referential device number - FILE file; // stdio FILE stream structure - void *x; // extended device struct binding (static) - - // function bindings - FILE *(*x_open)(const uint8_t dev, const char *addr, const flags_t flags); - int (*x_ctrl)(struct xioDEVICE *d, const flags_t flags); // set device control flags - int (*x_gets)(struct xioDEVICE *d, char *buf, const int size);// non-blocking line reader - int (*x_getc)(FILE *); // read char (stdio compatible) - int (*x_putc)(char, FILE *); // write char (stdio compatible) - void (*x_flow)(struct xioDEVICE *d); // flow control callback function - - // device configuration flags - uint8_t flag_block; - uint8_t flag_echo; - uint8_t flag_crlf; - uint8_t flag_ignorecr; - uint8_t flag_ignorelf; - uint8_t flag_linemode; - uint8_t flag_xoff; // xon/xoff enabled - - // private working data and runtime flags - int size; // text buffer length (dynamic) - uint8_t len; // chars read so far (buf array index) - uint8_t signal; // signal value - uint8_t flag_in_line; // used as a state variable for line reads - uint8_t flag_eol; // end of line detected - uint8_t flag_eof; // end of file detected - char *buf; // text buffer binding (can be dynamic) - uint16_t magic_end; -} xioDev_t; - -typedef FILE *(*x_open_t)(const uint8_t dev, const char *addr, const flags_t flags); -typedef int (*x_ctrl_t)(xioDev_t *d, const flags_t flags); -typedef int (*x_gets_t)(xioDev_t *d, char *buf, const int size); -typedef int (*x_getc_t)(FILE *); -typedef int (*x_putc_t)(char, FILE *); -typedef void (*x_flow_t)(xioDev_t *d); - -// Put all sub-includes here so only xio.h is needed elsewhere -#include "xio_file.h" -#include "xio_usart.h" -#include "xio_spi.h" - -// Static structure allocations -xioDev_t ds[XIO_DEV_COUNT]; // allocate top-level dev structs -xioUsart_t us[XIO_DEV_USART_COUNT]; // USART extended IO structs -xioSpi_t spi[XIO_DEV_SPI_COUNT]; // SPI extended IO structs -xioFile_t fs[XIO_DEV_FILE_COUNT]; // FILE extended IO structs -extern struct controllerSingleton tg; // needed by init() for default source - -// Advance RX or TX head or tail. Buffers count down, so advance is a decrement. -// The zero condition is the wrap that sets the index back to the top. -#define advance_buffer(buf,len) { if ((--(buf)) == 0) buf = len-1;} - -// public functions (virtual class) -void xio_init(); -void xio_init_assertions(); -uint8_t xio_test_assertions(); -uint8_t xio_isbusy(); - -void xio_reset_working_flags(xioDev_t *d); -FILE *xio_open(const uint8_t dev, const char *addr, const flags_t flags); -int xio_ctrl(const uint8_t dev, const flags_t flags); -int xio_gets(const uint8_t dev, char *buf, const int size); -int xio_getc(const uint8_t dev); -int xio_putc(const uint8_t dev, const char c); -int xio_set_baud(const uint8_t dev, const uint8_t baud_rate); - -// generic functions (private, but at virtual level) -int xio_ctrl_generic(xioDev_t *d, const flags_t flags); - -void xio_open_generic(uint8_t dev, x_open_t x_open, - x_ctrl_t x_ctrl, - x_gets_t x_gets, - x_getc_t x_getc, - x_putc_t x_putc, - x_flow_t x_flow); - -void xio_fc_null(xioDev_t *d); // 0 flow control callback -void xio_fc_usart(xioDev_t *d); // XON/XOFF flow control callback - -// std devices -void xio_init_stdio(); // set std devs & do startup prompt -void xio_set_stdin(const uint8_t dev); -void xio_set_stdout(const uint8_t dev); -void xio_set_stderr(const uint8_t dev); - -/************************************************************************* - * SUPPORTING DEFINTIONS - SHOULD NOT NEED TO CHANGE - *************************************************************************/ -/* - * xio control flag values - * - * if using 32 bits must cast 1 to uint32_t for bit evaluations to work correctly - * #define XIO_BLOCK ((uint32_t)1<<1) // 32 bit example. Change flags_t to uint32_t - */ - -#define XIO_BLOCK ((uint16_t)1<<0) // enable blocking reads -#define XIO_NOBLOCK ((uint16_t)1<<1) // disable blocking reads -#define XIO_XOFF ((uint16_t)1<<2) // enable XON/OFF flow control -#define XIO_NOXOFF ((uint16_t)1<<3) // disable XON/XOFF flow control -#define XIO_ECHO ((uint16_t)1<<4) // echo reads from device to stdio -#define XIO_NOECHO ((uint16_t)1<<5) // disable echo -#define XIO_CRLF ((uint16_t)1<<6) // convert to on writes -#define XIO_NOCRLF ((uint16_t)1<<7) // do not convert to on writes -#define XIO_IGNORECR ((uint16_t)1<<8) // ignore on reads -#define XIO_NOIGNORECR ((uint16_t)1<<9) // don't ignore on reads -#define XIO_IGNORELF ((uint16_t)1<<10) // ignore on reads -#define XIO_NOIGNORELF ((uint16_t)1<<11) // don't ignore on reads -#define XIO_LINEMODE ((uint16_t)1<<12) // special read handling -#define XIO_NOLINEMODE ((uint16_t)1<<13) // no special read handling - -/* - * Generic XIO signals and error conditions. - * See signals.h for application specific signal defs and routines. - */ - -enum xioSignals { - XIO_SIG_OK, // OK - XIO_SIG_EAGAIN, // would block - XIO_SIG_EOL, // end-of-line encountered (string has data) - XIO_SIG_EOF, // end-of-file encountered (string has no data) - XIO_SIG_OVERRUN, // buffer overrun - XIO_SIG_RESET, // cancel operation immediately - XIO_SIG_FEEDHOLD, // pause operation - XIO_SIG_CYCLE_START, // start or resume operation - XIO_SIG_QUEUE_FLUSH, // flush planner queue - XIO_SIG_DELETE, // backspace or delete character (BS, DEL) - XIO_SIG_BELL, // BELL character (BEL, ^g) - XIO_SIG_BOOTLOADER // ESC character - start bootloader -}; - -// Some useful ASCII definitions -#define STX (char)0x02 // ^b - STX -#define ETX (char)0x03 // ^c - ETX -#define ENQ (char)0x05 // ^e - ENQuire -#define BEL (char)0x07 // ^g - BEL -#define BS (char)0x08 // ^h - backspace -#define TAB (char)0x09 // ^i - character -#define LF (char)0x0A // ^j - line feed -#define VT (char)0x0B // ^k - kill stop -#define CR (char)0x0D // ^m - carriage return -#define XON (char)0x11 // ^q - DC1, XON, resume -#define XOFF (char)0x13 // ^s - DC3, XOFF, pause -#define SYN (char)0x16 // ^v - SYN - Used for queue flush -#define CAN (char)0x18 // ^x - Cancel, abort -#define ESC (char)0x1B // ^[ - ESC(ape) -#define DEL (char)0x7F // DEL(ete) - -#define Q_EMPTY (char)0xFF // signal no character - -// Signal character mappings -#define CHAR_RESET CAN -#define CHAR_FEEDHOLD (char)'!' -#define CHAR_CYCLE_START (char)'~' -#define CHAR_QUEUE_FLUSH (char)'%' - -/* XIO return codes - * These codes are the "inner nest" for the STAT_ return codes. - * The first N TG codes correspond directly to these codes. - * This eases using XIO by itself (without tinyg) and simplifes using - * tinyg codes with no mapping when used together. This comes at the cost - * of making sure these lists are aligned. STAT_should be based on this list. - */ - -enum xioCodes { - XIO_OK = 0, // OK - ALWAYS ZERO - XIO_ERR, // generic error return errors start here - XIO_EAGAIN, // function would block here (must be called again) - XIO_NOOP, // function had no-operation - XIO_COMPLETE, // operation complete - XIO_TERMINATE, // operation terminated (gracefully) - XIO_RESET, // operation reset (ungraceful) - XIO_EOL, // function returned end-of-line - XIO_EOF, // function returned end-of-file - XIO_FILE_NOT_OPEN, // file is not open - XIO_FILE_SIZE_EXCEEDED, // maximum file size exceeded - XIO_NO_SUCH_DEVICE, // illegal or unavailable device - XIO_BUFFER_EMPTY, // more of a statement of fact than an error code - XIO_BUFFER_FULL, - XIO_BUFFER_FULL_FATAL, - XIO_INITIALIZING, // system initializing, not ready for use - XIO_ERROR_16, // reserved - XIO_ERROR_17, // reserved - XIO_ERROR_18, // reserved - XIO_ERROR_19 // NOTE: XIO codes align to here -}; -#define XIO_ERRNO_MAX XIO_BUFFER_FULL_NON_FATAL - - - -/* ASCII characters used by Gcode or otherwise unavailable for special use. - See NIST sections 3.3.2.2, 3.3.2.3 and Appendix E for Gcode uses. - See http://www.json.org/ for JSON notation - - hex char name used by: - ---- ---- ---------- -------------------- - 0x00 0 null everything - 0x01 SOH ctrl-A - 0x02 STX ctrl-B Kinen SPI protocol - 0x03 ETX ctrl-C Kinen SPI protocol - 0x04 EOT ctrl-D - 0x05 ENQ ctrl-E - 0x06 ACK ctrl-F - 0x07 BEL ctrl-G - 0x08 BS ctrl-H - 0x09 HT ctrl-I - 0x0A LF ctrl-J - 0x0B VT ctrl-K - 0x0C FF ctrl-L - 0x0D CR ctrl-M - 0x0E SO ctrl-N - 0x0F SI ctrl-O - 0x10 DLE ctrl-P - 0x11 DC1 ctrl-Q XOFF - 0x12 DC2 ctrl-R - 0x13 DC3 ctrl-S XON - 0x14 DC4 ctrl-T - 0x15 NAK ctrl-U - 0x16 SYN ctrl-V - 0x17 ETB ctrl-W - 0x18 CAN ctrl-X TinyG / grbl software reset - 0x19 EM ctrl-Y - 0x1A SUB ctrl-Z - 0x1B ESC ctrl-[ - 0x1C FS ctrl-\ - 0x1D GS ctrl-] - 0x1E RS ctrl-^ - 0x1F US ctrl-_ - - 0x20 Gcode blocks - 0x21 ! excl point TinyG feedhold (trapped and removed from serial stream) - 0x22 " quote JSON notation - 0x23 # number Gcode parameter prefix; JSON topic prefix character - 0x24 $ dollar TinyG / grbl out-of-cycle settings prefix - 0x25 & ampersand universal symbol for logical AND (not used here) - 0x26 % percent Queue Flush character (trapped and removed from serial stream) - Also sometimes used as a file-start and file-end character in Gcode files - 0x27 ' single quote - 0x28 ( open paren Gcode comments - 0x29 ) close paren Gcode comments - 0x2A * asterisk Gcode expressions; JSON wildcard character - 0x2B + plus Gcode numbers, parameters and expressions - 0x2C , comma JSON notation - 0x2D - minus Gcode numbers, parameters and expressions - 0x2E . period Gcode numbers, parameters and expressions - 0x2F / fwd slash Gcode expressions & block delete char - 0x3A : colon JSON notation - 0x3B ; semicolon Gcode comemnt delimiter (alternate) - 0x3C < less than Gcode expressions - 0x3D = equals Gcode expressions - 0x3E > greaterthan Gcode expressions - 0x3F ? question mk TinyG / grbl query - 0x40 @ at symbol JSON address prefix character - - 0x5B [ open bracketGcode expressions - 0x5C \ backslash JSON notation (escape) - 0x5D ] close brack Gcode expressions - 0x5E ^ caret Reserved for TinyG in-cycle command prefix - 0x5F _ underscore - - 0x60 ` grave accnt - 0x7B { open curly JSON notation - 0x7C | pipe universal symbol for logical OR (not used here) - 0x7D } close curly JSON notation - 0x7E ~ tilde TinyG cycle start (trapped and removed from serial stream) - 0x7F DEL -*/ - -#endif // end of include guard: XIO_H_ONCE diff --git a/src/xio/xio_file.c b/src/xio/xio_file.c deleted file mode 100755 index 6e9d17e..0000000 --- a/src/xio/xio_file.c +++ /dev/null @@ -1,92 +0,0 @@ -/* - * xio_file.c - device driver for program memory "files" - * - works with avr-gcc stdio library - * - * Part of TinyG project - * - * Copyright (c) 2011 - 2015 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include // precursor for xio.h -#include // true and false -#include // for memset -#include // precursor for xio.h -#include "xio.h" // includes for all devices are in here - -/****************************************************************************** - * FILE CONFIGURATION RECORDS - ******************************************************************************/ - -struct cfgFILE { - x_open_t x_open; // see xio.h for typedefs - x_ctrl_t x_ctrl; - x_gets_t x_gets; - x_getc_t x_getc; - x_putc_t x_putc; - x_flow_t x_flow; -}; - -static struct cfgFILE const cfgFile[] PROGMEM = { -{ // PGM config - xio_open_file, // open function - xio_ctrl_generic, // ctrl function - xio_gets_pgm, // get string function - xio_getc_pgm, // stdio getc function - xio_putc_pgm, // stdio putc function - xio_fc_null, // flow control callback -} -}; -/****************************************************************************** - * FUNCTIONS - ******************************************************************************/ - -/* - * xio_init_file() - initialize and set controls for file IO - * - * Need to bind the open function or a subsequent opens will fail - */ - -void xio_init_file() -{ - for (uint8_t i=0; ix = &fs[dev - XIO_DEV_FILE_OFFSET]; // bind extended struct to device - xioFile_t *dx = (xioFile_t *)d->x; - - memset (dx, 0, sizeof(xioFile_t)); // clear all values - xio_reset_working_flags(d); - xio_ctrl_generic(d, flags); // setup control flags - dx->filebase_P = (PROGMEM const char *)addr; // might want to range check this - dx->max_offset = PGM_ADDR_MAX; - return(&d->file); // return pointer to the FILE stream -} diff --git a/src/xio/xio_file.h b/src/xio/xio_file.h deleted file mode 100755 index 0846738..0000000 --- a/src/xio/xio_file.h +++ /dev/null @@ -1,93 +0,0 @@ -/* - * xio_file.h - device driver for file-type devices - * - works with avr-gcc stdio library - * - * Part of TinyG project - * - * Copyright (c) 2011 - 2013 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/*--- How to setup and use program memory "files" ---- - - Setup a memory file (OK, it's really just a string) - should be declared as so: - - const char g0_test1[] PROGMEM= "\ - g0 x10 y20 z30\n\ - g0 x0 y21 z-34.2"; - - Line 1 is the initial declaration of the array (string) in program memory - Line 2 is a continuation line. - - Must end with a newline and a continuation backslash - - (alternately can use a semicolon instead of \n is XIO_SEMICOLONS is set) - - Each line will be read as a single line of text using fgets() - Line 3 is the terminating line. Note the closing quote and semicolon. - - - Initialize: xio_pgm_init() must be called first. See the routine for options. - - Open the file: xio_pgm_open() is called, like so: - xio_pgm_open(PGMFILE(&g0_test1)); // simple linear motion test - - PGMFILE does the right cast. If someone more familiar with all this - can explain why PSTR doesn't work I'd be grateful. - - Read a line of text. Example from parsers.c - if (fgets(textbuf, BUF_LEN, srcin) == 0) { - printf_P(PSTR("\r\nEnd of file encountered\r\n")); - clearerr(srcin); - srcin = stdin; - tg_prompt(); - return; - } -*/ - -#ifndef xio_file_h -#define xio_file_h - -#define PGMFILE (const PROGMEM char *) // extends pgmspace.h - -/* - * FILE DEVICE CONFIGS - */ - -#define PGM_FLAGS (XIO_BLOCK | XIO_CRLF | XIO_LINEMODE) -#define PGM_ADDR_MAX (0x4000) // 16K - -/* - * FILE device extended control structure - * Note: As defined this struct won't do files larger than 65,535 chars - * Note: As defined this struct won't do files larger than 4 Gbytes chars - */ - -// file-type device control struct -typedef struct xioFILE { - uint32_t rd_offset; // read index into file - uint32_t wr_offset; // write index into file - uint32_t max_offset; // max size of file - const char * filebase_P; // base location in program memory (PROGMEM) -} xioFile_t; - -/* - * FILE DEVICE FUNCTION PROTOTYPES - */ -void xio_init_file(); -FILE *xio_open_file(const uint8_t dev, const char *addr, const flags_t flags); -int xio_gets_pgm(xioDev_t *d, char *buf, const int size); // read string from program memory -int xio_getc_pgm(FILE *stream); // get a character from PROGMEM -int xio_putc_pgm(const char c, FILE *stream); // always returns ERROR - -// SD Card functions - -#endif diff --git a/src/xio/xio_pgm.c b/src/xio/xio_pgm.c deleted file mode 100755 index aa5d15d..0000000 --- a/src/xio/xio_pgm.c +++ /dev/null @@ -1,118 +0,0 @@ -/* - * xio_pgm.c - device driver for program memory "files" - * - works with avr-gcc stdio library - * - * Part of TinyG project - * - * Copyright (c) 2011 - 2015 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include // precursor for xio.h -#include // true and false -#include // precursor for xio.h -#include "xio.h" // includes for all devices are in here - -// Fast accessors (cheating) -#define PGM ds[XIO_DEV_PGM] // device struct accessor -#define PGMf fs[XIO_DEV_PGM - XIO_DEV_FILE_OFFSET] // file extended struct accessor - -/* - * xio_gets_pgm() - main loop task for program memory device - * - * Non-blocking, run-to-completion return a line from memory - * Note: LINEMODE flag is ignored. It's ALWAYS LINEMODE here. - */ - -int xio_gets_pgm(xioDev_t *d, char *buf, const int size) -{ - if ((PGMf.filebase_P) == 0) { // return error if no file is open - return XIO_FILE_NOT_OPEN; - } - PGM.signal = XIO_SIG_OK; // initialize signal - if (fgets(buf, size, &PGM.file) == 0) { - PGMf.filebase_P = 0; - clearerr(&PGM.file); - return XIO_EOF; - } - return XIO_OK; -} - -/* - * xio_getc_pgm() - read a character from program memory device - * - * Get next character from program memory file. - * - * END OF FILE (EOF) - * - the first time you encounter 0, return ETX - * - all subsequent times rreturn 0 - * This allows the higherlevel stdio routines to return a line that - * terminates with a 0, but reads from the end of file will return - * errors. - * - * - return ETX (as returning 0 is indistinguishable from an error) - * - return 0 (this is NOT EOF, wich is -1 and signifies and error) - * - * LINEMODE and SEMICOLONS behaviors - * - consider and to be EOL chars (not just ) - * - also consider semicolons (';') to be EOL chars if SEMICOLONS enabled - * - convert any EOL char to to signal end-of-string (e.g. to fgets()) - * - * ECHO behaviors - * - if ECHO is enabled echo character to stdout - * - echo all line termination chars as newlines ('\n') - * - Note: putc should expand newlines to - */ - -int xio_getc_pgm(FILE *stream) -{ - char c; - - if (PGM.flag_eof ) { - PGM.signal = XIO_SIG_EOF; - return _FDEV_EOF; - } - if ((c = pgm_read_byte(&PGMf.filebase_P[PGMf.rd_offset])) == 0) { - PGM.flag_eof = true; - } - ++PGMf.rd_offset; - - // processing is simple if not in LINEMODE - if (PGM.flag_linemode == false) { - if (PGM.flag_echo) putchar(c); // conditional echo - return c; - } - - // now do the LINEMODE stuff - if (c == 0) { // perform newline substitutions - c = '\n'; - } else if (c == '\r') { - c = '\n'; - } - if (PGM.flag_echo) putchar(c); // conditional echo - return c; -} - -/* - * xio_putc_pgm() - write character to to program memory device - * - * Always returns error. You cannot write to program memory - */ - -int xio_putc_pgm(const char c, FILE *stream) -{ - return -1; // always returns an error. Big surprise. -} - - diff --git a/src/xio/xio_rs485.c b/src/xio/xio_rs485.c deleted file mode 100755 index f2637a5..0000000 --- a/src/xio/xio_rs485.c +++ /dev/null @@ -1,190 +0,0 @@ -/* - * xio_rs485.c - RS-485 device driver for xmega family - * - works with avr-gcc stdio library - * - * Part of TinyG project - * - * Copyright (c) 2010 - 2015 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* - * The RS485 driver has some specializations in the putc() that requires - * its own routines (in this file). As with other devices, its interrupts - * are also in this file. - * - * The RS485 driver is a half duplex driver that works over a single A/B - * differential pair. So the USART can only be in RX or TX mode at any - * given time, never both. Most of the specialized logic in the RS485 - * driver deals with this constraint. - */ - -#include // precursor for xio.h -#include // true and false -#include // precursor for xio.h -#include -#include // needed for blocking character writes - -#include "xio.h" -#include "../xmega/xmega_interrupts.h" - -#include "../tinyg.h" // needed for canonical machine -#include "../hardware.h" // needed for hardware reset -#include "../controller.h" // needed for trapping kill char -#include "../canonical_machine.h" // needed for fgeedhold and cycle start - -// Fast accessors -#define RS ds[XIO_DEV_RS485] -#define RSu us[XIO_DEV_RS485 - XIO_DEV_USART_OFFSET] - -/* - * Helper functions - * xio_enable_rs485_tx() - specialized routine to enable rs488 TX mode - * xio_enable_rs485_rx() - specialized routine to enable rs488 RX mode - * - * enables one mode and disables the other - */ -void xio_enable_rs485_tx() -{ - // enable TX, related interrupts & set DE and RE lines (disabling RX) - RSu.usart->CTRLB = USART_TXEN_bm; - RSu.usart->CTRLA = CTRLA_RXOFF_TXON_TXCON; - RSu.port->OUTSET = (RS485_DE_bm | RS485_RE_bm); -} - -void xio_enable_rs485_rx() -{ - // enable RX, related interrupts & clr DE and RE lines (disabling TX) - RSu.usart->CTRLB = USART_RXEN_bm; - RSu.usart->CTRLA = CTRLA_RXON_TXOFF_TXCON; - RSu.port->OUTCLR = (RS485_DE_bm | RS485_RE_bm); -} - -/* - * xio_putc_rs485() - stdio compatible char writer for rs485 devices - * - * The TX putc() / interrupt dilemma: TX interrupts occur when the - * USART DATA register is empty (ready for TX data) - and will keep - * firing as long as the TX buffer is completely empty (ready for TX - * data). So putc() and its ISR henchmen must disable interrupts when - * there's nothing left to write or they will just keep firing. - * - * To make matters worse, for some reason if you enable the TX interrupts - * and TX DATA is ready, it won't actually generate an interrupt. Putc() - * must "prime" the first write itself. This requires a mutual exclusion - * region around the dequeue operation to make sure the ISR and main - * routines don't collide. - * - * Lastly, the system must detect the end of transmission (TX complete) - * to know when to revert the RS485 driver to RX mode. So there are 2 TX - * interrupt conditions and handlers, not 1 like other USART TXs. - * - * NOTE: Finding a buffer empty condition on the first byte of a string - * is common as the TX byte is often written by the task itself. - */ -int xio_putc_rs485(const char c, FILE *stream) -{ - buffer_t next_tx_buf_head; - - if ((next_tx_buf_head = (RSu.tx_buf_head)-1) == 0) { // adv. head & wrap - next_tx_buf_head = TX_BUFFER_SIZE-1; // -1 avoids the off-by-one - } - while(next_tx_buf_head == RSu.tx_buf_tail) { // buf full. sleep or ret - if (RS.flag_block) { - sleep_mode(); - } else { - RS.signal = XIO_SIG_EAGAIN; - return(_FDEV_ERR); - } - }; - // enable TX mode and write data to TX buffer - xio_enable_rs485_tx(); // enable for TX - RSu.tx_buf_head = next_tx_buf_head; // accept next buffer head - RSu.tx_buf[RSu.tx_buf_head] = c; // ...write char to buffer - - if ((c == '\n') && (RS.flag_crlf)) { // detect LF & add CR - return RS.x_putc('\r', stream); // recurse - } - // force a TX interupt to attempt to send the character - RSu.usart->CTRLA = CTRLA_RXON_TXON; // doesn't work if you just |= it - return XIO_OK; -} - -/* - * RS485_TX_ISR - RS485 transmitter interrupt (TX) - * RS485_TXC_ISR - RS485 transmission complete (See notes in xio_putc_rs485) - */ - -ISR(RS485_TX_ISR_vect) //ISR(USARTC1_DRE_vect) // USARTC1 data register empty -{ - // NOTE: Assumes the USART is in TX mode before this interrupt is fired - if (RSu.tx_buf_head == RSu.tx_buf_tail) { // buffer empty - disable ints (NOTE) - RSu.usart->CTRLA = CTRLA_RXON_TXOFF_TXCON; // doesn't work if you just &= it - return; - } - if (--(RSu.tx_buf_tail) == 0) { // advance tail and wrap if needed - RSu.tx_buf_tail = TX_BUFFER_SIZE-1; // -1 avoids off-by-one error (OBOE) - } - RSu.usart->DATA = RSu.tx_buf[RSu.tx_buf_tail]; // write char to TX DATA reg -} - -ISR(RS485_TXC_ISR_vect) // ISR(USARTC1_TXC_vect) -{ - xio_enable_rs485_rx(); // revert to RX mode -} - -/* - * RS485_RX_ISR - RS485 receiver interrupt (RX) - */ - -ISR(RS485_RX_ISR_vect) //ISR(USARTC1_RXC_vect) // serial port C0 RX isr -{ - char c; - - if ((RSu.usart->STATUS & USART_RX_DATA_READY_bm) != 0) { - c = RSu.usart->DATA; // can only read DATA once - } else { - return; // shouldn't ever happen; bit of a fail-safe here - } - - // trap async commands - do not insert into RX queue - if (c == CHAR_RESET) { // trap Kill character - hw_request_hard_reset(); - return; - } - if (c == CHAR_FEEDHOLD) { // trap feedhold signal - cm_request_feedhold(); - return; - } - if (c == CHAR_CYCLE_START) { // trap end_feedhold signal - cm_request_cycle_start(); - return; - } - // filter out CRs and LFs if they are to be ignored - if ((c == CR) && (RS.flag_ignorecr)) return; - if ((c == LF) && (RS.flag_ignorelf)) return; - - // normal character path - advance_buffer(RSu.rx_buf_head, RX_BUFFER_SIZE); - if (RSu.rx_buf_head != RSu.rx_buf_tail) { // write char unless buffer full - RSu.rx_buf[RSu.rx_buf_head] = c; // (= USARTC1.DATA;) - RSu.rx_buf_count++; - // flow control detection goes here - should it be necessary - return; - } - // buffer-full handling - if ((++RSu.rx_buf_head) > RX_BUFFER_SIZE -1) { // reset the head - RSu.rx_buf_count = RX_BUFFER_SIZE-1; // reset count for good measure - RSu.rx_buf_head = 1; - } -} diff --git a/src/xio/xio_spi.c b/src/xio/xio_spi.c deleted file mode 100755 index 8a22b6c..0000000 --- a/src/xio/xio_spi.c +++ /dev/null @@ -1,451 +0,0 @@ -/* - * xio_spi.c - General purpose SPI device driver for xmega family - * - works with avr-gcc stdio library - * - * Part of TinyG project - * - * Copyright (c) 2015 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* ---- SPI Protocol ---- - * - * The SPI master/slave protocol is designed to be as simple as possible. - * In short, the master transmits whenever it wants to and the slave returns - * the next character in its output buffer whenever there's an SPI transfer. - * No flow control is needed as the master initiates and drives all transfers. - * - * More details: - * - * - A "message" is a line of text. Examples of messages are requests from the - * master to a slave, responses to these requests, and asynchronous messages - * (from a slave) that are not tied to a request. - * - * Messages are terminated with a newline (aka NL, LF, line-feed). The - * terminating NL is considered part of the message and should be transmitted. - * - * If multiple NLs are transmitted each trailing NL is interpreted as a blank - * message. This is generally not good practice - so watch it. - * - * Carriage return CR is not recognized as a newline. A CR in a message is - * treated as any other non-special ASCII character. - * - * 0s (0x00) are not transmitted in either direction (e.g. string terminations). - * Depending on the master or slave internals, it may convert 0s to NLs. - * - * - A slave is always in RX state - it must always be able to receive message data (MOSI). - * - * - All SPI transmissions are initiated by the master and are 8 bits long. As the - * slave is receiving the byte on MOSI it should be returning the next character - * in its output buffer on MISO. Note that there is no inherent correlation between - * the char (or message) being received from the master and transmitted from the - * slave. It's just IO. - * - * If the slave has no data to send it should return ETX (0x03) on MISO. This is - * useful to distinghuish between an "empty" slave and an unpopulated or non- - * responsive SPI slave - which would return 0s or possibly 0xFF. - * - * - The master may poll for more message data from the slave by sending STX chars to - * the slave. The slave discards all STXs and simply returns output data on these - * transfers. Presumably the master would stop polling once it receives an ETX - * from the slave. - */ -/* ---- Low level SPI stuff ---- - * - * Uses Mode3, MSB first. See Atmel Xmega A 8077.doc, page 231 - */ -#include // precursor for xio.h -#include // true and false -#include // for memset -#include // precursor for xio.h -#include -#include // needed for blocking TX - -#include "xio.h" // includes for all devices are in here -#include "../xmega/xmega_interrupts.h" -#include "../tinyg.h" // needed for AXES definition - -// statics -static char _read_rx_buffer(xioSpi_t *dx); -static char _write_rx_buffer(xioSpi_t *dx, char c); -static char _read_tx_buffer(xioSpi_t *dx); -//static char _write_tx_buffer(xioSpi_t *dx, char c); -static char _xfer_spi_char(xioSpi_t *dx, char c_out); -static char _read_spi_char(xioSpi_t *dx); - -/****************************************************************************** - * SPI CONFIGURATION RECORDS - ******************************************************************************/ - -typedef struct cfgSPI { - x_open_t x_open; // see xio.h for typedefs - x_ctrl_t x_ctrl; - x_gets_t x_gets; - x_getc_t x_getc; - x_putc_t x_putc; - x_flow_t x_flow; - USART_t *usart; // usart binding or BIT_BANG if no usart used - PORT_t *comm_port; // port for SCK, MISO and MOSI - PORT_t *ssel_port; // port for slave select line - uint8_t ssbit; // slave select bit on ssel_port - uint8_t inbits; // bits to set as inputs - uint8_t outbits; // bits to set as outputs - uint8_t outclr; // output bits to initialize as CLRd - uint8_t outset; // output bits to initialize as SET -} cfgSpi_t; - -static cfgSpi_t const cfgSpi[] PROGMEM = { - { - xio_open_spi, // SPI #1 configs - xio_ctrl_generic, - xio_gets_spi, - xio_getc_spi, - xio_putc_spi, - xio_fc_null, - BIT_BANG, - &SPI_DATA_PORT, - &SPI_SS1_PORT, - SPI_SS1_bm, - SPI_INBITS_bm, - SPI_OUTBITS_bm, - SPI_OUTCLR_bm, - SPI_OUTSET_bm, - }, - { - xio_open_spi, // SPI #2 configs - xio_ctrl_generic, - xio_gets_spi, - xio_getc_spi, - xio_putc_spi, - xio_fc_null, - BIT_BANG, - &SPI_DATA_PORT, - &SPI_SS2_PORT, - SPI_SS2_bm, - SPI_INBITS_bm, - SPI_OUTBITS_bm, - SPI_OUTCLR_bm, - SPI_OUTSET_bm, - } -}; - -/****************************************************************************** - * FUNCTIONS - ******************************************************************************/ - -/* - * xio_init_spi() - top-level init for SPI sub-system - */ -void xio_init_spi() -{ - for (uint8_t i=0; ix = &spi[idx]; // setup extended struct pointer - xioSpi_t *dx = (xioSpi_t *)d->x; - - memset (dx, 0, sizeof(xioSpi_t)); - xio_reset_working_flags(d); - xio_ctrl_generic(d, flags); - - // setup internal RX/TX control buffers - dx->rx_buf_head = 1; - dx->rx_buf_tail = 1; - dx->tx_buf_head = 1; - dx->tx_buf_tail = 1; - - // structure and device bindings and setup - dx->usart = (USART_t *)pgm_read_word(&cfgSpi[idx].usart); - dx->data_port = (PORT_t *)pgm_read_word(&cfgSpi[idx].comm_port); - dx->ssel_port = (PORT_t *)pgm_read_word(&cfgSpi[idx].ssel_port); - - dx->ssbit = (uint8_t)pgm_read_byte(&cfgSpi[idx].ssbit); - dx->data_port->DIRCLR = (uint8_t)pgm_read_byte(&cfgSpi[idx].inbits); - dx->data_port->DIRSET = (uint8_t)pgm_read_byte(&cfgSpi[idx].outbits); - dx->data_port->OUTCLR = (uint8_t)pgm_read_byte(&cfgSpi[idx].outclr); - dx->data_port->OUTSET = (uint8_t)pgm_read_byte(&cfgSpi[idx].outset); - return &d->file; // return FILE reference -} - -/* - * xio_gets_spi() - read a complete line (message) from an SPI device - * - * Reads from the local RX buffer until it's empty, then reads from the - * slave until the line is complete or the slave is exhausted. Retains line - * context across calls - so it can be called multiple times. Reads as many - * characters as it can until any of the following is true: - * - * - Encounters newline indicating a complete line. Terminate the buffer - * but do not write the newlinw into the buffer. Reset line flag. Return XIO_OK. - * - * - Encounters an empty buffer and no more data in the slave. Leave in_line - * Return XIO_EAGAIN. - * - * - A successful read would cause output buffer overflow. Return XIO_BUFFER_FULL - * - * Note: LINEMODE flag in device struct is ignored. It's ALWAYS LINEMODE here. - * Note: CRs are not recognized as NL chars - slaves must send LF to terminate a line - */ -int xio_gets_spi(xioDev_t *d, char *buf, const int size) -{ - xioSpi_t *dx = (xioSpi_t *)d->x; // get SPI device struct pointer - char c_out; - - // first time thru initializations - if (d->flag_in_line == false) { - d->flag_in_line = true; // yes, we are busy getting a line - d->buf = buf; // bind the output buffer - d->len = 0; // zero the buffer count - d->size = size; // set the max size of the message -// d->signal = XIO_SIG_OK; // reset signal register - } - while (true) { - if (d->len >= (d->size)-1) { // size is total count - aka 'num' in fgets() - d->buf[d->size] = 0; // string termination preserves latest char - return XIO_BUFFER_FULL; - } - if ((c_out = _read_rx_buffer(dx)) == Q_EMPTY) { - if ((c_out = _read_spi_char(dx)) == ETX) { // get a char from slave - return XIO_EAGAIN; - } - } - if (c_out == LF) { - d->buf[(d->len)++] = 0; - d->flag_in_line = false; // clear in-line state (reset) - return XIO_OK; // return for end-of-line - } - d->buf[(d->len)++] = c_out; // write character to buffer - } -} - -/* - * xio_getc_spi() - stdio compatible character RX routine - * - * This function first tries to return a character from the master's RX queue - * and if that fails it tries to get the next character from the slave. - * - * This function is always non-blocking or it would create a deadlock as the - * bit-banger SPI transmitter is not interrupt driven - * - * This function is not optimized for transfer rate, as it returns a single - * character and has no state information about the slave. gets() is much more - * efficient. - */ -int xio_getc_spi(FILE *stream) -{ - xioDev_t *d = (xioDev_t *)stream->udata; // get device struct pointer - xioSpi_t *dx = (xioSpi_t *)d->x; // get SPI device struct pointer - char c; - - if ((c = _read_rx_buffer(dx)) == Q_EMPTY) { - if ((c = _read_spi_char(dx)) == ETX) { - d->signal = XIO_SIG_EOL; - return(_FDEV_ERR); - } - } - return c; -} - -//void _xio_tx_spi_dx(xioSpi_t *dx); - -/* - * xio_putc_spi() - stdio compatible character TX routine - * - * Putc is split in 2: xio_putc_spi() puts the char in the TX buffer, while - * xio_tx_spi() transmits a char from the TX buffer to the slave. This is not - * necessary for a pure main-loop bitbanger, but is needed for interrupts or - * other asynchronous IO processing. - */ -int xio_putc_spi(const char c, FILE *stream) -{ - // Native SPI device version - // write to TX queue - char TX occurs via SPI interrupt -// return (int_write_tx_buffer(((xioDev_t *)stream->udata)->x,c)); - - // bit banger version - unbuffered IO - xioSpi_t *dx = ((xioDev_t *)stream->udata)->x; - char c_in; - - if ((c_in = _xfer_spi_char(dx,c)) != ETX) { - if ((c_in == 0x00) || (c_in == 0xFF)) { - return XIO_NO_SUCH_DEVICE; - } - _write_rx_buffer(dx, c_in); - } - return XIO_OK; -} -/* - int status = _write_tx_buffer(dx,c); - if (status != XIO_OK) { return status;} - char c_out, c_in; - if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return ();} - if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { - _write_rx_buffer(dx, c_in); - } - return XIO_OK; -} -*/ - -/* - * xio_tx_spi() - send a character trom the TX buffer to the slave - * - * Send a char to the slave while receiving a char from the slave on MISO. - * Load received char into the RX buffer if it's a legitimate.character. - */ -void xio_tx_spi(uint8_t dev) -{ - xioDev_t *d = &ds[dev]; - xioSpi_t *dx = (xioSpi_t *)d->x; - char c_out, c_in; - - if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return;} - if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { - _write_rx_buffer(dx, c_in); - } -} -/* -void _xio_tx_spi_dx(xioSpi_t *dx) -{ - char c_out, c_in; - if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return;} - if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { - _write_rx_buffer(dx, c_in); - } -} -*/ -/* - * Buffer read and write helpers - * - * READ: Read from the tail. Read sequence is: - * - test buffer and return Q_empty if empty - * - read char from buffer - * - advance the tail (post-advance) - * - return C with tail left pointing to next char to be read (or no data) - * - * WRITES: Write to the head. Write sequence is: - * - advance a temporary head (pre-advance) - * - test buffer and return Q_empty if empty - * - commit head advance to structure - * - return status with head left pointing to latest char written - * - * You can make these blocking routines by calling them in an infinite - * while() waiting for something other than Q_EMPTY to be returned. - */ - -static char _read_rx_buffer(xioSpi_t *dx) -{ - if (dx->rx_buf_head == dx->rx_buf_tail) { return Q_EMPTY;} - char c = dx->rx_buf[dx->rx_buf_tail]; - if ((--(dx->rx_buf_tail)) == 0) { dx->rx_buf_tail = SPI_RX_BUFFER_SIZE-1;} - return c; -} - -static char _write_rx_buffer(xioSpi_t *dx, char c) -{ - spibuf_t next_buf_head = dx->rx_buf_head-1; - if (next_buf_head == 0) { next_buf_head = SPI_RX_BUFFER_SIZE-1;} - if (next_buf_head == dx->rx_buf_tail) { return Q_EMPTY;} - dx->rx_buf[next_buf_head] = c; - dx->rx_buf_head = next_buf_head; - return XIO_OK; -} - -static char _read_tx_buffer(xioSpi_t *dx) -{ - if (dx->tx_buf_head == dx->tx_buf_tail) { return Q_EMPTY;} - char c = dx->tx_buf[dx->tx_buf_tail]; - if ((--(dx->tx_buf_tail)) == 0) { dx->tx_buf_tail = SPI_TX_BUFFER_SIZE-1;} - return c; -} -/* -static char _write_tx_buffer(xioSpi_t *dx, char c) -{ - spibuf_t next_buf_head = dx->tx_buf_head-1; - if (next_buf_head == 0) { next_buf_head = SPI_TX_BUFFER_SIZE-1;} - if (next_buf_head == dx->tx_buf_tail) { return Q_EMPTY;} - dx->tx_buf[next_buf_head] = c; - dx->tx_buf_head = next_buf_head; - return XIO_OK; -} -*/ -/* - * Bitbangers used by the SPI routines - * _xfer_spi_char() - send a character on MOSI and receive incoming char on MISO - * _read_spi_char() - send an STX on MOSI and receive incoming char on MISO - */ - -#define xfer_bit(mask, c_out, c_in) \ - dx->data_port->OUTCLR = SPI_SCK_bm; \ - if ((c_out & mask) == 0) { dx->data_port->OUTCLR = SPI_MOSI_bm; } \ - else { dx->data_port->OUTSET = SPI_MOSI_bm; } \ - if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ - dx->data_port->OUTSET = SPI_SCK_bm; - -#define read_bit_clr(mask, c_in) \ - dx->data_port->OUTCLR = SPI_SCK_bm; \ - dx->data_port->OUTCLR = SPI_MOSI_bm; \ - if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ - dx->data_port->OUTSET = SPI_SCK_bm; - -#define read_bit_set(mask, c_in) \ - dx->data_port->OUTCLR = SPI_SCK_bm; \ - dx->data_port->OUTSET = SPI_MOSI_bm; \ - if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ - dx->data_port->OUTSET = SPI_SCK_bm; - -static char _xfer_spi_char(xioSpi_t *dx, char c_out) -{ - char c_in = 0; - dx->ssel_port->OUTCLR = dx->ssbit; // drive slave select lo (active) - xfer_bit(0x80, c_out, c_in); - xfer_bit(0x40, c_out, c_in); - xfer_bit(0x20, c_out, c_in); - xfer_bit(0x10, c_out, c_in); - xfer_bit(0x08, c_out, c_in); - xfer_bit(0x04, c_out, c_in); - xfer_bit(0x02, c_out, c_in); - xfer_bit(0x01, c_out, c_in); - dx->ssel_port->OUTSET = dx->ssbit; - return c_in; -} - -static char _read_spi_char(xioSpi_t *dx) -{ - char c_in = 0; - dx->ssel_port->OUTCLR = dx->ssbit; // drive slave select lo (active) - read_bit_clr(0x80, c_in); // hard coded to send STX - read_bit_clr(0x40, c_in); - read_bit_clr(0x20, c_in); - read_bit_clr(0x10, c_in); - read_bit_clr(0x08, c_in); - read_bit_clr(0x04, c_in); - read_bit_set(0x02, c_in); - read_bit_clr(0x01, c_in); - dx->ssel_port->OUTSET = dx->ssbit; - return c_in; -} diff --git a/src/xio/xio_spi.h b/src/xio/xio_spi.h deleted file mode 100755 index 4142221..0000000 --- a/src/xio/xio_spi.h +++ /dev/null @@ -1,101 +0,0 @@ -/* - * xio_spi.h - General purpose SPI device driver for xmega family - * - works with avr-gcc stdio library - * - * Part of TinyG project - * - * Copyright (c) 2013 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#ifndef xio_spi_h -#define xio_spi_h - -/****************************************************************************** - * SPI DEVICE CONFIGS (applied during device-specific inits) - ******************************************************************************/ - -// SPI global accessor defines -#define SPI1 ds[XIO_DEV_SPI1] // device struct accessor -#define SPI1u sp[XIO_DEV_SPI1 - XIO_DEV_SPI_OFFSET] // usart extended struct accessor - -#define SPI2 ds[XIO_DEV_SPI2] // device struct accessor -#define SPI2u sp[XIO_DEV_SPI2 - XIO_DEV_SPI_OFFSET] // usart extended struct accessor - -// Buffer sizing -#define spibuf_t uint_fast8_t // fast, but limits SPI buffers to 255 char max -#define SPI_RX_BUFFER_SIZE (spibuf_t)64 -#define SPI_TX_BUFFER_SIZE (spibuf_t)64 - -//**** SPI device configuration **** -//NOTE: XIO_BLOCK / XIO_NOBLOCK affects reads only. Writes always block. (see xio.h) - -#define SPI_FLAGS (XIO_BLOCK | XIO_ECHO | XIO_LINEMODE) - -#define BIT_BANG 0 // use this value if no USART is being used -#define SPI_USART BIT_BANG // USB usart or BIT_BANG value -#define SPI_RX_ISR_vect BIT_BANG // (RX) reception complete IRQ -#define SPI_TX_ISR_vect BIT_BANG // (TX) data register empty IRQ - -// The bit mappings for SCK / MISO / MOSI / SS1 map to the xmega SPI device pinouts -#define SPI_DATA_PORT PORTC // port for SPI data lines -#define SPI_SCK_bp 7 // SCK - clock bit position (pin is wired on board) -#define SPI_MISO_bp 6 // MISO - bit position (pin is wired on board) -#define SPI_MOSI_bp 5 // MOSI - bit position (pin is wired on board) -#define SPI_SS1_PORT SPI_DATA_PORT // slave select assignments -#define SPI_SS1_bp 4 // SS1 - slave select #1 -// additional slave selects -#define SPI_SS2_PORT PORTB -#define SPI_SS2_bp 3 // SS1 - slave select #2 - -#define SPI_MOSI_bm (1<. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -#include // precursor for xio.h -#include // true and false -#include // for memset -#include // precursor for xio.h -#include -#include // needed for blocking character writes - -#include "xio.h" // includes for all devices are in here -#include "../xmega/xmega_interrupts.h" - -#include "../tinyg.h" // needed for AXES definition -#include "../gpio.h" // needed for XON/XOFF LED indicator -#include "../util.h" // needed to pick up __debug defines -#include "../config.h" // needed to write back usb baud rate - -/****************************************************************************** - * USART CONFIGURATION RECORDS - ******************************************************************************/ -// See xio_usart.h for baud rate configuration settings -static const uint8_t bsel[] PROGMEM = { 0, 207, 103, 51, 34, 33, 31, 27, 19, 1, 1 }; -static const uint8_t bscale[] PROGMEM = { 0, 0, 0, 0, 0, (-1<<4), (-2<<4), (-3<<4), (-4<<4), (1<<4), 1 }; - -struct cfgUSART { - x_open_t x_open; - x_ctrl_t x_ctrl; - x_gets_t x_gets; - x_getc_t x_getc; - x_putc_t x_putc; - x_flow_t x_flow; - USART_t *usart; // usart binding - PORT_t *port; // port binding - uint8_t baud; - uint8_t inbits; - uint8_t outbits; - uint8_t outclr; - uint8_t outset; -}; - -static struct cfgUSART const cfgUsart[] PROGMEM = { - { - xio_open_usart, // USB config record - xio_ctrl_generic, - xio_gets_usart, - xio_getc_usart, - xio_putc_usb, - xio_fc_usart, - &USB_USART, - &USB_PORT, - USB_BAUD, - USB_INBITS_bm, - USB_OUTBITS_bm, - USB_OUTCLR_bm, - USB_OUTSET_bm - }, - { - xio_open_usart, // RS485 config record - xio_ctrl_generic, - xio_gets_usart, - xio_getc_usart, - xio_putc_rs485, - xio_fc_null, - &RS485_USART, - &RS485_PORT, - RS485_BAUD, - RS485_INBITS_bm, - RS485_OUTBITS_bm, - RS485_OUTCLR_bm, - RS485_OUTSET_bm - } -}; - -/****************************************************************************** - * FUNCTIONS - ******************************************************************************/ - -static int _gets_helper(xioDev_t *d, xioUsart_t *dx); - -/* - * xio_init_usart() - general purpose USART initialization (shared) - */ -void xio_init_usart() -{ - for (uint8_t i=0; ix = &us[idx]; // bind extended struct to device - xioUsart_t *dx = (xioUsart_t *)d->x; - - memset (dx, 0, sizeof(xioUsart_t)); // clear all values - xio_reset_working_flags(d); - xio_ctrl_generic(d, flags); // setup control flags - if (d->flag_xoff) { // initialize flow control settings - dx->fc_state_rx = FC_IN_XON; - dx->fc_state_tx = FC_IN_XON; - } - - // setup internal RX/TX control buffers - dx->rx_buf_head = 1; // can't use location 0 in circular buffer - dx->rx_buf_tail = 1; - dx->tx_buf_head = 1; - dx->tx_buf_tail = 1; - - // baud rate and USART setup (do this last) - dx->usart = (USART_t *)pgm_read_word(&cfgUsart[idx].usart); - dx->port = (PORT_t *)pgm_read_word(&cfgUsart[idx].port); - uint8_t baud = (uint8_t)pgm_read_byte(&cfgUsart[idx].baud); - if (baud == XIO_BAUD_UNSPECIFIED) { baud = XIO_BAUD_DEFAULT; } - xio_set_baud_usart(dx, baud); // usart must be bound first - dx->port->DIRCLR = (uint8_t)pgm_read_byte(&cfgUsart[idx].inbits); - dx->port->DIRSET = (uint8_t)pgm_read_byte(&cfgUsart[idx].outbits); - dx->port->OUTCLR = (uint8_t)pgm_read_byte(&cfgUsart[idx].outclr); - dx->port->OUTSET = (uint8_t)pgm_read_byte(&cfgUsart[idx].outset); - dx->usart->CTRLB = (USART_TXEN_bm | USART_RXEN_bm); // enable tx and rx - dx->usart->CTRLA = CTRLA_RXON_TXON; // enable tx and rx IRQs - - dx->port->USB_CTS_PINCTRL = PORT_OPC_TOTEM_gc | PORT_ISC_BOTHEDGES_gc; - dx->port->INTCTRL = USB_CTS_INTLVL; // see xio_usart.h for setting - dx->port->USB_CTS_INTMSK = USB_CTS_bm; - - return &d->file; // return FILE reference - - // here's a bag for the RS485 device - if (dev == XIO_DEV_RS485) { - xio_enable_rs485_rx(); // sets RS-485 to RX mode initially - } -} - -void xio_set_baud_usart(xioUsart_t *dx, const uint8_t baud) -{ - dx->usart->BAUDCTRLA = (uint8_t)pgm_read_byte(&bsel[baud]); - dx->usart->BAUDCTRLB = (uint8_t)pgm_read_byte(&bscale[baud]); - cfg.usb_baud_rate = baud; -} - -/* - * USART flow control functions and helpers - * - * xio_xoff_usart() - send XOFF flow control for USART devices - * xio_xon_usart() - send XON flow control for USART devices - * xio_fc_usart() - Usart device flow control callback - * xio_get_tx_bufcount_usart() - returns number of chars in TX buffer - * xio_get_rx_bufcount_usart() - returns number of chars in RX buffer - * - * Reminder: tx/rx queues fill from top to bottom, w/0 being the wrap location - */ - -void xio_xoff_usart(xioUsart_t *dx) -{ - if (dx->fc_state_rx == FC_IN_XON) { - dx->fc_state_rx = FC_IN_XOFF; - - // If using XON/XOFF flow control - if (cfg.enable_flow_control == FLOW_CONTROL_XON) { - dx->fc_char_rx = XOFF; - dx->usart->CTRLA = CTRLA_RXON_TXON; // force a TX interrupt - } - - // If using hardware flow control. The CTS pin on the *FTDI* is our RTS. - // Logic 1 means we're NOT ready for more data. - if (cfg.enable_flow_control == FLOW_CONTROL_RTS) { - dx->port->OUTSET = USB_RTS_bm; - } - } -} - -void xio_xon_usart(xioUsart_t *dx) -{ - if (dx->fc_state_rx == FC_IN_XOFF) { - dx->fc_state_rx = FC_IN_XON; - - // If using XON/XOFF flow control - if (cfg.enable_flow_control == FLOW_CONTROL_XON) { - dx->fc_char_rx = XON; - dx->usart->CTRLA = CTRLA_RXON_TXON; // force a TX interrupt - } - - // If using hardware flow control. The CTS pin on the *FTDI* is our RTS. - // Logic 0 means we're ready for more data. - if (cfg.enable_flow_control == FLOW_CONTROL_RTS) { - dx->port->OUTCLR = USB_RTS_bm; - } - } -} - -void xio_fc_usart(xioDev_t *d) // callback from the usart handlers -{ - xioUsart_t *dx = d->x; - if (xio_get_rx_bufcount_usart(dx) < XOFF_RX_LO_WATER_MARK) { - xio_xon_usart(dx); - } -} - -buffer_t xio_get_tx_bufcount_usart(const xioUsart_t *dx) -{ - if (dx->tx_buf_head <= dx->tx_buf_tail) { - return dx->tx_buf_tail - dx->tx_buf_head; - } else { - return TX_BUFFER_SIZE - (dx->tx_buf_head - dx->tx_buf_tail); - } -} - -buffer_t xio_get_rx_bufcount_usart(const xioUsart_t *dx) -{ -// return dx->rx_buf_count; - if (dx->rx_buf_head <= dx->rx_buf_tail) { - return dx->rx_buf_tail - dx->rx_buf_head; - } else { - return RX_BUFFER_SIZE - (dx->rx_buf_head - dx->rx_buf_tail); - } -} - -/* - * xio_gets_usart() - read a complete line from the usart device - * _gets_helper() - non-blocking character getter for gets - * - * Retains line context across calls - so it can be called multiple times. - * Reads as many characters as it can until any of the following is true: - * - * - RX buffer is empty on entry (return XIO_EAGAIN) - * - no more chars to read from RX buffer (return XIO_EAGAIN) - * - read would cause output buffer overflow (return XIO_BUFFER_FULL) - * - read returns complete line (returns XIO_OK) - * - * Note: LINEMODE flag in device struct is ignored. It's ALWAYS LINEMODE here. - * Note: This function assumes ignore CR and ignore LF handled upstream before the RX buffer - */ -int xio_gets_usart(xioDev_t *d, char *buf, const int size) -{ - xioUsart_t *dx = d->x; // USART pointer - - if (d->flag_in_line == false) { // first time thru initializations - d->flag_in_line = true; // yes, we are busy getting a line - d->len = 0; // zero buffer - d->buf = buf; - d->size = size; - d->signal = XIO_SIG_OK; // reset signal register - } - while (true) { - switch (_gets_helper(d,dx)) { - case (XIO_BUFFER_EMPTY): return XIO_EAGAIN; // empty condition - case (XIO_BUFFER_FULL): return XIO_BUFFER_FULL;// overrun err - case (XIO_EOL): return XIO_OK; // got complete line - case (XIO_EAGAIN): break; // loop for next character - } - } - return XIO_OK; -} - -static int _gets_helper(xioDev_t *d, xioUsart_t *dx) -{ - char c = 0; - - if (dx->rx_buf_head == dx->rx_buf_tail) { // RX ISR buffer empty - dx->rx_buf_count = 0; // reset count for good measure - return(XIO_BUFFER_EMPTY); // stop reading - } - advance_buffer(dx->rx_buf_tail, RX_BUFFER_SIZE); - dx->rx_buf_count--; - d->x_flow(d); // run flow control -// c = dx->rx_buf[dx->rx_buf_tail]; // get char from RX Q - c = (dx->rx_buf[dx->rx_buf_tail] & 0x007F); // get char from RX Q & mask MSB - if (d->flag_echo) d->x_putc(c, stdout); // conditional echo regardless of character - - if (d->len >= d->size) { // handle buffer overruns - d->buf[d->size] = 0; // terminate line (d->size is zero based) - d->signal = XIO_SIG_EOL; - return XIO_BUFFER_FULL; - } - if ((c == CR) || (c == LF)) { // handle CR, LF termination - d->buf[(d->len)++] = 0; - d->signal = XIO_SIG_EOL; - d->flag_in_line = false; // clear in-line state (reset) - return XIO_EOL; // return for end-of-line - } - d->buf[(d->len)++] = c; // write character to buffer - return XIO_EAGAIN; -} - -/* - * xio_getc_usart() - generic char reader for USART devices - * - * Compatible with stdio system - may be bound to a FILE handle - * - * Get next character from RX buffer. - * See https://www.synthetos.com/wiki/index.php?title=Projects:TinyG-Module-Details#Notes_on_Circular_Buffers - * for a discussion of how the circular buffers work - * - * This routine returns a single character from the RX buffer to the caller. - * It's typically called by fgets() and is useful for single-threaded IO cases. - * Cases with multiple concurrent IO streams may want to use the gets() function - * which is incompatible with the stdio system. - * - * Flags that affect behavior: - * - * BLOCKING behaviors - * - execute blocking or non-blocking read depending on controls - * - return character or -1 & XIO_SIG_WOULDBLOCK if non-blocking - * - return character or sleep() if blocking - * - * ECHO behaviors - * - if ECHO is enabled echo character to stdout - * - echo all line termination chars as newlines ('\n') - * - Note: putc is responsible for expanding newlines to if needed - */ -int xio_getc_usart(FILE *stream) -{ - // these convenience pointers optimize faster than resolving the references each time - xioDev_t *d = (xioDev_t *)stream->udata; - xioUsart_t *dx = d->x; - char c; - - while (dx->rx_buf_head == dx->rx_buf_tail) { // RX ISR buffer empty - dx->rx_buf_count = 0; // reset count for good measure - if (d->flag_block) { - sleep_mode(); - } else { - d->signal = XIO_SIG_EAGAIN; - return(_FDEV_ERR); - } - } - advance_buffer(dx->rx_buf_tail, RX_BUFFER_SIZE); - dx->rx_buf_count--; - d->x_flow(d); // flow control callback - c = (dx->rx_buf[dx->rx_buf_tail] & 0x007F); // get char from RX buf & mask MSB - - // Triage the input character for handling. This code does not handle deletes - if (d->flag_echo) d->x_putc(c, stdout); // conditional echo regardless of character - if (c > CR) return(c); // fast cutout for majority cases - if ((c == CR) || (c == LF)) { - if (d->flag_linemode) return('\n'); - } - return(c); -} - -/* - * xio_putc_usart() - stdio compatible char writer for usart devices - * This routine is not needed at the class level. - * See xio_putc_usb() and xio_putc_rs485() - */ -int xio_putc_usart(const char c, FILE *stream) -{ - return XIO_OK; -} - -/* Fakeout routines - * - * xio_queue_RX_string_usart() - fake ISR to put a string in the RX buffer - * xio_queue_RX_char_usart() - fake ISR to put a char in the RX buffer - * - * String must be 0 terminated but doesn't require a CR or LF - * Also has wrappers for USB and RS485 - */ -//void xio_queue_RX_char_usb(const char c) { xio_queue_RX_char_usart(XIO_DEV_USB, c); } -void xio_queue_RX_string_usb(const char *buf) { xio_queue_RX_string_usart(XIO_DEV_USB, buf); } -//void xio_queue_RX_char_rs485(const char c) { xio_queue_RX_char_usart(XIO_DEV_RS485, c); } -//void xio_queue_RX_string_rs485(const char *buf) { xio_queue_RX_string_usart(XIO_DEV_RS485, buf); } - -void xio_queue_RX_string_usart(const uint8_t dev, const char *buf) -{ - uint8_t i=0; - while (buf[i] != 0) { - xio_queue_RX_char_usart(dev, buf[i++]); - } -} - -void xio_queue_RX_char_usart(const uint8_t dev, const char c) -{ - xioDev_t *d = &ds[dev]; - xioUsart_t *dx = d->x; - - // normal path - advance_buffer(dx->rx_buf_head, RX_BUFFER_SIZE); - if (dx->rx_buf_head != dx->rx_buf_tail) { // write char unless buffer full - dx->rx_buf[dx->rx_buf_head] = c; // FAKE INPUT DATA - dx->rx_buf_count++; - return; - } - // buffer-full handling - if ((++dx->rx_buf_head) > RX_BUFFER_SIZE-1) { // reset the head - dx->rx_buf_count = RX_BUFFER_SIZE-1; - dx->rx_buf_head = 1; - } -} - diff --git a/src/xio/xio_usart.h b/src/xio/xio_usart.h deleted file mode 100755 index bdab410..0000000 --- a/src/xio/xio_usart.h +++ /dev/null @@ -1,191 +0,0 @@ -/* - * xio_usart.h - Common USART definitions - * Part of TinyG project - * - * Copyright (c) 2010 - 2013 Alden S. Hart Jr. - * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ -/* - * Rob: The usart.h and .c files can be considered the parent class for the - * USB and RS485 devices which are derived from them. The usart.h file acts - * as the header file for all three classes: usart.c, usb.c and rs485.c - */ - -#ifndef xio_usart_h -#define xio_usart_h - -/****************************************************************************** - * USART DEVICE CONFIGS (applied during device-specific inits) - ******************************************************************************/ - -// Serial IO Interrupt levels -// Maps both RX and TX to medium interrupt levels -#define CTRLA_RXON_TXON (USART_RXCINTLVL_MED_gc | USART_DREINTLVL_MED_gc) -#define CTRLA_RXON_TXOFF (USART_RXCINTLVL_MED_gc) -#define CTRLA_RXON_TXOFF_TXCON (USART_RXCINTLVL_MED_gc | USART_TXCINTLVL_MED_gc) -#define CTRLA_RXOFF_TXON_TXCON (USART_DREINTLVL_MED_gc | USART_TXCINTLVL_MED_gc) -#define CTRLA_RXOFF_TXOFF_TXCON (USART_TXCINTLVL_MED_gc) - -// Buffer sizing -#define buffer_t uint_fast8_t // fast, but limits buffer to 255 char max -//#define buffer_t uint16_t // larger buffers - -// Must reserve 2 bytes for buffer management -#define RX_BUFFER_SIZE (buffer_t)254 // buffer_t can be 8 bits -#define TX_BUFFER_SIZE (buffer_t)254 // buffer_t can be 8 bits - -// XON/XOFF hi and lo watermarks. At 115.200 the host has approx. 100 uSec per char -// to react to an XOFF. 90% (0.9) of 255 chars gives 25 chars to react, or about 2.5 ms. -#define XOFF_RX_HI_WATER_MARK (RX_BUFFER_SIZE * 0.8) // % to issue XOFF -#define XOFF_RX_LO_WATER_MARK (RX_BUFFER_SIZE * 0.1) // % to issue XON -#define XOFF_TX_HI_WATER_MARK (TX_BUFFER_SIZE * 0.9) // % to issue XOFF -#define XOFF_TX_LO_WATER_MARK (TX_BUFFER_SIZE * 0.05) // % to issue XON - -// General -#define USART_TX_REGISTER_READY_bm USART_DREIF_bm -#define USART_RX_DATA_READY_bm USART_RXCIF_bm - -//**** USB device configuration **** -//NOTE: XIO_BLOCK / XIO_NOBLOCK affects reads only. Writes always block. (see xio.h) - -#define USB_BAUD XIO_BAUD_115200 -#define USB_FLAGS (XIO_BLOCK | XIO_ECHO | XIO_XOFF | XIO_LINEMODE ) - -#define USB_USART USARTC0 // USB usart -#define USB_RX_ISR_vect USARTC0_RXC_vect // (RX) reception complete IRQ -#define USB_TX_ISR_vect USARTC0_DRE_vect // (TX) data register empty IRQ - -#define USB_PORT PORTC // port where the USART is located -#define USB_CTS_bp (1) // CTS - bit position (pin is wired on board) -#define USB_CTS_bm (1<. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include // precursor for xio.h -#include // true and false -#include // precursor for xio.h -#include -#include // needed for blocking TX - -#include "xio.h" // includes for all devices are in here -#include "../xmega/xmega_interrupts.h" - -// application specific stuff that's littered into the USB handler -#include "../tinyg.h" -#include "../config.h" // needed to find flow control setting -#include "../network.h" -#include "../hardware.h" -#include "../controller.h" -#include "../canonical_machine.h" // trapped characters communicate directly with the canonical machine - -/* - * xio_putc_usb() - * USB_TX_ISR - USB transmitter interrupt (TX) used by xio_usb_putc() - * - * These are co-routines that work in tandem. - * xio_putc_usb() is a more efficient form derived from xio_putc_usart() - * - * The TX interrupt dilemma: TX interrupts occur when the USART DATA register is - * empty (and the ISR must disable interrupts when nothing's left to read, or they - * keep firing). If the TX buffer is completely empty (TXCIF is set) then enabling - * interrupts does no good. The USART won't interrupt and the TX circular buffer - * never empties. So the routine that puts chars in the TX buffer must always force - * an interrupt. - */ - -int xio_putc_usb(const char c, FILE *stream) -{ - buffer_t next_tx_buf_head = USBu.tx_buf_head-1; // set next head while leaving current one alone - if (next_tx_buf_head == 0) - next_tx_buf_head = TX_BUFFER_SIZE-1; // detect wrap and adjust; -1 avoids off-by-one - while (next_tx_buf_head == USBu.tx_buf_tail) - sleep_mode(); // sleep until there is space in the buffer - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // disable TX interrupt (mutex region) - USBu.tx_buf_head = next_tx_buf_head; // accept next buffer head - USBu.tx_buf[USBu.tx_buf_head] = c; // write char to buffer - - // expand to if $ec is set - if ((c == '\n') && (USB.flag_crlf)) { - USBu.usart->CTRLA = CTRLA_RXON_TXON; // force interrupt to send the queued - buffer_t next_tx_buf_head = USBu.tx_buf_head-1; - if (next_tx_buf_head == 0) next_tx_buf_head = TX_BUFFER_SIZE-1; - while (next_tx_buf_head == USBu.tx_buf_tail) sleep_mode(); - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // MUTEX region - USBu.tx_buf_head = next_tx_buf_head; - USBu.tx_buf[USBu.tx_buf_head] = CR; - } - // finish up - USBu.usart->CTRLA = CTRLA_RXON_TXON; // force interrupt to send char(s) - doesn't work if you just |= it - return (XIO_OK); -} - -ISR(USB_TX_ISR_vect) //ISR(USARTC0_DRE_vect) // USARTC0 data register empty -{ - // If the CTS pin (FTDI's RTS) is HIGH, then we cannot send anything, so exit - if ((cfg.enable_flow_control == FLOW_CONTROL_RTS) && (USBu.port->IN & USB_CTS_bm)) { - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // force another TX interrupt - return; - } - - // Send an RX-side XON or XOFF character if queued - if (USBu.fc_char_rx != 0) { // an XON/ of XOFF needs to be sent - USBu.usart->DATA = USBu.fc_char_rx; // send the XON/XOFF char and exit - USBu.fc_char_rx = 0; - return; - } - - // Halt transmission while in TX-side XOFF - if (USBu.fc_state_tx == FC_IN_XOFF) { - return; - } - - // Otherwise process normal TX transmission - if (USBu.tx_buf_head != USBu.tx_buf_tail) { // buffer has data - advance_buffer(USBu.tx_buf_tail, TX_BUFFER_SIZE); - USBu.usart->DATA = USBu.tx_buf[USBu.tx_buf_tail]; - } else { - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // buffer has no data; force another interrupt - } -} - -/* - * Pin Change (edge-detect) interrupt for CTS pin. - */ - -ISR(USB_CTS_ISR_vect) -{ - USBu.usart->CTRLA = CTRLA_RXON_TXON; // force another interrupt -} - -/* - * USB_RX_ISR - USB receiver interrupt (RX) - * - * RX buffer states can be one of: - * - buffer has space (CTS should be asserted) - * - buffer is full (CTS should be not_asserted) - * - buffer becomes full with this character (write char and assert CTS) - * - * Signals: - * - Signals are captured at the ISR level and either dispatched or flag-set - * - As RX ISR is a critical code region signal handling is stupid and fast - * - signal characters are not put in the RX buffer - * - * Flow Control: - * - Flow control is not implemented. Need to work RTS line. - * - Flow control should cut off at high water mark, re-enable at low water mark - * - High water mark should have about 4 - 8 bytes left in buffer (~95% full) - * - Low water mark about 50% full - */ - -ISR(USB_RX_ISR_vect) //ISR(USARTC0_RXC_vect) // serial port C0 RX int -{ - char c = USBu.usart->DATA; // can only read DATA once - - if (cs.network_mode == NETWORK_MASTER) { // forward character if you are a master - net_forward(c); - } - // trap async commands - do not insert character into RX queue - if (c == CHAR_RESET) { // trap Kill signal - hw_request_hard_reset(); - return; - } - if (c == CHAR_FEEDHOLD) { // trap feedhold signal - cm_request_feedhold(); - return; - } - if (c == CHAR_QUEUE_FLUSH) { // trap queue flush signal - cm_request_queue_flush(); - return; - } - if (c == CHAR_CYCLE_START) { // trap cycle start signal - cm_request_cycle_start(); - return; - } - if (USB.flag_xoff) { - if (c == XOFF) { // trap incoming XON/XOFF signals - USBu.fc_state_tx = FC_IN_XOFF; - return; - } - if (c == XON) { - USBu.fc_state_tx = FC_IN_XON; - USBu.usart->CTRLA = CTRLA_RXON_TXOFF;// force a TX interrupt - return; - } - } - - // filter out CRs and LFs if they are to be ignored -// if ((c == CR) && (USB.flag_ignorecr)) return; // REMOVED IGNORE_CR and IGNORE LF handling -// if ((c == LF) && (USB.flag_ignorelf)) return; - - // normal character path - advance_buffer(USBu.rx_buf_head, RX_BUFFER_SIZE); - if (USBu.rx_buf_head != USBu.rx_buf_tail) { // buffer is not full - USBu.rx_buf[USBu.rx_buf_head] = c; // write char unless full - USBu.rx_buf_count++; - if ((USB.flag_xoff) && (xio_get_rx_bufcount_usart(&USBu) > XOFF_RX_HI_WATER_MARK)) { - xio_xoff_usart(&USBu); - } - } else { // buffer-full - toss the incoming character - if ((++USBu.rx_buf_head) > RX_BUFFER_SIZE-1) { // reset the head - USBu.rx_buf_count = RX_BUFFER_SIZE-1; // reset count for good measure - USBu.rx_buf_head = 1; - } - } -} - -/* - * xio_get_usb_rx_free() - returns free space in the USB RX buffer - * - * Remember: The queues fill from top to bottom, w/0 being the wrap location - */ -buffer_t xio_get_usb_rx_free(void) -{ - return (RX_BUFFER_SIZE - xio_get_rx_bufcount_usart(&USBu)); -} - -/* - * xio_reset_usb_rx_buffers() - clears the USB RX buffer - */ -void xio_reset_usb_rx_buffers(void) -{ - // reset xio_gets() buffer - USB.len = 0; - USB.flag_in_line = false; - - // reset RX interrupt circular buffer - USBu.rx_buf_head = 1; // can't use location 0 in circular buffer - USBu.rx_buf_tail = 1; -}