int cmd_mode_init(t_hydra_console *con, t_tokenline_parsed *p)
{
unsigned int i;
for (i = 0; i < ARRAY_SIZE(modes); i++) {
if (p->tokens[0] != modes[i].token)
continue;
con->mode->exec = modes[i].exec;
if (con->mode->exec->init(con, p) == 0)
return FALSE;
if (!tl_mode_push(con->tl, modes[i].tokens))
return FALSE;
con->console_mode = modes[i].token;
tl_set_prompt(con->tl, (char *)con->mode->exec->get_prompt(con));
}
return TRUE;
}
static int exec(t_hydra_console *con, t_tokenline_parsed *p, int token_pos)
{
mode_config_proto_t* proto = &con->mode->proto;
float arg_float;
int arg_int, t, i;
bsp_status_t bsp_status;
for (t = token_pos; p->tokens[t]; t++) {
switch (p->tokens[t]) {
case T_SHOW:
t += show(con, p);
break;
case T_DEVICE:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if (arg_int < 1 || arg_int > 2) {
cprintf(con, "SPI device must be 1 or 2.\r\n");
return t;
}
init_proto_default(con);
proto->dev_num = arg_int - 1;
bsp_status = bsp_spi_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
tl_set_prompt(con->tl, (char *)con->mode->exec->get_prompt(con));
cprintf(con, "Note: SPI parameters have been reset to default values.\r\n");
break;
case T_PULL:
switch (p->tokens[++t]) {
case T_UP:
proto->dev_gpio_pull = MODE_CONFIG_DEV_GPIO_PULLUP;
break;
case T_DOWN:
proto->dev_gpio_pull = MODE_CONFIG_DEV_GPIO_PULLDOWN;
break;
case T_FLOATING:
proto->dev_gpio_pull = MODE_CONFIG_DEV_GPIO_NOPULL;
break;
}
bsp_status = bsp_spi_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
case T_MODE:
if (p->tokens[++t] == T_MASTER)
proto->dev_mode = DEV_SPI_MASTER;
else
proto->dev_mode = DEV_SPI_SLAVE;
bsp_status = bsp_spi_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
case T_FREQUENCY:
t += 2;
memcpy(&arg_float, p->buf + p->tokens[t], sizeof(float));
for (i = 0; i < SPEED_NB; i++) {
if (arg_float == speeds[proto->dev_num][i]) {
proto->dev_speed = i;
break;
}
}
if (i == 8) {
cprintf(con, "Invalid frequency.\r\n");
return t;
}
bsp_status = bsp_spi_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
case T_POLARITY:
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if (arg_int < 0 || arg_int > 1) {
cprintf(con, "Polarity device must be 0 or 1.\r\n");
return t;
}
proto->dev_polarity = arg_int;
bsp_status = bsp_spi_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
case T_PHASE:
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if (arg_int < 0 || arg_int > 1) {
cprintf(con, "Phase device must be 0 or 1.\r\n");
return t;
}
proto->dev_phase = arg_int;
bsp_spi_init(proto->dev_num, proto);
break;
case T_MSB_FIRST:
proto->dev_bit_lsb_msb = DEV_SPI_FIRSTBIT_MSB;
bsp_status = bsp_spi_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
case T_LSB_FIRST:
proto->dev_bit_lsb_msb = DEV_SPI_FIRSTBIT_LSB;
bsp_status = bsp_spi_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
default:
return t - token_pos;
}
}
return t - token_pos;
}
static int exec(t_hydra_console *con, t_tokenline_parsed *p, int token_pos)
{
mode_config_proto_t* proto = &con->mode->proto;
int arg_int, t;
bsp_status_t bsp_status;
for (t = token_pos; p->tokens[t]; t++) {
switch (p->tokens[t]) {
case T_SHOW:
t += show(con, p);
break;
case T_DEVICE:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if (arg_int < 1 || arg_int > 2) {
cprintf(con, "CAN device must be 1 or 2.\r\n");
return t;
}
proto->dev_num = arg_int - 1;
bsp_status = bsp_can_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
tl_set_prompt(con->tl, (char *)con->mode->exec->get_prompt(con));
cprintf(con, "Note: CAN parameters have been reset to default values.\r\n");
break;
case T_SPEED:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
bsp_status = bsp_can_set_speed(proto->dev_num, arg_int);
proto->config.can.dev_speed = bsp_can_get_speed(proto->dev_num);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
cprintf(con, "Speed: %d bps\r\n", proto->config.can.dev_speed);
break;
case T_TS1:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if(arg_int > 0 && arg_int <= 16) {
bsp_status = bsp_can_set_ts1(proto->dev_num, proto, arg_int);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
} else {
cprintf(con, "Incorrect value\r\n");
}
break;
case T_TS2:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if(arg_int > 0 && arg_int <= 8) {
bsp_status = bsp_can_set_ts2(proto->dev_num, proto, arg_int);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
} else {
cprintf(con, "Incorrect value\r\n");
}
break;
case T_SJW:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if(arg_int > 0 && arg_int <= 4) {
bsp_status = bsp_can_set_sjw(proto->dev_num, proto, arg_int);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
} else {
cprintf(con, "Incorrect value\r\n");
}
break;
case T_FILTER:
/* Integer parameter. */
switch(p->tokens[t+1]) {
case T_OFF:
bsp_status = bsp_can_init_filter(proto->dev_num,
proto);
if(bsp_status != BSP_OK) {
cprintf(con, "Reset filter error %02X", bsp_status);
}
break;
case T_LOW:
memcpy(&arg_int, p->buf + p->tokens[t+3], sizeof(int));
proto->config.can.filter_id_low = arg_int;
bsp_status = bsp_can_set_filter(proto->dev_num,
proto,
proto->config.can.filter_id_low,
proto->config.can.filter_id_high);
break;
case T_HIGH:
memcpy(&arg_int, p->buf + p->tokens[t+3], sizeof(int));
proto->config.can.filter_id_high = arg_int;
bsp_status = bsp_can_set_filter(proto->dev_num,
proto,
proto->config.can.filter_id_low,
proto->config.can.filter_id_high);
break;
}
t+=3;
break;
case T_ID:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t+3], sizeof(int));
proto->config.can.can_id = arg_int;
cprintf(con, "ID set to %d\r\n", proto->config.can.can_id);
break;
case T_CONTINUOUS:
while(!USER_BUTTON) {
read(con, NULL, 0);
}
break;
case T_SLCAN:
if(proto->config.can.dev_mode == BSP_CAN_MODE_RO) {
bsp_can_mode_rw(proto->dev_num, proto);
}
slcan(con);
break;
default:
return t - token_pos;
}
}
return t - token_pos;
}
static int exec(t_hydra_console *con, t_tokenline_parsed *p, int token_pos)
{
mode_config_proto_t* proto = &con->mode->proto;
int arg_int, t;
bsp_status_t bsp_status;
uint32_t final_baudrate;
int baudrate_error_percent;
int baudrate_err_int_part;
int baudrate_err_dec_part;
for (t = token_pos; p->tokens[t]; t++) {
switch (p->tokens[t]) {
case T_SHOW:
t += show(con, p);
break;
case T_DEVICE:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if (arg_int < 1 || arg_int > 2) {
cprintf(con, "UART device must be 1 or 2.\r\n");
return t;
}
proto->dev_num = arg_int - 1;
bsp_status = bsp_uart_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
tl_set_prompt(con->tl, (char *)con->mode->exec->get_prompt(con));
cprintf(con, "Note: UART parameters have been reset to default values.\r\n");
break;
case T_SPEED:
/* Integer parameter. */
t += 2;
memcpy(&proto->dev_speed, p->buf + p->tokens[t], sizeof(int));
bsp_status = bsp_uart_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
final_baudrate = bsp_uart_get_final_baudrate(proto->dev_num);
baudrate_error_percent = 10000 - (int)((float)proto->dev_speed/(float)final_baudrate * 10000.0f);
if(baudrate_error_percent < 0)
baudrate_error_percent = -baudrate_error_percent;
baudrate_err_int_part = (baudrate_error_percent / 100);
baudrate_err_dec_part = (baudrate_error_percent - (baudrate_err_int_part * 100));
if( (final_baudrate < 1) || (baudrate_err_int_part > 5)) {
cprintf(con, "Invalid final baudrate(%d bps/%d.%02d%% err) restore default %d bauds\r\n", final_baudrate, baudrate_err_int_part, baudrate_err_dec_part, UART_DEFAULT_SPEED);
proto->dev_speed = UART_DEFAULT_SPEED;
bsp_status = bsp_uart_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
} else {
cprintf(con, "Final speed: %d bps(%d.%02d%% err)\r\n", final_baudrate, baudrate_err_int_part, baudrate_err_dec_part);
}
break;
case T_PARITY:
/* Token parameter. */
switch (p->tokens[++t]) {
case T_NONE:
proto->dev_parity = 0;
break;
case T_EVEN:
proto->dev_parity = 1;
break;
case T_ODD:
proto->dev_parity = 2;
break;
}
bsp_status = bsp_uart_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
case T_STOP_BITS:
/* Integer parameter. */
t += 2;
memcpy(&arg_int, p->buf + p->tokens[t], sizeof(int));
if (arg_int < 1 || arg_int > 2) {
cprintf(con, "Stop bits must be 1 or 2.\r\n");
return t;
}
proto->dev_stop_bit = arg_int;
bsp_status = bsp_uart_init(proto->dev_num, proto);
if( bsp_status != BSP_OK) {
cprintf(con, str_bsp_init_err, bsp_status);
return t;
}
break;
case T_BRIDGE:
bridge(con);
break;
default:
return t - token_pos;
}
}
return t - token_pos;
}
