solos: Handle new line status change packets, hook up to ATM layer info (87ebb186) · Commits · e / devices / android_kernel_xiaomi_markw

drivers/atm/solos-pci.c

+92 −1

Original line number	Diff line number	Diff line
		@@ -82,6 +82,7 @@ struct pkt_hdr {
		#define PKT_COMMAND 1
		#define PKT_POPEN 3
		#define PKT_PCLOSE 4
		#define PKT_STATUS 5

		struct solos_card {
		void __iomem *config_regs;
		@@ -275,6 +276,72 @@ static ssize_t solos_param_store(struct device dev, struct device_attribute at
		return ret;
		}

		static char next_string(struct sk_buff skb)
		{
		int i = 0;
		char *this = skb->data;

		while (i < skb->len) {
		if (this[i] == '\n') {
		this[i] = 0;
		skb_pull(skb, i);
		return this;
		}
		}
		return NULL;
		}

		/*
		* Status packet has fields separated by \n, starting with a version number
		* for the information therein. Fields are....
		*
		* packet version
		* TxBitRate (version >= 1)
		* RxBitRate (version >= 1)
		* State (version >= 1)
		*/
		static int process_status(struct solos_card card, int port, struct sk_buff skb)
		{
		char str, end;
		int ver, rate_up, rate_down, state;

		if (!card->atmdev[port])
		return -ENODEV;

		str = next_string(skb);
		if (!str)
		return -EIO;

		ver = simple_strtol(str, NULL, 10);
		if (ver < 1) {
		dev_warn(&card->dev->dev, "Unexpected status interrupt version %d\n",
		ver);
		return -EIO;
		}

		str = next_string(skb);
		rate_up = simple_strtol(str, &end, 10);
		if (*end)
		return -EIO;

		str = next_string(skb);
		rate_down = simple_strtol(str, &end, 10);
		if (*end)
		return -EIO;

		str = next_string(skb);
		if (!strcmp(str, "Showtime"))
		state = ATM_PHY_SIG_FOUND;
		else state = ATM_PHY_SIG_LOST;

		card->atmdev[port]->link_rate = rate_down;
		card->atmdev[port]->signal = state;

		dev_info(&card->dev->dev, "ATM state: '%s', %d/%d kb/s up/down.\n",
		str, rate_up/1000, rate_down/1000);
		return 0;
		}

		static int process_command(struct solos_card card, int port, struct sk_buff skb)
		{
		struct solos_param *prm;
		@@ -512,7 +579,7 @@ void solos_bh(unsigned long card_arg)

		size = le16_to_cpu(header.size);

		skb = alloc_skb(size, GFP_ATOMIC);
		skb = alloc_skb(size + 1, GFP_ATOMIC);
		if (!skb) {
		if (net_ratelimit())
		dev_warn(&card->dev->dev, "Failed to allocate sk_buff for RX\n");
		@@ -547,6 +614,11 @@ void solos_bh(unsigned long card_arg)
		atomic_inc(&vcc->stats->rx);
		break;

		case PKT_STATUS:
		process_status(card, port, skb);
		dev_kfree_skb(skb);
		break;

		case PKT_COMMAND:
		default: /* FIXME: Not really, surely? */
		if (process_command(card, port, skb))
		@@ -996,6 +1068,9 @@ static int atm_init(struct solos_card *card)
		int i;

		for (i = 0; i < card->nr_ports; i++) {
		struct sk_buff *skb;
		struct pkt_hdr *header;

		skb_queue_head_init(&card->tx_queue[i]);
		skb_queue_head_init(&card->cli_queue[i]);

		@@ -1016,6 +1091,22 @@ static int atm_init(struct solos_card *card)
		card->atmdev[i]->ci_range.vci_bits = 16;
		card->atmdev[i]->dev_data = card;
		card->atmdev[i]->phy_data = (void *)(unsigned long)i;
		card->atmdev[i]->signal = ATM_PHY_SIG_UNKNOWN;

		skb = alloc_skb(sizeof(*header), GFP_ATOMIC);
		if (!skb) {
		dev_warn(&card->dev->dev, "Failed to allocate sk_buff in atm_init()\n");
		continue;
		}

		header = (void )skb_put(skb, sizeof(header));

		header->size = cpu_to_le16(0);
		header->vpi = cpu_to_le16(0);
		header->vci = cpu_to_le16(0);
		header->type = cpu_to_le16(PKT_STATUS);

		fpga_queue(card, i, skb, NULL);
		}
		return 0;
		}