libertas: rework event subscription

This patch moves re-works the implementation of event subscription
via debugfs. For this:

* it tells cmd.c and cmdresp.c about CMD_802_11_SUBSCRIBE_EVENT
* removes lots of low-level cmd stuff from debugfs.c
* create unified functions to read/write snr, rssi, bcnmiss and
  failcount
* introduces #define's for subscription event bitmask values
* add a function to search for a specific element in an IE
  (a.k.a. TLV)
* add a function to find out the size of the TLV. This is needed
  because lbs_prepare_and_send_command() has an argument for a
  data buffer, but not for it's lengths and TLVs can be, by
  definition, vary in size.
* fix a bug where it was not possible to disable an event

Signed-off-by: Holger Schurig <hs4233@mail.mn-solutions.de>
Acked-by: Dan Williams <dcbw@redhat.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/libertas/cmd.c

@@ -246,6 +246,52 @@ static int lbs_cmd_802_11_enable_rsn(struct lbs_private *priv,
 }
 
 
+static ssize_t lbs_tlv_size(const u8 *tlv, u16 size)
+{
+	ssize_t pos = 0;
+	struct mrvlietypesheader *tlv_h;
+	while (pos < size) {
+		u16 length;
+		tlv_h = (struct mrvlietypesheader *) tlv;
+		if (tlv_h->len == 0)
+			return pos;
+		length = le16_to_cpu(tlv_h->len) +
+			sizeof(struct mrvlietypesheader);
+		pos += length;
+		tlv += length;
+	}
+	return pos;
+}
+
+
+static void lbs_cmd_802_11_subscribe_event(struct lbs_private *priv,
+	struct cmd_ds_command *cmd, u16 cmd_action,
+	void *pdata_buf)
+{
+	struct cmd_ds_802_11_subscribe_event *events =
+		(struct cmd_ds_802_11_subscribe_event *) pdata_buf;
+
+	/* pdata_buf points to a struct cmd_ds_802_11_subscribe_event and room
+	 * for various Marvell TLVs */
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	cmd->size = cpu_to_le16(sizeof(*events)
+			- sizeof(events->tlv)
+			+ S_DS_GEN);
+	cmd->params.subscribe_event.action = cpu_to_le16(cmd_action);
+	if (cmd_action == CMD_ACT_GET) {
+		cmd->params.subscribe_event.events = 0;
+	} else {
+		ssize_t sz = lbs_tlv_size(events->tlv, sizeof(events->tlv));
+		cmd->size = cpu_to_le16(le16_to_cpu(cmd->size) + sz);
+		cmd->params.subscribe_event.events = events->events;
+		memcpy(cmd->params.subscribe_event.tlv, events->tlv, sz);
+	}
+
+	lbs_deb_leave(LBS_DEB_CMD);
+}
+
 static void set_one_wpa_key(struct MrvlIEtype_keyParamSet * pkeyparamset,
                             struct enc_key * pkey)
 {
@@ -1394,6 +1440,10 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 			ret = 0;
 			break;
 		}
+	case CMD_802_11_SUBSCRIBE_EVENT:
+		lbs_cmd_802_11_subscribe_event(priv, cmdptr,
+			cmd_action, pdata_buf);
+		break;
 	case CMD_802_11_PWR_CFG:
 		cmdptr->command = cpu_to_le16(CMD_802_11_PWR_CFG);
 		cmdptr->size =

drivers/net/wireless/libertas/cmdresp.c

@@ -554,6 +554,26 @@ static int lbs_ret_802_11_bcn_ctrl(struct lbs_private * priv,
 	return 0;
 }
 
+static int lbs_ret_802_11_subscribe_event(struct lbs_private *priv,
+	struct cmd_ds_command *resp)
+{
+	struct lbs_adapter *adapter = priv->adapter;
+	struct cmd_ds_802_11_subscribe_event *cmd_event =
+		&resp->params.subscribe_event;
+	struct cmd_ds_802_11_subscribe_event *dst_event =
+		adapter->cur_cmd->pdata_buf;
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	if (dst_event->action == cpu_to_le16(CMD_ACT_GET)) {
+		dst_event->events = le16_to_cpu(cmd_event->events);
+		memcpy(dst_event->tlv, cmd_event->tlv, sizeof(dst_event->tlv));
+	}
+
+	lbs_deb_leave(LBS_DEB_CMD);
+	return 0;
+}
+
 static inline int handle_cmd_response(u16 respcmd,
 				      struct cmd_ds_command *resp,
 				      struct lbs_private *priv)
@@ -689,6 +709,10 @@ static inline int handle_cmd_response(u16 respcmd,
 			sizeof(struct cmd_ds_802_11_led_ctrl));
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		break;
+	case CMD_RET(CMD_802_11_SUBSCRIBE_EVENT):
+		ret = lbs_ret_802_11_subscribe_event(priv, resp);
+		break;
+
 	case CMD_RET(CMD_802_11_PWR_CFG):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		memmove(adapter->cur_cmd->pdata_buf, &resp->params.pwrcfg,