Work on control protocols

jolly-gsmtap
Andreas Eversberg 2 months ago
parent c01c48a0aa
commit b885d47b45
  1. 2
      Makefile
  2. 525
      v51_le_ctrl.c
  3. 10
      v51_le_ctrl.h

@ -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:

@ -20,197 +20,350 @@
*
*/
#include <unitstd.h>
#include <unistd.h>
#include <stdint.h>
#include <errno.h>
#include <assert.h>
#include <arpa/inet.h>
#include <osmocom/core/util.h>
#include <osmocom/core/utils.h>
#include <osmocom/core/msgb.h>
#include <osmocom/gsm/tlv.h>
#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,
};
/* display MDU-error_indication */
static void v51_mdu_error(struct osmo_fsm_inst *fi, const char *error)
{
LOGP(DV5CTRL, LOGL_NOTICE, "MDU-error_indication: %s\n", error);
}
/***********************************************************************
* common CTRL protocol FSM / G.964 Section 14
***********************************************************************/
/* 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;
}
}
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,
};
/* 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);
}
}
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,
};
/* stop traffic */
static void v51_ctrl_stop(struct osmo_fsm_inst *fi)
{
struct v5x_ctrl_proto *ctrl = fi->priv;
struct msgb *msg;
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 }
};
/* 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);
}
static int v51_ctrl_common_fsm_timer_cb(struct osmo_fsm_inst *fi)
/* T01 / T02 */
static int v51_ctrl_fsm_timer_cb(struct osmo_fsm_inst *fi)
{
if () {
/* first expiry: repeat COMMON_CONTROL; re-start T02 */
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 */
osmo_fsm_inst_state_chg(fi, V51_CTRL_COM_ST_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;
}
static const struct osmo_fsm_state v51_ctrl_common_fsm_states[] = {
[V51_CTRL_COM_ST_OUT_OF_SERVICE] = {
/* 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;
l3_addr = v51_l3_addr_dec(l3h->l3_addr, &is_isdn);
if (!is_isdn)
return -EINVAL;
struct v5x_user_port *v5up;
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);
}

@ -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);
Loading…
Cancel
Save