diff --git a/Makefile b/Makefile index 828de0c..8d84926 100644 --- a/Makefile +++ b/Makefile @@ -4,7 +4,7 @@ CFLAGS=-Wall -g -DPACKAGE_VERSION=\"0.0\" %.o: %.c $(CC) $(CFLAGS) -o $@ -c $^ -I/usr/local/include/ -v5le: main.o logging.o v5x_data.o lapv5.o v5x_protocol.o +v5le: main.o logging.o v5x_data.o lapv5.o v5x_protocol.o v51_le_ctrl.o $(CC) -o $@ $^ $(LIBS) clean: diff --git a/v51_le_ctrl.c b/v51_le_ctrl.c index 1a1a84b..c1bddb8 100644 --- a/v51_le_ctrl.c +++ b/v51_le_ctrl.c @@ -20,197 +20,350 @@ * */ -#include +#include #include #include +#include +#include -#include +#include #include #include #include "v5x_internal.h" +#include "v5x_protocol.h" +#include "v51_le_ctrl.h" +#include "logging.h" + +#define S(x) (1 << (x)) + +#define TIMEOUT 1 + +/* uncomment to test lost TX messages at the first transmission */ +#warning fixme +//#define TEST_TX_FAILURE /* PSTN Protocol FSM */ -enum v51_le_ctrl_port_fsm_state { +enum v51_le_ctrl_fsm_state { V51_PSTN_PROT_S_OUT_OF_SERVICE, /* LE0 */ - V51_PSTN_PROT_S_NULL /* LE1 */ - V51_PSTN_PROT_S_PATH_INIT_BY_LE /* LE2 */ - V51_PSTN_PROT_S_PATH_INIT_BY_AN /* LE3 */ - V51_PSTN_PROT_S_PATH_ACTIVE /* LE4 */ - V51_PSTN_PROT_S_PATH_DISC_REQ /* LE5 */ - V51_PSTN_PROT_S_PORT_BLOCKED /* LE6 */ + V51_PSTN_PROT_S_NULL, /* LE1 */ + V51_PSTN_PROT_S_PATH_INIT_BY_LE, /* LE2 */ + V51_PSTN_PROT_S_PATH_INIT_BY_AN, /* LE3 */ + V51_PSTN_PROT_S_PATH_ACTIVE, /* LE4 */ + V51_PSTN_PROT_S_PATH_DISC_REQ, /* LE5 */ + V51_PSTN_PROT_S_PORT_BLOCKED, /* LE6 */ }; /*********************************************************************** - * user port CTRL protocol FSM / G.964 Section 14 + * port/common CTRL protocol FSM / G.964 Section 14 ***********************************************************************/ -enum v51_ctrl_port_fsm_state { - V51_CTRL_PORT_ST_OUT_OF_SERVICE, - V51_CTRL_PORT_ST_IN_SERVICE, - V51_CTRL_PORT_AWAIT_PORT_ACK, +enum v51_ctrl_fsm_state { + V51_CTRL_ST_OUT_OF_SERVICE = 0, + V51_CTRL_ST_IN_SERVICE, + V51_CTRL_ST_AWAIT_ACK, }; -enum v51_ctrl_port_fsm_event { - V51_CTRL_PE_MDU_START_TRAFFIC, - V51_CTRL_PE_MDU_STOP_TRAFFIC, - V51_CTRL_PE_MDU_CTRL, - V51_CTRL_PE_RX_PORT_CONTROL, - V51_CTRL_PE_RX_PORT_CONTROL_ACK, +enum v51_ctrl_fsm_event { + V51_CTRL_EV_MDU_START_TRAFFIC, + V51_CTRL_EV_MDU_STOP_TRAFFIC, + V51_CTRL_EV_MDU_CTRL, + V51_CTRL_EV_RX_CONTROL, + V51_CTRL_EV_RX_CONTROL_ACK, }; -static const struct value_string v51_ctrl_port_fsm_event_names[] = { - { V51_CTRL_PE_MDU_START_TRAFFIC, "MDU-start_traffic" }, - { V51_CTRL_PE_MDU_STOP_TRAFFIC, "MDU-stop_traffic" }, - { V51_CTRL_PE_MDU_CTRL, "MDU-CTRL" }, - { V51_CTRL_PE_RX_PORT_CONTROL, "PORT_CONTROL" }, - { V51_CTRL_PE_RX_PORT_CONTROL_ACK, "PORT_CONTROL_ACK" }, +static const struct value_string v51_ctrl_fsm_event_names[] = { + { V51_CTRL_EV_MDU_START_TRAFFIC, "MDU-start_traffic" }, + { V51_CTRL_EV_MDU_STOP_TRAFFIC, "MDU-stop_traffic" }, + { V51_CTRL_EV_MDU_CTRL, "MDU-CTRL" }, + { V51_CTRL_EV_RX_CONTROL, "CONTROL" }, + { V51_CTRL_EV_RX_CONTROL_ACK, "CONTROL_ACK" }, { 0, NULL } }; -static int v51_ctrl_port_fsm_timer_cb(struct osmo_fsm_inst *fi) +static int blocked = 1; +static void v51_mdu_ctrl(struct osmo_fsm_inst *fi, const struct tlv_parsed *tp) { - if () { - /* first expiry: repeat PORT_CONTROL; re-start T01 */ - } else { - /* second expiry: send MDU-error_ind; go to IN_SERVICE */ - osmo_fsm_inst_state_chg(fi, V51_CTRL_PORT_ST_IN_SERVICE); + struct v5x_ctrl_proto *ctrl = fi->priv; + + LOGP(DV5CTRL, LOGL_NOTICE, "MDU-CTRL\n"); + + if (ctrl->v5if) { + puts("common control"); + enum v51_ctrl_func_id cfi = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ID) & 0x7f; + printf("cfi=%d\n", cfi); + if (cfi == V51_CTRL_ID_REQUEST_VARIANT_AND_INTERFACE_ID) { + puts("request variant+id"); + v51_snd_ctrl_common(ctrl->v5if, V51_CTRL_ID_VARIANT_AND_INTERFACE_ID, NULL, &ctrl->v5if->variant, &ctrl->v5if->id); + } + if (cfi == V51_CTRL_ID_RESTART_REQUEST) { + puts("request restart"); + v51_snd_ctrl_common(ctrl->v5if, V51_CTRL_ID_RESTART_COMPLETE, NULL, NULL, NULL); + } + } + if (ctrl->v5up) { + puts("port control"); + enum v51_ctrl_func_el cfe = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ELEMENT) & 0x7f; + printf("cfe=%d\n", cfe); + if (cfe == V51_CTRL_FE201_UNBLOCK) { + puts("request unblock"); + if (blocked) { + blocked = 0; + v51_snd_ctrl_port(ctrl->v5up, V51_CTRL_FE201_UNBLOCK); + } + } + if (cfe == V51_CTRL_FE203_BLOCK) { + puts("request block"); + if (!blocked) { + blocked = 1; + v51_snd_ctrl_port(ctrl->v5up, V51_CTRL_FE203_BLOCK); + } + } } } -static const struct osmo_fsm_state v51_ctrl_port_fsm_states[] = { - [V51_CTRL_PORT_ST_OUT_OF_SERVICE] = { - .name = "OUT_OF_SERVICE", - .in_event_mask = S(V51_CTRL_PE_MDU_START_TRAFFIC) | - S(V51_CTRL_PE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_PE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_PE_RX_PORT_CONTROL) | - S(V51_CTRL_PE_RX_PORT_CONTROL_ACK), - .out_state_mask = S(V51_CTRL_PORT_ST_IN_SERVICE), - }, - [V51_CTRL_PORT_ST_IN_SERVICE] = { - .name = "IN_SERVICE", - .in_event_mask = S(V51_CTRL_PE_MDU_START_TRAFFIC) | - S(V51_CTRL_PE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_PE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_PE_RX_PORT_CONTROL), - - .out_state_mask = S(V51_CTRL_PORT_ST_OUT_OF_SERVICE) | - S(V51_CTRL_PORT_AWAIT_PORT_ACK), - }, - [V51_CTRL_PORT_AWAIT_PORT_ACK] = { - .name = "AWAIT_PORT_ACK", - .in_event_mask = S(V51_CTRL_PE_MDU_START_TRAFFIC) | - S(V51_CTRL_PE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_PE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_PE_RX_PORT_CONTROL) | - S(V51_CTRL_PE_RX_PORT_CONTROL_ACK), - .out_state_mask = S(V51_CTRL_PORT_ST_OUT_OF_SERVICE) | - S(V51_CTRL_PORT_ST_IN_SERVICE), - }, -}; - -static struct osmo_fsm v51_ctrl_port_fsm = { - .name = "V51_CTRL_PORT", - .states = v51_ctrl_port_fsm_states, - .num_states = ARRAY_SIZE(v51_ctrl_port_fsm_states), - .allstate_event_mask = 0, - .allstate_action = NULL, - .cleanup = NULL, - .timer_cb = v51_ctrl_port_fsm_timer_cb, - .log_subsys = DV5CTRL, - .event_names = v51_ctrl_port_fsm_event_names, -}; - -/*********************************************************************** - * common CTRL protocol FSM / G.964 Section 14 - ***********************************************************************/ - -enum v51_ctrl_common_fsm_state { - V51_CTRL_COM_ST_OUT_OF_SERVICE, - V51_CTRL_COM_ST_IN_SERVICE, - V51_CTRL_COM_AWAIT_COMMON_ACK, -}; - -enum v51_ctrl_common_fsm_event { - V51_CTRL_CE_MDU_START_TRAFFIC, - V51_CTRL_CE_MDU_STOP_TRAFFIC, - V51_CTRL_CE_MDU_CTRL, - V51_CTRL_CE_RX_COMMON_CONTROL, - V51_CTRL_CE_RX_COMMON_CONTROL_ACK, -}; - -static const struct value_string v51_ctrl_common_fsm_event_names[] = { - { V51_CTRL_CE_MDU_START_TRAFFIC, "MDU-start_traffic" }, - { V51_CTRL_CE_MDU_STOP_TRAFFIC, "MDU-stop_traffic" }, - { V51_CTRL_CE_MDU_CTRL, "MDU-CTRL" }, - { V51_CTRL_CE_RX_COMMON_CONTROL, "COMMON_CONTROL" }, - { V51_CTRL_CE_RX_COMMON_CONTROL_ACK, "COMMON_CONTROL_ACK" }, - { 0, NULL } -}; - -static int v51_ctrl_common_fsm_timer_cb(struct osmo_fsm_inst *fi) +/* display MDU-error_indication */ +static void v51_mdu_error(struct osmo_fsm_inst *fi, const char *error) { - if () { - /* first expiry: repeat COMMON_CONTROL; re-start T02 */ - } else { - /* second expiry: send MDU-error_ind; go to IN_SERVICE */ - osmo_fsm_inst_state_chg(fi, V51_CTRL_COM_ST_IN_SERVICE); + LOGP(DV5CTRL, LOGL_NOTICE, "MDU-error_indication: %s\n", error); +} + +/* send control towards lower (DL) layer */ +static void v51_ctrl_send(struct v5x_ctrl_proto *ctrl, struct msgb *msg) +{ + struct osmo_fsm_inst *fi = ctrl->fi; + + switch (fi->state) { + case V51_CTRL_ST_IN_SERVICE: + LOGP(DV5CTRL, LOGL_DEBUG, "We are in service, so we send our message now.\n"); + /* no message, clone current message and store to be repeated */ + ctrl->tx_msg = msgb_copy(msg, NULL); + /* go to AWAIT_PENDING_ACK */ + osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_AWAIT_ACK, TIMEOUT, (ctrl->v5up) ? 1 : 2); + /* send message towards DL */ +#ifdef TEST_TX_FAILURE + msgb_free(msg); +#else + v5x_snd((ctrl->v5if) ? : ctrl->v5up->inst, V51_DLADDR_CTRL, msg); +#endif + break; + case V51_CTRL_ST_AWAIT_ACK: + LOGP(DV5CTRL, LOGL_DEBUG, "We are waiting for ack, so we queue our message.\n"); + /* pending message, save message in queue */ + msgb_enqueue(&ctrl->tx_queue, msg); + break; } } -static const struct osmo_fsm_state v51_ctrl_common_fsm_states[] = { - [V51_CTRL_COM_ST_OUT_OF_SERVICE] = { +/* receive acknowledge */ +static void v51_ctrl_ack(struct osmo_fsm_inst *fi, const struct tlv_parsed *tp) +{ + struct v5x_ctrl_proto *ctrl = fi->priv; + struct msgb *msg; + + LOGP(DV5CTRL, LOGL_DEBUG, "Received acknowledge, removing pending message, if any.\n"); + /* free pending copy of message, if acked before retry */ + if (ctrl->tx_msg) { + msgb_free(ctrl->tx_msg); + ctrl->tx_msg = NULL; + } + /* go to IN_SERVICE */ + osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_IN_SERVICE, 0, 0); + /* sending next pending message in queue */ + msg = msgb_dequeue(&ctrl->tx_queue); + if (msg) { + LOGP(DV5CTRL, LOGL_DEBUG, "Found pending message in queue.\n"); + v51_ctrl_send(ctrl, msg); + } +} + +/* stop traffic */ +static void v51_ctrl_stop(struct osmo_fsm_inst *fi) +{ + struct v5x_ctrl_proto *ctrl = fi->priv; + struct msgb *msg; + + /* flush pending messages */ + if (ctrl->tx_msg) { + msgb_free(ctrl->tx_msg); + ctrl->tx_msg = NULL; + } + while ((msg = msgb_dequeue(&ctrl->tx_queue))) + msgb_free(msg); + /* go to OUT_OF_SERVICE */ + osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_OUT_OF_SERVICE, 0, 0); +} + +/* T01 / T02 */ +static int v51_ctrl_fsm_timer_cb(struct osmo_fsm_inst *fi) +{ + struct v5x_ctrl_proto *ctrl = fi->priv; + struct msgb *msg; + + if (ctrl->tx_msg) { + LOGP(DV5CTRL, LOGL_DEBUG, "Timer fired the first time, resending message.\n"); + /* first expiry: repeat CONTROL; re-start T01 */ + /* send message towards DL */ + osmo_timer_schedule(&fi->timer, TIMEOUT, 0); + msg = ctrl->tx_msg; + ctrl->tx_msg = NULL; + v5x_snd((ctrl->v5if) ? : ctrl->v5up->inst, V51_DLADDR_CTRL, msg); + } else { + LOGP(DV5CTRL, LOGL_DEBUG, "Timer fired the second time, indicate an error.\n"); + /* second expiry: send MDU-error_ind; go to IN_SERVICE */ + v51_mdu_error(fi, "Second timeout while waiting for CONTROL ACK."); + osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_IN_SERVICE, 0, 0); + /* sending next pending message in queue */ + msg = msgb_dequeue(&ctrl->tx_queue); + if (msg) { + LOGP(DV5CTRL, LOGL_DEBUG, "Found pending message in queue.\n"); + v51_ctrl_send(ctrl, msg); + } + } + + return 0; +} + +/* events in state OUT OF SERVICE */ +static void v51_ctrl_fsm_oos(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case V51_CTRL_EV_MDU_START_TRAFFIC: + /* go to IN_SERVICE */ + osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_IN_SERVICE, 0, 0); + break; + case V51_CTRL_EV_MDU_STOP_TRAFFIC: + v51_mdu_error(fi, "Got MDU-stop, but traffic not started."); + break; + case V51_CTRL_EV_MDU_CTRL: + msgb_free(data); + v51_mdu_error(fi, "Got MDU-CTRL, but traffic not started."); + break; + case V51_CTRL_EV_RX_CONTROL: + v51_mdu_error(fi, "Received CONTROL, but traffic not started."); + break; + case V51_CTRL_EV_RX_CONTROL_ACK: + v51_mdu_error(fi, "Received CONTROL ACK, but traffic not started."); + break; + } +} + +/* events in state IN SERVICE */ +static void v51_ctrl_fsm_ins(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case V51_CTRL_EV_MDU_START_TRAFFIC: + break; + case V51_CTRL_EV_MDU_STOP_TRAFFIC: + v51_ctrl_stop(fi); + break; + case V51_CTRL_EV_MDU_CTRL: + v51_ctrl_send(fi->priv, data); + break; + case V51_CTRL_EV_RX_CONTROL: + v51_mdu_ctrl(fi, data); + break; + } +} + +/* events in state AWAIT PORT/COMMON ACK */ +static void v51_ctrl_fsm_aa(struct osmo_fsm_inst *fi, uint32_t event, void *data) +{ + switch (event) { + case V51_CTRL_EV_MDU_START_TRAFFIC: + break; + case V51_CTRL_EV_MDU_STOP_TRAFFIC: + v51_ctrl_stop(fi); + break; + case V51_CTRL_EV_MDU_CTRL: + v51_ctrl_send(fi->priv, data); + break; + case V51_CTRL_EV_RX_CONTROL: + v51_mdu_ctrl(fi, data); + break; + case V51_CTRL_EV_RX_CONTROL_ACK: + v51_ctrl_ack(fi, data); + break; + } +} + +static const struct osmo_fsm_state v51_ctrl_fsm_states[] = { + [V51_CTRL_ST_OUT_OF_SERVICE] = { .name = "OUT_OF_SERVICE", - .in_event_mask = S(V51_CTRL_CE_MDU_START_TRAFFIC) | - S(V51_CTRL_CE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_CE_MDU_CTRL) | - S(V51_CTRL_CE_RX_COMMON_CONTROL) | - S(V51_CTRL_CE_RX_COMMON_CONTROL_ACK), - .out_state_mask = S(V51_CTRL_COM_ST_IN_SERVICE), + .in_event_mask = S(V51_CTRL_EV_MDU_START_TRAFFIC) | + S(V51_CTRL_EV_MDU_STOP_TRAFFIC) | + S(V51_CTRL_EV_MDU_CTRL) | + S(V51_CTRL_EV_RX_CONTROL) | + S(V51_CTRL_EV_RX_CONTROL_ACK), + .out_state_mask = S(V51_CTRL_ST_IN_SERVICE), + .action = v51_ctrl_fsm_oos, }, - [V51_CTRL_COM_ST_IN_SERVICE] = { + [V51_CTRL_ST_IN_SERVICE] = { .name = "IN_SERVICE", - .in_event_mask = S(V51_CTRL_CE_MDU_START_TRAFFIC) | - S(V51_CTRL_CE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_CE_MDU_CTRL) | - S(V51_CTRL_CE_RX_COMMON_CONTROL), - .out_state_mask = S(V51_CTRL_COM_ST_OUT_OF_SERVICE) | - S(V51_CTRL_COM_AWAIT_COMMON_ACK), + .in_event_mask = S(V51_CTRL_EV_MDU_START_TRAFFIC) | + S(V51_CTRL_EV_MDU_STOP_TRAFFIC) | + S(V51_CTRL_EV_MDU_CTRL) | + S(V51_CTRL_EV_RX_CONTROL), + + .out_state_mask = S(V51_CTRL_ST_OUT_OF_SERVICE) | + S(V51_CTRL_ST_AWAIT_ACK), + .action = v51_ctrl_fsm_ins, }, - [V51_CTRL_COM_AWAIT_COMMON_ACK] = { - .name = "AWAIT_COMMON_ACK", - .in_event_mask = S(V51_CTRL_CE_MDU_START_TRAFFIC) | - S(V51_CTRL_CE_MDU_STOP_TRAFFIC) | - S(V51_CTRL_CE_MDU_CTRL) | - S(V51_CTRL_CE_RX_COMMON_CONTROL) | - S(V51_CTRL_CE_RX_COMMON_CONTROL_ACK), - .out_state_mask = S(V51_CTRL_COM_ST_OUT_OF_SERVICE) | - S(V51_CTRL_COM_ST_IN_SERVICE), + [V51_CTRL_ST_AWAIT_ACK] = { + .name = "AWAIT_ACK", + .in_event_mask = S(V51_CTRL_EV_MDU_START_TRAFFIC) | + S(V51_CTRL_EV_MDU_STOP_TRAFFIC) | + S(V51_CTRL_EV_MDU_CTRL) | + S(V51_CTRL_EV_RX_CONTROL) | + S(V51_CTRL_EV_RX_CONTROL_ACK), + .out_state_mask = S(V51_CTRL_ST_OUT_OF_SERVICE) | + S(V51_CTRL_ST_IN_SERVICE), + .action = v51_ctrl_fsm_aa, }, }; -static struct osmo_fsm v51_ctrl_common_fsm = { - .name = "V51_CTRL_COMMON", - .states = v51_ctrl_common_fsm_states, - .num_states = ARRAY_SIZE(v51_ctrl_common_fsm_states), +struct osmo_fsm v51_ctrl_fsm = { + .name = "V51_CTRL", + .states = v51_ctrl_fsm_states, + .num_states = ARRAY_SIZE(v51_ctrl_fsm_states), .allstate_event_mask = 0, .allstate_action = NULL, .cleanup = NULL, - .timer_cb = v51_ctrl_common_fsm_timer_cb, + .timer_cb = v51_ctrl_fsm_timer_cb, .log_subsys = DV5CTRL, - .event_names = v51_ctrl_common_fsm_event_names, + .event_names = v51_ctrl_fsm_event_names, }; +struct v5x_ctrl_proto *v51_ctrl_create(struct v5x_interface *v5if, struct v5x_user_port *v5up, uint16_t nr) +{ + struct v5x_ctrl_proto *ctrl; + + assert((v5if && !v5up) || (!v5if && v5up)); + + ctrl = talloc_zero((v5if) ? : v5up->inst, struct v5x_ctrl_proto); + if (!ctrl) + return NULL; + ctrl->v5if = v5if; + ctrl->v5up = v5up; + INIT_LLIST_HEAD(&ctrl->tx_queue); + + ctrl->fi = osmo_fsm_inst_alloc(&v51_ctrl_fsm, ctrl, ctrl, LOGL_DEBUG, NULL); + if (!ctrl->fi) + return NULL; + osmo_fsm_inst_update_id_f(ctrl->fi, "%d", nr); + + return ctrl; +} + /*********************************************************************** * V5 Message encoding / sending ***********************************************************************/ /* G.964 Section 14.4.1.1 / Table 48 */ -struct mgsb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe) +struct msgb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe) { - uint8_t cfe_ie = cfe; + uint8_t cfe_ie = cfe | 0x80; struct v51_l3_hdr *l3h; struct msgb *msg = msgb_alloc_v5x(); if (!msg) @@ -218,7 +371,7 @@ struct mgsb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h)); l3h->pdisc = V51_CTRL_PDISC; - l3h->l3_addr = v51_l3_addr_enc(v5up->nr, true); + l3h->l3_addr = ntohs(v51_l3_addr_enc(v5up->nr, true)); l3h->msg_type = V51_CTRL_MSGT_PORT_CTRL; msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ELEMENT, 1, &cfe_ie); @@ -229,7 +382,7 @@ struct mgsb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el /* G.964 Section 14.4.1.2 / Table 49 */ static struct msgb *v51_enc_ctrl_port_ack(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe) { - uint8_t cfe_ie = cfe; + uint8_t cfe_ie = cfe | 0x80; struct v51_l3_hdr *l3h; struct msgb *msg = msgb_alloc_v5x(); if (!msg) @@ -237,7 +390,7 @@ static struct msgb *v51_enc_ctrl_port_ack(struct v5x_user_port *v5up, enum v51_c l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h)); l3h->pdisc = V51_CTRL_PDISC; - l3h->l3_addr = v51_l3_addr_enc(v5up->nr, true); + l3h->l3_addr = ntohs(v51_l3_addr_enc(v5up->nr, true)); l3h->msg_type = V51_CTRL_MSGT_PORT_CTRL_ACK; msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ELEMENT, 1, &cfe_ie); @@ -246,10 +399,10 @@ static struct msgb *v51_enc_ctrl_port_ack(struct v5x_user_port *v5up, enum v51_c } /* G.964 Section 14.4.1.3 / Table 50 */ -static struct msgb *v51_enc_ctrl_common(struct v5x_instance *v5i, enum v51_ctrl_func_id cfi, +static struct msgb *v51_enc_ctrl_common(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi, uint8_t *rej_cause, uint8_t *variant, uint32_t *interface_id) { - uint8_t cfi_ie = cfi; + uint8_t cfi_ie = cfi | 0x80; struct v51_l3_hdr *l3h; struct msgb *msg = msgb_alloc_v5x(); if (!msg) @@ -257,17 +410,18 @@ static struct msgb *v51_enc_ctrl_common(struct v5x_instance *v5i, enum v51_ctrl_ l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h)); l3h->pdisc = V51_CTRL_PDISC; - l3h->l3_addr = v51_l3_addr_enc(V51_DLADDR_CTRL, true); + l3h->l3_addr = ntohs(v51_l3_addr_enc(V51_DLADDR_CTRL, true)); l3h->msg_type = V51_CTRL_MSGT_COMMON_CTRL; msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ID, 1, &cfi_ie); if (variant) { /* Conditional: Variant */ - msgb_tlv_put(msg, V51_CTRL_IEI_VARIANT, 1, variant); + uint8_t variant_ie = *variant | 0x80; + msgb_tlv_put(msg, V51_CTRL_IEI_VARIANT, 1, &variant_ie); } if (rej_cause) { /* Conditional: Rejection Cause */ - msgb_put_u8(0xF0 | (*rej_cause & 0x0F)); + msgb_put_u8(msg, 0xF0 | (*rej_cause & 0x0F)); } if (interface_id) { /* Conditional: Interface Id */ @@ -280,9 +434,9 @@ static struct msgb *v51_enc_ctrl_common(struct v5x_instance *v5i, enum v51_ctrl_ } /* G.964 Section 14.4.1.4 / Table 51 */ -static struct msgb *v51_enc_ctrl_common_ack(struct v5x_instance *v5i, enum v51_ctrl_func_id cfi) +static struct msgb *v51_enc_ctrl_common_ack(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi) { - uint8_t cfi_ie = cfi; + uint8_t cfi_ie = cfi | 0x80; struct v51_l3_hdr *l3h; struct msgb *msg = msgb_alloc_v5x(); if (!msg) @@ -290,7 +444,7 @@ static struct msgb *v51_enc_ctrl_common_ack(struct v5x_instance *v5i, enum v51_c l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h)); l3h->pdisc = V51_CTRL_PDISC; - l3h->l3_addr = v51_l3_addr_enc(V51_DLADDR_CTRL, true); + l3h->l3_addr = htons(v51_l3_addr_enc(V51_DLADDR_CTRL, true)); l3h->msg_type = V51_CTRL_MSGT_COMMON_CTRL_ACK; msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ID, 1, &cfi_ie); @@ -302,70 +456,89 @@ static struct msgb *v51_enc_ctrl_common_ack(struct v5x_instance *v5i, enum v51_c * V5 Message receiving / decoding ***********************************************************************/ -static int v51_rcv_ctrl_port(struct v5x_user_port *v5up, struct msgb *msg, const struct tlv_parsed *tp) +static int v51_rcv_ctrl_port(struct v5x_user_port *v5up, uint8_t msg_type, const struct tlv_parsed *tp) { - uint16_t l3_addr; - enum v51_ctrl_func_el cfe = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ELEMENT); - struct v5x_user_port *port = FIXME; + enum v51_ctrl_func_el cfe = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ELEMENT) & 0x7f; - switch (l3h->msg_type) { + switch (msg_type) { case V51_CTRL_MSGT_PORT_CTRL: /* FIXME: send event to FSM */ - osmo_fsm_inst_dispatch(port->ctrl_fi, V51_CTRL_PE_RX_PORT_CONTROL, tp); + osmo_fsm_inst_dispatch(v5up->ctrl->fi, V51_CTRL_EV_RX_CONTROL, (void *)tp); /* send ACK to AN */ - return v51_tx(v5up->inst, v51_enc_ctrl_port_ack(v5up, cfe)); + return v5x_snd(v5up->inst, V51_DLADDR_CTRL, v51_enc_ctrl_port_ack(v5up, cfe)); case V51_CTRL_MSGT_PORT_CTRL_ACK: - osmo_fsm_inst_dispatch(port->ctrl_fi, V51_CTRL_PE_RX_PORT_CONTROL_ACK, tp); + osmo_fsm_inst_dispatch(v5up->ctrl->fi, V51_CTRL_EV_RX_CONTROL_ACK, (void *)tp); default: return -EINVAL; } } -static int v51_rcv_ctrl_common(struct v5x_instance *v5i, struct msgb *msg, const struct tlv_parsed *tp) +static int v51_rcv_ctrl_common(struct v5x_interface *v5if, uint8_t msg_type, const struct tlv_parsed *tp) { - uint16_t l3_addr; - enum v51_ctrl_func_id cfi = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ID); + enum v51_ctrl_func_id cfi = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ID) & 0x7f; - switch (l3h->msg_type) { + switch (msg_type) { case V51_CTRL_MSGT_COMMON_CTRL: /* send event to FSM */ - osmo_fsm_inst_dispatch(v5i->ctrl_fi, V51_CTRL_CE_RX_COMMON_CONTROL, tp); + osmo_fsm_inst_dispatch(v5if->control.ctrl->fi, V51_CTRL_EV_RX_CONTROL, (void *)tp); /* send ACK to AN */ - return v51_tx(v5up->inst, v51_enc_ctrl_common_ack(v5i, cfi)); + printf("sending ack\n"); + return v5x_snd(v5if, V51_DLADDR_CTRL, v51_enc_ctrl_common_ack(v5if, cfi)); case V51_CTRL_MSGT_COMMON_CTRL_ACK: /* send event to FSM */ - osmo_fsm_inst_dispatch(v5i->ctrl_fi, V51_CTRL_CE_RX_COMMON_CONTROL_ACK, tp); - break; + osmo_fsm_inst_dispatch(v5if->control.ctrl->fi, V51_CTRL_EV_RX_CONTROL_ACK, (void *)tp); + return 0; default: return -EINVAL; } } - -static int v51_rcv_ctrl(struct v5x_instance *v5i, struct msgb *msg, const struct tlv_parsed *tp) +/* receive message from lower (DL) layer */ +int v51_rcv_ctrl(struct v5x_interface *v5if, uint16_t l3_addr, uint8_t msg_type, const struct tlv_parsed *tp) { - struct v51_l3_hdr *l3h = msgb_l3(msg); - uint16_t l3_addr; - bool is_isdn; - int rc; + struct v5x_user_port *v5up; - l3_addr = v51_l3_addr_dec(l3h->l3_addr, &is_isdn); - if (!is_isdn) - return -EINVAL; - - switch (l3h->msg_type) { + switch (msg_type) { case V51_CTRL_MSGT_PORT_CTRL: case V51_CTRL_MSGT_PORT_CTRL_ACK: - v5up = v5x_user_port_find(v5i, l4_addr); + v5up = v5x_user_port_find(v5if, l3_addr); if (!v5up) return -ENODEV; - return v5x_rcv_ctrl_port(v5up, msg, tp); + return v51_rcv_ctrl_port(v5up, msg_type, tp); case V51_CTRL_MSGT_COMMON_CTRL: case V51_CTRL_MSGT_COMMON_CTRL_ACK: if (l3_addr != V51_DLADDR_CTRL) return -EINVAL; - return v5x_rcv_ctrl_common(v5i, msg, tp); + return v51_rcv_ctrl_common(v5if, msg_type, tp); } - return rc; + return -EINVAL; +} + +/*********************************************************************** + * V5 Message sending / encoding + ***********************************************************************/ + +/* send common message from upper layer */ +int v51_snd_ctrl_common(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi, + uint8_t *rej_cause, uint8_t *variant, uint32_t *interface_id) +{ + osmo_fsm_inst_dispatch(v5if->control.ctrl->fi, V51_CTRL_EV_MDU_CTRL, v51_enc_ctrl_common(v5if, cfi, rej_cause, variant, interface_id)); + return 0; +} + +/* send port message from upper layer */ +int v51_snd_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe) +{ + osmo_fsm_inst_dispatch(v5up->ctrl->fi, V51_CTRL_EV_MDU_CTRL, v51_enc_ctrl_port(v5up, cfe)); + return 0; +} + +void v51_start_ctrl(struct v5x_ctrl_proto *ctrl) +{ + osmo_fsm_inst_dispatch(ctrl->fi, V51_CTRL_EV_MDU_START_TRAFFIC, NULL); +} +void v51_stop_ctrl(struct v5x_ctrl_proto *ctrl) +{ + osmo_fsm_inst_dispatch(ctrl->fi, V51_CTRL_EV_MDU_START_TRAFFIC, NULL); } diff --git a/v51_le_ctrl.h b/v51_le_ctrl.h new file mode 100644 index 0000000..205ae78 --- /dev/null +++ b/v51_le_ctrl.h @@ -0,0 +1,10 @@ + +extern struct osmo_fsm v51_ctrl_fsm; + +struct v5x_ctrl_proto *v51_ctrl_create(struct v5x_interface *v5if, struct v5x_user_port *v5up, uint16_t nr); +int v51_rcv_ctrl(struct v5x_interface *v5if, uint16_t l3_addr, uint8_t msg_type, const struct tlv_parsed *tp); +int v51_snd_ctrl_common(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi, + uint8_t *rej_cause, uint8_t *variant, uint32_t *interface_id); +int v51_snd_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe); +void v51_start_ctrl(struct v5x_ctrl_proto *ctrl); +void v51_stop_ctrl(struct v5x_ctrl_proto *ctrl);