drivers/usb/gadget/dropcam_fsdrv.c

/*
 * Copyright (C) 2011-2014 Dropcam
 * All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <net/sock.h>
#include <net/netlink.h>

#include "fsprotodef.h"
#define LAST_RECV_BUF_SIZE 16384

//#define VERBOSE_LOGGING

static struct fsg_common *the_common;

static struct sock *fs_nl_sock;
static int fs_nl_seqnum;
static int fs_nl_last_recv;
static void *last_recv_buf;
static int last_recv_buf_len;

static int fs_nl_active(void) {
  return (mod_data.fs_drv_pid != 0);
}

static int fs_intercept_io(loff_t length, loff_t offset) {
	uint64_t intercept_start, intercept_end;
	
    if (!fs_nl_active())
		return 0;
		
	intercept_start = mod_data.fs_drv_offset;
	intercept_end = mod_data.fs_drv_offset + mod_data.fs_drv_length;
	return offset >= intercept_start;
}

static void fs_nl_rcv_msg(struct sk_buff *skb)
{
	struct nlmsghdr *nlh = NULL;
	if (the_common == NULL)	{
      printk(KERN_ERR "%s: common is NULL\n", __func__);
      return;
	}

	if(skb == NULL) {
		printk(KERN_ERR "%s: invalid sk_buff\n", __func__);
		return;
	}

	nlh = (struct nlmsghdr *)skb->data;
	
	if (nlh->nlmsg_len > LAST_RECV_BUF_SIZE) {
		printk(KERN_ERR "%s: netlink message is too large\n", __func__);
		return;
	}
	
	smp_wmb();
	spin_lock(&the_common->lock);

	fs_nl_last_recv = nlh->nlmsg_seq;
	memcpy(last_recv_buf, nlh, nlh->nlmsg_len);
	last_recv_buf_len = nlh->nlmsg_len;
		
#ifdef VERBOSE_LOGGING
	printk(KERN_ERR "%s: recv nl_msg len: %d\n", __func__, (int)nlh->nlmsg_len);
#endif
	
	wakeup_thread(the_common);
	spin_unlock(&the_common->lock);
}

static int fs_create_netlink(void)
{
  fs_nl_sock = netlink_kernel_create(&init_net, NETLINK_USERSOCK, 0,
      fs_nl_rcv_msg, NULL, THIS_MODULE);
  if (!fs_nl_sock) {
      printk(KERN_ERR "%s: receive handler registration failed\n", __func__);
      return -ENOMEM;
  }

  last_recv_buf = kmalloc(LAST_RECV_BUF_SIZE, GFP_KERNEL);
  return 0;
}

static int fs_release_netlink(void)
{
  if (fs_nl_sock) {
      netlink_kernel_release(fs_nl_sock);
      fs_nl_sock = NULL;
  }

  if (last_recv_buf) {
      kfree(last_recv_buf);
      last_recv_buf = NULL;
  }

  return 0;
}

static int fs_register_common(struct fsg_common *fsg)
{
    the_common = fsg;
    return 0;
}

static int fs_unregister_common(void)
{
  the_common = NULL;
  return 0;
}

static int fs_msg_send(struct fsg_common *fsg, struct fsdrv_nl_hdr *hdr, void *payload)
{
	struct sk_buff *skb_out;
	struct nlmsghdr *nlh;
	int res;
	
	int total_msg_len = sizeof(*hdr) + hdr->payload_len;
	skb_out = nlmsg_new(total_msg_len, 0);
	nlh = nlmsg_put(skb_out, 0, ++fs_nl_seqnum, FSDRV_NL_MSG_TYPE, total_msg_len, 0);
	NETLINK_CB(skb_out).dst_group = 0;
	memcpy(nlmsg_data(nlh), hdr, sizeof(*hdr));
	if (payload > 0) {
	  memcpy(nlmsg_data(nlh) + sizeof(*hdr), payload, hdr->payload_len);
	}

	res = nlmsg_unicast(fs_nl_sock, skb_out, mod_data.fs_drv_pid);

	if(res < 0) {
	    printk(KERN_ERR "Error in nlmsg_unicast: %d\n", res);
		
		/* zero out the PID so we stop trying to send to the process */
		mod_data.fs_drv_pid = 0;
	}
	
	return res;
}
	
static int fs_msg_wait_for_resp(struct fsg_common *fsg)
{
	int rc;
	
	rc = 0;
	/* wait for the response */
	while (fs_nl_last_recv != fs_nl_seqnum) {
		rc = sleep_thread(fsg);
		if (rc)
			return rc;
	}
	
	return rc;
}

static int fs_msg_read(struct fsg_common *fsg, void *buf, loff_t buf_len, loff_t file_offset)
{
	int res;
	struct fsdrv_nl_hdr hdr;
	struct nlhdr *nlh;
	struct fsdrv_nl_hdr *res_hdr;
	
#ifdef VERBOSE_LOGGING
  printk(KERN_ERR "%s: %d, %d\n", __func__, (int)buf_len, (int)file_offset);
#endif

	hdr.op = FSDRV_NL_MSG_READ_REQ;
	hdr.payload_len = buf_len;
	hdr.offset = file_offset;

	/* issue the read request */
	res = fs_msg_send(fsg, &hdr, buf);
	if (res < 0) {
		return res;
	}
	
	/* wait for the response */
	res = fs_msg_wait_for_resp(fsg);
	if (res < 0) {
		return res;
	}
	
	/* response received - copy to the output buffer */
	spin_lock(&fsg->lock);
	nlh = (struct nlhdr*)last_recv_buf;
	res_hdr = NLMSG_DATA(nlh);
	memcpy(buf, NLMSG_DATA(nlh) + sizeof(struct fsdrv_nl_hdr), buf_len);
	spin_unlock(&fsg->lock);
	
	return buf_len;
}

static int fs_msg_write(struct fsg_common *fsg, void *buf, loff_t buf_len, loff_t file_offset)
{
	int res;
	struct fsdrv_nl_hdr hdr;
	hdr.op = FSDRV_NL_MSG_WRITE_REQ;
	hdr.payload_len = buf_len;
	hdr.offset = file_offset;

#ifdef VERBOSE_LOGGING
  printk(KERN_ERR "%s: %d, %d\n", __func__, (int)buf_len, (int)file_offset);
#endif

	/* issue the write request */
	res = fs_msg_send(fsg, &hdr, buf);
	if (res < 0) {
		return res;
	}
	
	/* wait for the response */
	res = fs_msg_wait_for_resp(fsg);
	if (res < 0) {
		return res;
	}
	
	return buf_len;
}