ecp22 add notify module call back function

Add a module call back function named notify. The prototype is

        int (*notify)(int, char *, void *);

It has 3 parameters:
1. The senders module identifier.
2. The interface name the modications apply to.
3. The changed data itself as an opaque data structure.
The data structure is sender and receipient specific.

The function returns true if the changes have been applied successfully
by the receiving module and false otherwise. The module might not be
loaded, or the interface not active or the data simple not understood.

The data is an overlay of structures, the first member identifies
the type of the data structure:
struct qbg22_imm {              /* Intermodule message data structure */
	int data_type;          /* Identifies union data */
	union {                 /* Overlay possible data */
		struct evb22_to_ecp22 a;
		struct evb22_to_vdp22 b;
	} u;
};

For example the EVB Module wants to notify the ECP module about
some parameter changes, then ECP module offers a notify call back
function. EVB module calls it like:
	EVB-modops->notify(EVB_MOD_ID, "ethx", &qbg22_data);
with qbg22_data.data_type indicating which union member to use to
extract the data.

Signed-off-by: Thomas Richter <[email protected]>
Signed-off-by: John Fastabend <[email protected]>

include/lldp_ecp22.h

@@ -59,6 +59,11 @@ struct ecp22_hdr {		/* ECP22 header */
 	u16 seqno;		/* ECP22 sequence number */
 } __attribute__ ((__packed__));
 
+/*
+ * Define maximum ECP protocol payload length. Leave room for END TLV.
+ */
+#define	ECP22_MAXPAYLOAD_LEN	(ETH_DATA_LEN - sizeof(struct ecp22_hdr) - 2)
+
 struct ecp22_buffer {			/* ECP payload buffer */
 	unsigned char frame[ETH_FRAME_LEN];	/* Payload buffer */
 	unsigned short frame_len;	/* # of bytes of valid data */

include/lldp_mod.h

@@ -51,6 +51,7 @@
  * @print_tlv: routine for client to pretty print TLVs
  * @lookup_tlv_name: find tlvid given a tlv 'name'
  * @get_arg_handler: return an arg handler list
+ * @lldp_mod_notify: send data to a module
  */
 struct lldp_mod_ops {
 	struct lldp_module * 	(* lldp_mod_register)(void);
@@ -73,6 +74,7 @@ struct lldp_mod_ops {
 	int			(* print_help)();
 	int			(* timer)(struct port *, struct lldp_agent *);
 	struct arg_handlers *	(* get_arg_handler)(void);
+	int			(*lldp_mod_notify)(int, char *, void *);
 };
 
 /*

qbg/lldp_ecp22.c

@@ -27,6 +27,7 @@
 #include <stdio.h>
 #include <assert.h>
 #include <sys/socket.h>
+#include <errno.h>
 
 #include "eloop.h"
 #include "lldp_ecp22.h"
@@ -161,7 +162,7 @@ static bool ecp22_build_ecpdu(struct ecp22 *ecp)
 
 	ecp22_hdr_set_version(&ecph, 1);
 	ecp22_hdr_set_op(&ecph, ECP22_REQUEST);
-	ecp22_hdr_set_subtype(&ecph, 1);
+	ecp22_hdr_set_subtype(&ecph, ECP22_VDP);
 	ecph.ver_op_sub = htons(ecph.ver_op_sub);
 	ecph.seqno = htons(ecp->tx.seqno);
 	ecp22_append(ecp->tx.frame, &fb_offset, (void *)&ecph, sizeof ecph);
@@ -796,9 +797,154 @@ void ecp22_stop(char *ifname)
 		ecp22_remove(ecp);
 }
 
+/*
+ * Update data exchanged via EVB protocol.
+ * Returns true when data update succeeded.
+ */
+static int data_from_evb(char *ifname, struct evb22_to_ecp22 *ptr)
+{
+	struct ecp22_user_data *eud;
+	struct ecp22 *ecp;
+
+	eud = find_module_user_data_by_id(&lldp_head, LLDP_MOD_ECP22);
+	ecp = find_ecpdata(ifname, eud);
+	if (ecp) {
+		ecp->max_rte = ptr->max_rte;
+		ecp->max_retries = ptr->max_retry;
+		return 1;
+	}
+	return 0;
+}
+
+/*
+ * Add ecp payload data at the end of the queue.
+ */
+static void ecp22_add_payload(struct ecp22 *ecp,
+			      struct ecp22_payload_node *elem)
+{
+	if (LIST_EMPTY(&ecp->inuse.head))
+		LIST_INSERT_HEAD(&ecp->inuse.head, elem, node);
+	else
+		LIST_INSERT_AFTER(ecp->inuse.last, elem, node);
+	ecp->inuse.last = elem;
+	if (!ecp->tx.ecpdu_received)	/* Transmit buffer free */
+		ecp22_tx_run_sm(ecp);
+}
+
+/*
+ * Copy the payload data.
+ */
+static struct packed_tlv *copy_ptlv(struct packed_tlv *from)
+{
+	struct packed_tlv *ptlv = create_ptlv();
+
+	if (!ptlv)
+		return NULL;
+	ptlv->size = from->size;
+	ptlv->tlv = calloc(ptlv->size, sizeof(unsigned char));
+	if (!ptlv->tlv) {
+		free_pkd_tlv(ptlv);
+		return NULL;
+	}
+	memcpy(ptlv->tlv, from->tlv, from->size);
+	return ptlv;
+}
+
+/*
+ * Create a node for the ecp payload data. Get it from the free list if not
+ * empty. Otherwise allocate from heap.
+ */
+static struct ecp22_payload_node *ecp22_getnode(struct ecp22_freelist *list)
+{
+	struct ecp22_payload_node *elem = LIST_FIRST(&list->head);
+
+	if (!elem)
+		elem = calloc(1, sizeof *elem);
+	else {
+		LIST_REMOVE(elem, node);
+		--list->freecnt;
+	}
+	return elem;
+}
+
+/*
+ * Receive upper layer protocol data unit for transmit.
+ * Returns error if the request could not be queued for transmision.
+ */
+static int ecp22_req2send(char *ifname, unsigned short subtype,
+			  unsigned const char *mac, struct packed_tlv *du)
+{
+	struct ecp22_user_data *eud;
+	struct ecp22 *ecp;
+	struct ecp22_payload_node *payda;
+	struct packed_tlv *ptlv = copy_ptlv(du);
+	int rc = 0;
+
+	LLDPAD_DBG("%s:%s subtype:%d\n", __func__, ifname, subtype);
+
+	eud = find_module_user_data_by_id(&lldp_head, LLDP_MOD_ECP22);
+	ecp = find_ecpdata(ifname, eud);
+	if (!ecp) {
+		rc = -ENODEV;
+		goto out;
+	}
+	if (!ptlv) {
+		rc = -ENOMEM;
+		goto out;
+	}
+	if (ptlv->size >= ECP22_MAXPAYLOAD_LEN) {
+		rc = -E2BIG;
+		goto out;
+	}
+	payda = ecp22_getnode(&ecp->isfree);
+	if (!payda) {
+		free_pkd_tlv(ptlv);
+		rc = -ENOMEM;
+		goto out;
+	}
+	payda->ptlv = ptlv;
+	payda->subtype = subtype;
+	memcpy(payda->mac, mac, sizeof payda->mac);
+	ecp22_add_payload(ecp, payda);
+out:
+	LLDPAD_DBG("%s:%s rc:%d\n", __func__, ifname, rc);
+	return rc;
+}
+
+/*
+ * Payload data from VDP module.
+ * Returns true when data update succeeded.
+ */
+static int data_from_vdp(char *ifname, struct vdp22_to_ecp22 *ptr)
+{
+	struct packed_tlv d;
+
+	d.size = ptr->len;
+	d.tlv = ptr->data;
+	return ecp22_req2send(ifname, ECP22_VDP, nearest_customer_bridge, &d);
+}
+
+/*
+ * Handle notifications from other modules. Check if sender-id and data type
+ * indicator match. Return false when data could not be delivered.
+ */
+static int ecp22_notify(int sender_id, char *ifname, void *data)
+{
+	struct qbg22_imm *qbg = (struct qbg22_imm *)data;
+
+	LLDPAD_DBG("%s:%s sender-id:%#x data_type:%d\n", __func__, ifname,
+		   sender_id, qbg->data_type);
+	if (sender_id == LLDP_MOD_EVB22 && qbg->data_type == EVB22_TO_ECP22)
+		return data_from_evb(ifname, &qbg->u.a);
+	if (sender_id == LLDP_MOD_VDP22 && qbg->data_type == VDP22_TO_ECP22)
+		return data_from_vdp(ifname, &qbg->u.c);
+	return 0;
+}
+
 static const struct lldp_mod_ops ecp22_ops =  {
 	.lldp_mod_register = ecp22_register,
-	.lldp_mod_unregister = ecp22_unregister
+	.lldp_mod_unregister = ecp22_unregister,
+	.lldp_mod_notify = ecp22_notify
 };
 
 /*

qbg/lldp_evb22.c

@@ -37,6 +37,27 @@
 
 extern struct lldp_head lldp_head;
 
+/*
+ * Function to advertise changed variables to other modules.
+ *
+ * Parameters are interface name, target module id and data.
+ * When sending the data, the module call back function contains the
+ * module id of the sender.
+ *
+ * Return 0 when no addressee found or addressess found but addressee was
+ * unable to handle data.
+ */
+static int modules_notify(int id, char *ifname, void *data)
+{
+	struct lldp_module *mp = find_module_by_id(&lldp_head, id);
+	int rc = 0;
+
+	if (mp && mp->ops->lldp_mod_notify)
+		rc = mp->ops->lldp_mod_notify(LLDP_MOD_EVB22, ifname, data);
+	LLDPAD_DBG("%s:%s target-id:%#x rc:%d\n", __func__, ifname, id, rc);
+	return rc;
+}
+
 struct evb22_data *evb22_data(char *ifname, enum agent_type type)
 {
 	struct evb22_user_data *ud;
@@ -215,14 +236,27 @@ static void station_tlv(struct evb22_data *ed)
 /*
  * Checks values received in TLV and takes over some values.
  * Sets the new suggestion in member tie to be send out to switch.
+ *
+ * Also notify depending modules about the new values.
  */
 static void evb22_update_tlv(struct evb22_data *ed)
 {
+	struct qbg22_imm qbg;
 
 	if (evb_ex_evbmode(ed->policy.evb_mode) == EVB_STATION)
 		station_tlv(ed);
 	else
 		bridge_tlv(ed);
+
+	qbg.data_type = EVB22_TO_ECP22;
+	qbg.u.a.max_rte = evb_ex_rte(ed->out.r_rte);
+	qbg.u.a.max_retry = evb_ex_retries(ed->out.r_rte);
+	modules_notify(LLDP_MOD_ECP22, ed->ifname, &qbg);
+
+	qbg.data_type = EVB22_TO_VDP22;
+	qbg.u.b.max_rka = evb_ex_rka(ed->out.rl_rka);
+	qbg.u.b.max_rwd = evb_ex_rwd(ed->out.evb_mode);
+	modules_notify(LLDP_MOD_VDP22, ed->ifname, &qbg);
 }
 
 /*