nes-proj/examples/6tisch/etsi-plugtest-2017/sf-plugtest.c

1010 lines
33 KiB
C
Raw Normal View History

/*
* Copyright (c) 2017, Toshiba Corporation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
* OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <contiki.h>
#include <shell.h>
#include <shell-commands.h>
#include <net/linkaddr.h>
#include <net/mac/tsch/tsch-schedule.h>
#include <net/mac/tsch/sixtop/sixtop.h>
#include <net/mac/tsch/sixtop/sixp.h>
#include <net/mac/tsch/sixtop/sixp-nbr.h>
#include <net/mac/tsch/sixtop/sixp-trans.h>
#include <net/mac/tsch/sixtop/sixp-pkt.h>
#include <sys/log.h>
#define LOG_MODULE "6top"
#define LOG_LEVEL LOG_LEVEL_6TOP
#include <string.h>
#include <stdlib.h>
#include <stdarg.h>
#include <lib/assert.h>
#define SIXP_PKT_BUFLEN 128
static uint8_t sixp_pkt_buf[SIXP_PKT_BUFLEN];
static shell_output_func *shell_output = NULL;
/* this variable is used for LIST Request */
static uint16_t cell_list_offset = 0; /* XXX: should be had in sixp_nbr */
typedef struct {
uint8_t slot_offset[2];
uint8_t channel_offset[2];
} sf_plugtest_cell_t;
typedef struct {
sixp_pkt_cmd_t cmd;
linkaddr_t peer_addr;
uint16_t slot_offset;
uint16_t channel_offset;
} subcmd_args_t;
typedef void subcmd(shell_output_func output, subcmd_args_t *args);
typedef void (req_handler_t)(const linkaddr_t *peer_addr,
const uint8_t *body, size_t body_len);
typedef void (res_handler_t)(const linkaddr_t *peer_addr, sixp_pkt_rc_t rc,
const uint8_t *body, size_t body_len);
static void advance_gen(const linkaddr_t *peer_addr);
static void add_res_sent_callback(void *arg, uint16_t arg_len,
const linkaddr_t *dest_addr,
sixp_output_status_t status);
static void delete_res_sent_callback(void *arg, uint16_t arg_len,
const linkaddr_t *dest_addr,
sixp_output_status_t status);
static void send_list_req(const linkaddr_t *peer_addr);
static int add_cell(const linkaddr_t *peer_addr, const sf_plugtest_cell_t *cell,
uint8_t link_options);
static int delete_cell(const linkaddr_t *peer_addr,
const sf_plugtest_cell_t *cell);
static int reserve_cell(const linkaddr_t *peer_addr,
const sf_plugtest_cell_t *cell);
static void clear_cells(const linkaddr_t *peer_addr,
struct tsch_slotframe *slotframe);
static void help(shell_output_func output, subcmd_args_t *args);
static void add_delete(shell_output_func output, subcmd_args_t *args);
static void count(shell_output_func output, subcmd_args_t *args);
static void list(shell_output_func output, subcmd_args_t *args);
static void clear(shell_output_func output, subcmd_args_t *args);
static int parse_args(shell_output_func output,
char *args, const char **subcmd,
subcmd_args_t *subcmd_args);
static const char subcmd_help[] = "help";
static void shell_subcmd(shell_output_func output, char *args);
static req_handler_t add_req_handler;
static res_handler_t add_res_handler;
static req_handler_t delete_req_handler;
static res_handler_t delete_res_handler;
static req_handler_t count_req_handler;
static res_handler_t count_res_handler;
static req_handler_t list_req_handler;
static res_handler_t list_res_handler;
static req_handler_t clear_req_handler;
static res_handler_t clear_res_handler;
static const struct {
char *name;
subcmd *func;
sixp_pkt_cmd_t cmd;
} subcmds[] = {
{ "help", help, SIXP_PKT_CMD_UNAVAILABLE },
{ "add", add_delete, SIXP_PKT_CMD_ADD },
{ "delete", add_delete, SIXP_PKT_CMD_DELETE },
{ "count", count, SIXP_PKT_CMD_COUNT },
{ "list", list, SIXP_PKT_CMD_LIST },
{ "clear", clear, SIXP_PKT_CMD_CLEAR },
{ NULL, NULL, SIXP_PKT_CMD_UNAVAILABLE }
};
static const struct {
sixp_pkt_cmd_t cmd;
req_handler_t *req;
res_handler_t *res;
} handlers[] = {
{ SIXP_PKT_CMD_ADD, add_req_handler, add_res_handler },
{ SIXP_PKT_CMD_DELETE, delete_req_handler, delete_res_handler },
{ SIXP_PKT_CMD_COUNT, count_req_handler, count_res_handler },
{ SIXP_PKT_CMD_LIST, list_req_handler, list_res_handler },
{ SIXP_PKT_CMD_CLEAR, clear_req_handler, clear_res_handler },
{ SIXP_PKT_CMD_UNAVAILABLE, NULL, NULL }
};
static void
advance_gen(const linkaddr_t *peer_addr)
{
sixp_nbr_t *nbr;
assert((nbr = sixp_nbr_find(peer_addr)) != NULL);
if(nbr == NULL) {
return;
}
assert(sixp_nbr_advance_gen(nbr) == 0);
}
static void
add_res_sent_callback(void *arg, uint16_t arg_len,
const linkaddr_t *dest_addr,
sixp_output_status_t status)
{
if(arg_len != sizeof(sf_plugtest_cell_t) ||
status == SIXP_OUTPUT_STATUS_FAILURE ||
dest_addr == NULL) {
LOG_ERR("error in sending a response\n");
} else {
add_cell(dest_addr, (sf_plugtest_cell_t *)arg, LINK_OPTION_RX);
}
advance_gen(dest_addr);
}
static void
delete_res_sent_callback(void *arg, uint16_t arg_len,
const linkaddr_t *dest_addr,
sixp_output_status_t status)
{
if(arg_len != sizeof(sf_plugtest_cell_t) ||
status == SIXP_OUTPUT_STATUS_FAILURE ||
dest_addr == NULL) {
LOG_ERR("error in sending a response\n");
} else {
delete_cell(dest_addr, (sf_plugtest_cell_t *)arg);
advance_gen(dest_addr);
}
}
static void
send_list_req(const linkaddr_t *peer_addr)
{
const sixp_pkt_max_num_cells_t SF_PLUGTEST_MAC_NUM_CELLS = 1;
memset(&sixp_pkt_buf, 0, sizeof(sixp_pkt_buf));
assert(sixp_pkt_set_cell_options(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_LIST,
SIXP_PKT_CELL_OPTION_TX,
sixp_pkt_buf, sizeof(sixp_pkt_buf)) == 0);
assert(sixp_pkt_set_offset(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_LIST,
(sixp_pkt_offset_t)cell_list_offset,
sixp_pkt_buf, sizeof(sixp_pkt_buf)) == 0);
assert(sixp_pkt_set_max_num_cells(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_LIST,
SF_PLUGTEST_MAC_NUM_CELLS,
sixp_pkt_buf, sizeof(sixp_pkt_buf)) == 0);
assert(sixp_output(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_LIST,
SF_PLUGTEST_SFID, sixp_pkt_buf,
sizeof(sixp_pkt_metadata_t) +
sizeof(sixp_pkt_cell_options_t) +
sizeof(sixp_pkt_reserved_t) +
sizeof(sixp_pkt_offset_t) +
sizeof(sixp_pkt_max_num_cells_t),
peer_addr,
NULL, NULL, 0) == 0);
}
static int
add_cell(const linkaddr_t *peer_addr, const sf_plugtest_cell_t *cell,
uint8_t link_options)
{
struct tsch_slotframe *slotframe;
uint16_t timeslot;
uint16_t channel_offset;
assert(peer_addr != NULL && cell != NULL);
if(peer_addr == NULL || cell == NULL) {
return -1;
}
timeslot = cell->slot_offset[0] + (cell->slot_offset[1] << 8);
channel_offset = cell->channel_offset[0] + (cell->channel_offset[1] << 8);
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL ||
tsch_schedule_add_link(slotframe, link_options, LINK_TYPE_NORMAL,
peer_addr, timeslot, channel_offset) == NULL) {
LOG_ERR("cannot add a cell\n");
return -1;
}
LOG_INFO("Succeeded to add a cell [slot:%u]\n", timeslot);
return 0;
}
static int
delete_cell(const linkaddr_t *peer_addr, const sf_plugtest_cell_t *cell)
{
struct tsch_slotframe *slotframe;
uint16_t timeslot;
assert(peer_addr != NULL && cell != NULL);
if(peer_addr == NULL || cell == NULL) {
return -1;
}
timeslot = cell->slot_offset[0] + (cell->slot_offset[1] << 8);
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL ||
tsch_schedule_remove_link_by_timeslot(slotframe, timeslot) == 0) {
LOG_ERR("cannot delete a cell\n");
return -1;
}
LOG_INFO("Succeeded to delete a cell [slot:%u]\n", timeslot);
return 0;
}
static int
reserve_cell(const linkaddr_t *peer_addr, const sf_plugtest_cell_t *cell)
{
if(add_cell(peer_addr, cell, 0) < 0 ||
delete_cell(peer_addr, cell) < 0) {
/* fail to reserve the cell */
return -1;
}
return 0;
}
static void
clear_cells(const linkaddr_t *peer_addr, struct tsch_slotframe *slotframe)
{
struct tsch_link *cell, *next_cell;
assert(peer_addr != NULL);
if(peer_addr == NULL) {
return;
}
for(cell = (struct tsch_link *)list_head(slotframe->links_list);
cell != NULL; cell = next_cell) {
next_cell = (struct tsch_link *)list_item_next(cell);
if(memcmp(&cell->addr, peer_addr, sizeof(linkaddr_t)) == 0) {
assert(tsch_schedule_remove_link(slotframe, cell) == 1);
}
}
}
static void
add_req_handler(const linkaddr_t *peer_addr,
const uint8_t *body, size_t body_len)
{
sixp_pkt_cell_options_t cell_options;
const uint8_t *cell;
sixp_pkt_offset_t cell_list_len;
static sf_plugtest_cell_t pending_cell;
uint16_t timeslot;
struct tsch_slotframe *slotframe;
assert(peer_addr != NULL && body != NULL);
if(body_len != (sizeof(sixp_pkt_metadata_t) +
sizeof(sixp_pkt_cell_options_t) +
sizeof(sixp_pkt_num_cells_t) +
sizeof(sf_plugtest_cell_t))) {
LOG_ERR("invalid Add Request length: %u\n", (unsigned int)body_len);
}
assert(
sixp_pkt_get_cell_options(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_ADD,
&cell_options,
body, body_len) == 0);
if(cell_options != SIXP_PKT_CELL_OPTION_TX &&
cell_options != (SIXP_PKT_CELL_OPTION_TX | SIXP_PKT_CELL_OPTION_SHARED)) {
LOG_ERR("invalid Cell Options: %u\n", cell_options);
}
assert(
sixp_pkt_get_cell_list(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_ADD,
&cell, &cell_list_len, body, body_len) == 0);
assert(cell_list_len == sizeof(sf_plugtest_cell_t));
memcpy(&pending_cell, cell, sizeof(pending_cell));
timeslot = pending_cell.slot_offset[0] + (pending_cell.slot_offset[1] << 8);
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL ||
tsch_schedule_get_link_by_timeslot(slotframe, timeslot) != NULL ||
reserve_cell(peer_addr, &pending_cell) < 0) {
LOG_ERR("Failed to add a cell [slot:%u]\n", timeslot);
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_NORES,
SF_PLUGTEST_SFID, NULL, 0, peer_addr,
NULL, NULL, 0);
} else {
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS,
SF_PLUGTEST_SFID, cell, cell_list_len, peer_addr,
add_res_sent_callback, &pending_cell, sizeof(pending_cell));
}
}
static void
add_res_handler(const linkaddr_t *peer_addr, sixp_pkt_rc_t rc,
const uint8_t *body, size_t body_len)
{
const uint8_t *cell;
sixp_pkt_offset_t cell_list_len;
uint16_t timeslot;
struct tsch_slotframe *slotframe;
if(body_len != 4) {
LOG_ERR("invalid Add Response length: %u\n", (unsigned int)body_len);
return;
}
assert(sixp_pkt_get_cell_list(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS,
&cell, &cell_list_len, body, body_len) == 0);
timeslot = cell[0] + (cell[1] << 8);
if(rc != SIXP_PKT_RC_SUCCESS) {
LOG_ERR("received return code of %u\n", rc);
return;
}
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL ||
tsch_schedule_get_link_by_timeslot(slotframe, timeslot) != NULL ||
add_cell(peer_addr, (sf_plugtest_cell_t *)cell, LINK_OPTION_TX) < 0) {
LOG_ERR("Failed to add a cell [slot:%u]\n", timeslot);
}
advance_gen(peer_addr);
}
static void
delete_req_handler(const linkaddr_t *peer_addr,
const uint8_t *body, size_t body_len)
{
sixp_pkt_cell_options_t cell_options;
const uint8_t *cell;
sixp_pkt_offset_t cell_list_len;
static sf_plugtest_cell_t pending_cell;
uint16_t timeslot;
struct tsch_slotframe *slotframe;
struct tsch_link *link;
assert(peer_addr != NULL && body != NULL);
if(body_len != (sizeof(sixp_pkt_metadata_t) +
sizeof(sixp_pkt_cell_options_t) +
sizeof(sixp_pkt_num_cells_t) +
sizeof(sf_plugtest_cell_t))) {
LOG_ERR("invalid Delete Request length: %u\n", (unsigned int)body_len);
}
assert(
sixp_pkt_get_cell_options(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_DELETE,
&cell_options,
body, body_len) == 0);
if(cell_options != SIXP_PKT_CELL_OPTION_TX &&
cell_options != (SIXP_PKT_CELL_OPTION_TX | SIXP_PKT_CELL_OPTION_SHARED)) {
LOG_ERR("invalid Cell Options: %u\n", cell_options);
}
assert(
sixp_pkt_get_cell_list(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_DELETE,
&cell, &cell_list_len, body, body_len) == 0);
assert(cell_list_len == sizeof(sf_plugtest_cell_t));
memcpy(&pending_cell, cell, sizeof(pending_cell));
timeslot = pending_cell.slot_offset[0] + (pending_cell.slot_offset[1] << 8);
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL ||
(link = tsch_schedule_get_link_by_timeslot(slotframe, timeslot)) == NULL ||
memcmp(peer_addr, &link->addr, sizeof(linkaddr_t)) != 0) {
LOG_ERR("Failed to delete a cell [slot:%u]\n", timeslot);
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_NORES,
SF_PLUGTEST_SFID, NULL, 0, peer_addr,
NULL, NULL, 0);
} else {
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS,
SF_PLUGTEST_SFID, cell, cell_list_len, peer_addr,
delete_res_sent_callback, &pending_cell, sizeof(pending_cell));
}
}
static void
delete_res_handler(const linkaddr_t *peer_addr, sixp_pkt_rc_t rc,
const uint8_t *body, size_t body_len)
{
struct tsch_slotframe *slotframe;
struct tsch_link *link;
const sf_plugtest_cell_t *cell;
sixp_pkt_offset_t cell_list_len;
sixp_nbr_t *nbr;
uint16_t timeslot;
if(body_len != 4) {
LOG_ERR("invalid Delete Response length: %u\n", (unsigned int)body_len);
return;
}
assert(
sixp_pkt_get_cell_list(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS,
(const uint8_t **)&cell, &cell_list_len,
body, body_len) == 0);
timeslot = cell->slot_offset[0] + (cell->slot_offset[1] << 8);
if((nbr = sixp_nbr_find(peer_addr)) == NULL) {
LOG_ERR("unexpected error; cannot find nbr\n");
return;
}
if(rc != SIXP_PKT_RC_SUCCESS) {
LOG_ERR("received return code of %u\n", rc);
return;
}
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL ||
(link = tsch_schedule_get_link_by_timeslot(slotframe, timeslot)) == NULL ||
memcmp(peer_addr, &link->addr, sizeof(linkaddr_t)) != 0 ||
delete_cell(peer_addr, cell) < 0) {
LOG_ERR("Failed to delete a cell [slot:%u]\n", timeslot);
}
advance_gen(peer_addr);
}
static void
count_req_handler(const linkaddr_t *peer_addr,
const uint8_t *body, size_t body_len)
{
sixp_pkt_cell_options_t cell_options;
struct tsch_slotframe *slotframe;
struct tsch_link *cell;
sixp_pkt_total_num_cells_t total_num_cells = 0;
uint8_t buf[2];
assert(peer_addr != NULL && body != NULL);
if(body_len != (sizeof(sixp_pkt_metadata_t) +
sizeof(sixp_pkt_cell_options_t))) {
LOG_ERR("invalid Count Request length: %u\n", (unsigned int)body_len);
}
assert(
sixp_pkt_get_cell_options(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_COUNT,
&cell_options,
body, body_len) == 0);
if(cell_options != SIXP_PKT_CELL_OPTION_TX &&
cell_options != (SIXP_PKT_CELL_OPTION_TX | SIXP_PKT_CELL_OPTION_SHARED)) {
LOG_ERR("invalid Cell Options: %u\n", cell_options);
}
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL) {
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_ERROR,
SF_PLUGTEST_SFID, NULL, 0, peer_addr,
NULL, NULL, 0);
return;
}
for(cell = (struct tsch_link *)list_head(slotframe->links_list);
cell != NULL; cell = (struct tsch_link *)list_item_next(cell)) {
if(memcmp(&cell->addr, peer_addr, sizeof(linkaddr_t)) == 0 &&
cell->link_options == LINK_OPTION_RX) {
total_num_cells++;
}
}
/* make sure total_num_cells are set in little-endian */
buf[0] = (total_num_cells & 0xff);
buf[1] = (total_num_cells >> 8);
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS, SF_PLUGTEST_SFID,
(const uint8_t *)buf, sizeof(buf),
peer_addr, NULL, NULL, 0);
}
static void
count_res_handler(const linkaddr_t *peer_addr, sixp_pkt_rc_t rc,
const uint8_t *body, size_t body_len)
{
sixp_pkt_total_num_cells_t total_num_cells;
if(body_len != 2) {
LOG_ERR("invalid Count Response length: %u\n", (unsigned int)body_len);
return;
}
if(rc != SIXP_PKT_RC_SUCCESS) {
LOG_ERR("received return code of %u\n", rc);
return;
}
assert(
sixp_pkt_get_total_num_cells(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS,
&total_num_cells, body, body_len) == 0);
LOG_INFO("Succeeded to get COUNT: %u\n", total_num_cells);
}
static void
list_req_handler(const linkaddr_t *peer_addr,
const uint8_t *body, size_t body_len)
{
sixp_pkt_cell_options_t cell_options;
sixp_pkt_offset_t cell_list_offset;
sixp_pkt_max_num_cells_t max_num_cells;
struct tsch_slotframe *slotframe;
struct tsch_link *link;
sixp_pkt_offset_t cell_nums;
sf_plugtest_cell_t cell;
assert(peer_addr != NULL && body != NULL);
if(body_len != (sizeof(sixp_pkt_metadata_t) +
sizeof(sixp_pkt_cell_options_t) +
sizeof(sixp_pkt_reserved_t) +
sizeof(sixp_pkt_offset_t) +
sizeof(sixp_pkt_max_num_cells_t))) {
LOG_ERR("invalid List Request length: %u\n", (unsigned int)body_len);
}
assert(
sixp_pkt_get_cell_options(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_LIST,
&cell_options,
body, body_len) == 0);
assert(
sixp_pkt_get_offset(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_LIST,
&cell_list_offset,
body, body_len) == 0);
assert(
sixp_pkt_get_max_num_cells(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_LIST,
&max_num_cells,
body, body_len) == 0);
if(cell_options != SIXP_PKT_CELL_OPTION_TX &&
cell_options != (SIXP_PKT_CELL_OPTION_TX | SIXP_PKT_CELL_OPTION_SHARED)) {
LOG_ERR("invalid Cell Options: %u\n", cell_options);
return;
}
if(max_num_cells < 1) {
LOG_ERR("invalid MaxNumCells: %u\n", max_num_cells);
return;
}
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL) {
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_ERROR,
SF_PLUGTEST_SFID, NULL, 0, peer_addr,
NULL, NULL, 0);
return;
}
cell_nums = 0;
for(link = (struct tsch_link *)list_head(slotframe->links_list);
link != NULL; link = (struct tsch_link *)list_item_next(link)) {
if(memcmp(&link->addr, peer_addr, sizeof(linkaddr_t)) == 0 &&
link->link_options == LINK_OPTION_RX) {
if(cell_list_offset == cell_nums) {
cell.slot_offset[0] = link->timeslot & 0xff;
cell.slot_offset[1] = link->timeslot >> 8;
cell.channel_offset[0] = link->channel_offset & 0xff;
cell.channel_offset[1] = link->channel_offset >> 8;
}
cell_nums++;
}
}
if(cell_nums == 0) {
assert(sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_EOL,
SF_PLUGTEST_SFID, NULL, 0,
peer_addr, NULL, NULL, 0) == 0);
} else {
assert(sixp_output(SIXP_PKT_TYPE_RESPONSE,
(cell_list_offset + 1) == cell_nums ?
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_EOL :
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS,
SF_PLUGTEST_SFID,
(const uint8_t *)&cell, sizeof(cell),
peer_addr, NULL, NULL, 0) == 0);
}
}
static void
list_res_handler(const linkaddr_t *peer_addr, sixp_pkt_rc_t rc,
const uint8_t *body, size_t body_len)
{
sf_plugtest_cell_t *cell;
uint16_t slot_offset, channel_offset;
sixp_pkt_offset_t cell_list_len;
sixp_pkt_offset_t i;
assert(sixp_pkt_get_cell_list(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)rc,
(const uint8_t **)&cell, &cell_list_len,
body, body_len) == 0);
LOG_INFO("Succeeded to get LIST: ");
for(i = 0; i < cell_list_len; i += sizeof(sf_plugtest_cell_t)) {
slot_offset = cell->slot_offset[0] + (cell->slot_offset[1] << 8);
channel_offset = cell->channel_offset[0] + (cell->channel_offset[1] << 8);
LOG_INFO_("[slot:%u, channel:%u]", slot_offset, channel_offset);
}
if(rc == SIXP_PKT_RC_SUCCESS) {
LOG_INFO_(" continued\n");
cell_list_offset += (i / sizeof(sf_plugtest_cell_t));
} else {
/* EOL */
LOG_INFO_(" EOL\n");
cell_list_offset = 0;
}
}
static void
clear_req_handler(const linkaddr_t *peer_addr,
const uint8_t *body, size_t body_len)
{
struct tsch_slotframe *slotframe;
sixp_nbr_t *nbr;
assert(peer_addr != NULL && body != NULL);
if(body_len != sizeof(sixp_pkt_metadata_t)) {
LOG_ERR("invalid Clear Request length: %u\n", (unsigned int)body_len);
}
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) == NULL) {
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_ERROR,
SF_PLUGTEST_SFID, NULL, 0, peer_addr,
NULL, NULL, 0);
return;
}
clear_cells(peer_addr, slotframe);
sixp_output(SIXP_PKT_TYPE_RESPONSE,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_RC_SUCCESS,
SF_PLUGTEST_SFID, NULL, 0, peer_addr,
NULL, NULL, 0);
if((nbr = sixp_nbr_find(peer_addr)) != NULL) {
/* clear GEN */
sixp_nbr_free(nbr);
}
}
static void
clear_res_handler(const linkaddr_t *peer_addr, sixp_pkt_rc_t rc,
const uint8_t *body, size_t body_len)
{
struct tsch_slotframe *slotframe;
sixp_nbr_t *nbr;
assert(peer_addr != NULL && body != NULL);
if(peer_addr == NULL) {
return;
}
if(body_len != 0) {
LOG_ERR("invalid Clear Response length: %u\n", (unsigned int)body_len);
return;
}
if((slotframe = tsch_schedule_get_slotframe_by_handle(0)) != NULL) {
clear_cells(peer_addr, slotframe);
}
if((nbr = sixp_nbr_find(peer_addr)) != NULL) {
/* clear GEN */
sixp_nbr_free(nbr);
}
LOG_INFO("Succeeded to CLEAR\n");
}
static void
help(shell_output_func output, subcmd_args_t *args)
{
/* help doesn't use peer_addr */
SHELL_OUTPUT(output, "cmd [slot_offset] [channel_offset] [peer_addr]\n");
SHELL_OUTPUT(output, "available commands\n");
SHELL_OUTPUT(output, "add - add a TX cell\n");
SHELL_OUTPUT(output, "delete - delete a TX cell\n");
SHELL_OUTPUT(output, "count - get cell count\n");
SHELL_OUTPUT(output, "list - get a cell list\n");
SHELL_OUTPUT(output, "clear - clear TX cells\n");
SHELL_OUTPUT(output, "help - show this usage\n");
SHELL_OUTPUT(output, "example> 6p add 4 5 01:01:01:01:01:01:01:01\n");
}
static void
add_delete(shell_output_func output, subcmd_args_t *args)
{
assert(args != NULL);
sf_plugtest_cell_t cell;
/* set cell attributes in little-endian */
cell.slot_offset[0] = args->slot_offset & 0xff;
cell.slot_offset[1] = args->slot_offset >> 8;
cell.channel_offset[0] = args->channel_offset & 0xff;
cell.channel_offset[1] = args->channel_offset >> 8;
if(args->cmd == SIXP_PKT_CMD_ADD &&
reserve_cell(&(args->peer_addr), &cell) < 0) {
SHELL_OUTPUT(output, "invalid arguments for Add Request\n");
return;
}
memset(sixp_pkt_buf, 0, sizeof(sixp_pkt_buf));
assert(sixp_pkt_set_cell_options(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)args->cmd,
SIXP_PKT_CELL_OPTION_TX,
sixp_pkt_buf, sizeof(sixp_pkt_buf)) == 0);
assert(sixp_pkt_set_num_cells(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)args->cmd,
1,
sixp_pkt_buf, sizeof(sixp_pkt_buf)) == 0);
assert(sixp_pkt_set_cell_list(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)args->cmd,
(const uint8_t *)&cell,
sizeof(cell), 0,
sixp_pkt_buf, sizeof(sixp_pkt_buf)) == 0);
assert(sixp_output(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)args->cmd,
SF_PLUGTEST_SFID, sixp_pkt_buf,
sizeof(sixp_pkt_metadata_t) +
sizeof(sixp_pkt_cell_options_t) +
sizeof(sixp_pkt_num_cells_t) +
sizeof(cell),
&(args->peer_addr),
NULL, NULL, 0) == 0);
SHELL_OUTPUT(output, "sent %s request [slot:%u, channel:%u]\n",
args->cmd == SIXP_PKT_CMD_ADD ? "an Add" : "a Delete",
args->slot_offset, args->channel_offset);
}
static void
count(shell_output_func output, subcmd_args_t *args)
{
memset(&sixp_pkt_buf, 0, sizeof(sixp_pkt_buf));
assert(sixp_pkt_set_cell_options(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)args->cmd,
SIXP_PKT_CELL_OPTION_TX,
sixp_pkt_buf, sizeof(sixp_pkt_buf)) == 0);
assert(sixp_output(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)args->cmd,
SF_PLUGTEST_SFID, sixp_pkt_buf,
sizeof(sixp_pkt_metadata_t) +
sizeof(sixp_pkt_cell_options_t),
&(args->peer_addr),
NULL, NULL, 0) == 0);
SHELL_OUTPUT(output, "sent a Count request\n");
}
static void
list(shell_output_func output, subcmd_args_t *args)
{
send_list_req(&(args->peer_addr));
SHELL_OUTPUT(output, "sent a List request\n");
}
static void
clear(shell_output_func output, subcmd_args_t *args)
{
cell_list_offset = 0;
memset(sixp_pkt_buf, 0, sizeof(sixp_pkt_buf));
assert(sixp_output(SIXP_PKT_TYPE_REQUEST,
(sixp_pkt_code_t)(uint8_t)SIXP_PKT_CMD_CLEAR,
SF_PLUGTEST_SFID, sixp_pkt_buf,
sizeof(sixp_pkt_metadata_t),
&(args->peer_addr),
NULL, NULL, 0) == 0);
SHELL_OUTPUT(output, "sent a Clear request\n");
}
static int
parse_args(shell_output_func output,
char *args, const char **subcmd, subcmd_args_t *subcmd_args)
{
struct tsch_neighbor *time_source = NULL;
char *next_args;
#if CONTIKI_TARGET_COOJA
char *saveptr;
char *octet;
int i;
#endif /* CONTIKI_TARGET_COOJA */
SHELL_ARGS_INIT(args, next_args);
SHELL_ARGS_NEXT(args, next_args);
if(args == NULL) {
*subcmd = subcmd_help;
} else {
*subcmd = args;
}
if(strncmp("help", *subcmd, sizeof("help")) == 0) {
/* we don't need peer_addr for the help sub-command. */
return 0;
}
/* add and delete need slot_offset and channel_offset */
if(strncmp("add", *subcmd, sizeof("add")) == 0 ||
strncmp("delete", *subcmd, sizeof("delete")) == 0) {
SHELL_ARGS_NEXT(args, next_args);
if(args != NULL) {
subcmd_args->slot_offset = strtol(args, NULL, 10);
} else {
return -1;
}
SHELL_ARGS_NEXT(args, next_args);
if(args != NULL) {
subcmd_args->channel_offset = strtol(args, NULL, 10);
} else {
return -1;
}
}
SHELL_ARGS_NEXT(args, next_args);
if(args != NULL) {
#if CONTIKI_TARGET_COOJA
for(octet = strtok_r(args, ":", &saveptr), i = 0;
octet != NULL && i < LINKADDR_SIZE;
octet = strtok_r(NULL, ":", &saveptr), i++) {
subcmd_args->peer_addr.u8[i] = strtol(octet, NULL, 16);
}
if(i > 1 && i != LINKADDR_SIZE) {
/* invalid MAC address */
memset(&(subcmd_args->peer_addr), 0, sizeof(linkaddr_t));
return -1;
}
#else
SHELL_OUTPUT(output, "MAC address cannot be specified on this platform\n");
return -1;
#endif /* CONTIKI_TARGET_COOJA */
} else {
if((time_source = tsch_queue_get_time_source()) == NULL) {
SHELL_OUTPUT(output, "time source is not available\n");
return -1;
} else {
memcpy(&(subcmd_args->peer_addr), &(time_source->addr),
sizeof(linkaddr_t));
}
}
return 0;
}
static void
shell_subcmd(shell_output_func output, char *args)
{
const char *subcmd;
subcmd_args_t subcmd_args;
int i;
if(shell_output == NULL) {
shell_output = output;
}
if(parse_args(output, args, &subcmd, &subcmd_args) < 0) {
SHELL_OUTPUT(output,
"invalid argument; command argument parse error\n");
return;
}
for(i = 0; subcmds[i].name != NULL; i++) {
if(strcmp(subcmds[i].name, subcmd) == 0) {
subcmd_args.cmd = subcmds[i].cmd;
subcmds[i].func(output, &subcmd_args);
}
}
}
static void
init(void)
{
shell_commands_set_6top_sub_cmd(shell_subcmd);
}
static void
input(sixp_pkt_type_t type, sixp_pkt_code_t code,
const uint8_t *body, uint16_t body_len,
const linkaddr_t *src_addr)
{
sixp_trans_t *trans;
sixp_pkt_cmd_t cmd;
int i;
switch(type) {
case SIXP_PKT_TYPE_REQUEST:
cmd = code.cmd;
break;
case SIXP_PKT_TYPE_RESPONSE:
if((trans = sixp_trans_find(src_addr)) == NULL) {
LOG_ERR("internal error; cannot find a trans\n");
return;
}
cmd = sixp_trans_get_cmd(trans);
break;
default:
LOG_ERR("unsupported type %u by sf-plugtest\n", type);
return;
}
if(type == SIXP_PKT_TYPE_RESPONSE &&
code.rc != SIXP_PKT_RC_SUCCESS &&
cmd != SIXP_PKT_CMD_LIST &&
code.rc != SIXP_PKT_RC_EOL) {
LOG_ERR("received return code of %u\n", code.value);
return;
}
for(i = 0; i < sizeof(handlers); i++) {
if(handlers[i].cmd == cmd) {
if(type == SIXP_PKT_TYPE_REQUEST && handlers[i].req != NULL) {
handlers[i].req(src_addr, body, body_len);
return;
}
if(type == SIXP_PKT_TYPE_RESPONSE && handlers[i].res != NULL) {
handlers[i].res(src_addr, code.rc, body, body_len);
return;
}
i = sizeof(handlers);
break;
}
}
if(i == sizeof(handlers)) {
LOG_ERR("unsupported command %u by sf-plugtest\n",
code.cmd);
}
}
static void
timeout(sixp_pkt_cmd_t cmd, const linkaddr_t *peer_addr)
{
LOG_ERR("transaction timeout\n");
}
const sixtop_sf_t sf_plugtest = {
SF_PLUGTEST_SFID,
SF_PLUGTEST_TIMEOUT,
init,
input,
timeout
};