static int eth_stop(struct net_device *net)
{
struct eth_dev *dev = netdev_priv(net);
unsigned long flags;
VDBG(dev, "%s\n", __func__);
netif_stop_queue(net);
DBG(dev, "stop stats: rx/tx %ld/%ld, errs %ld/%ld\n",
dev->net->stats.rx_packets, dev->net->stats.tx_packets,
dev->net->stats.rx_errors, dev->net->stats.tx_errors
);
/* ensure there are no more active requests */
spin_lock_irqsave(&dev->lock, flags);
if (dev->port_usb) {
struct gether *link = dev->port_usb;
if (link->close)
link->close(link);
/* NOTE: we have no abort-queue primitive we could use
* to cancel all pending I/O. Instead, we disable then
* reenable the endpoints ... this idiom may leave toggle
* wrong, but that's a self-correcting error.
*
* REVISIT: we *COULD* just let the transfers complete at
* their own pace; the network stack can handle old packets.
* For the moment we leave this here, since it works.
*/
usb_ep_disable(link->in_ep);
usb_ep_disable(link->out_ep);
if (netif_carrier_ok(net)) {
DBG(dev, "host still using in/out endpoints\n");
usb_ep_enable(link->in_ep, link->in);
usb_ep_enable(link->out_ep, link->out);
}
}
spin_unlock_irqrestore(&dev->lock, flags);
return 0;
}
void
MulticastTransport::passive_connection(MulticastPeer local_peer, MulticastPeer remote_peer)
{
GuardThreadType guard(this->connections_lock_);
VDBG_LVL((LM_DEBUG, "(%P|%t) MulticastTransport[%C]::passive_connection "
"from remote peer 0x%x to local peer 0x%x\n",
this->config_i_->name().c_str(), remote_peer, local_peer), 2);
const Peers peers(remote_peer, local_peer);
const PendConnMap::iterator pend = this->pending_connections_.find(peers);
if (pend != pending_connections_.end()) {
Links::const_iterator server_link = this->server_links_.find(local_peer);
DataLink_rch link;
if (server_link != this->server_links_.end()) {
link = static_rchandle_cast<DataLink>(server_link->second);
}
VDBG((LM_DEBUG, "(%P|%t) MulticastTransport::passive_connection completing\n"));
PendConnMap::iterator updated_pend = pend;
do {
TransportClient* pend_client = updated_pend->second.front().first;
RepoId remote_repo = updated_pend->second.front().second;
guard.release();
pend_client->use_datalink(remote_repo, link);
guard.acquire();
} while ((updated_pend = pending_connections_.find(peers)) != pending_connections_.end());
}
//if connection was pending, calls to use_datalink finalized the connection
//if it was not previously pending, accept_datalink() will finalize connection
this->connections_.insert(peers);
}
/* Resets endpoint */
static void ep_init(struct udc_regs __iomem *regs, struct udc_ep *ep)
{
u32 tmp;
VDBG(ep->dev, "ep-%d reset\n", ep->num);
ep->ep.desc = NULL;
ep->ep.ops = &udc_ep_ops;
INIT_LIST_HEAD(&ep->queue);
ep->ep.maxpacket = (u16) ~0;
/* set NAK */
tmp = readl(&ep->regs->ctl);
tmp |= AMD_BIT(UDC_EPCTL_SNAK);
writel(tmp, &ep->regs->ctl);
ep->naking = 1;
/* disable interrupt */
tmp = readl(®s->ep_irqmsk);
tmp |= AMD_BIT(ep->num);
writel(tmp, ®s->ep_irqmsk);
if (ep->in) {
/* unset P and IN bit of potential former DMA */
tmp = readl(&ep->regs->ctl);
tmp &= AMD_UNMASK_BIT(UDC_EPCTL_P);
writel(tmp, &ep->regs->ctl);
tmp = readl(&ep->regs->sts);
tmp |= AMD_BIT(UDC_EPSTS_IN);
writel(tmp, &ep->regs->sts);
/* flush the fifo */
tmp = readl(&ep->regs->ctl);
tmp |= AMD_BIT(UDC_EPCTL_F);
writel(tmp, &ep->regs->ctl);
}
/* reset desc pointer */
writel(0, &ep->regs->desptr);
}
static int goku_ep_disable(struct usb_ep *_ep)
{
struct goku_ep *ep;
struct goku_udc *dev;
unsigned long flags;
ep = container_of(_ep, struct goku_ep, ep);
if (!_ep || !ep->ep.desc)
return -ENODEV;
dev = ep->dev;
if (dev->ep0state == EP0_SUSPEND)
return -EBUSY;
VDBG(dev, "disable %s\n", _ep->name);
spin_lock_irqsave(&dev->lock, flags);
nuke(ep, -ESHUTDOWN);
ep_reset(dev->regs, ep);
spin_unlock_irqrestore(&dev->lock, flags);
return 0;
}
int proc_status_read(char *buffer, char **start, off_t offset, int count,
int *eof, void *user_data)
{
struct psfreedom_device *dev = user_data;
unsigned int len;
unsigned long flags;
spin_lock_irqsave (&dev->lock, flags);
/* This file is meant to be read in a loop, so don't spam dmesg with an INFO
message when it gets read */
VDBG (dev, "proc_status_read (/proc/%s/%s) called. count %d\n",
PROC_DIR_NAME, PROC_STATUS_NAME, count);
*eof = 1;
/* fill the buffer, return the buffer size */
len = sprintf (buffer + offset, "%s\n", STATUS_STR (dev->status));
spin_unlock_irqrestore (&dev->lock, flags);
return len;
}
static int audioservice_send(struct bluetooth_data *data,
const bt_audio_msg_header_t *msg)
{
int err;
uint16_t length;
length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE;
VDBG("sending %s", bt_audio_strtype(msg->type));
if (send(data->server.fd, msg, length,
MSG_NOSIGNAL) > 0)
err = 0;
else {
err = -errno;
ERR("Error sending data to audio service: %s(%d)",
strerror(errno), errno);
if (err == -EPIPE)
bluetooth_close(data);
}
return err;
}
void calibrate_bitpool( struct bluetooth_data *data, int increase)
{
VDBG(" Bitpool Calibrationg bitpool %d",data->sbc.bitpool);
VDBG(" Bitpool Calibrationg freq %d",data->sbc.frequency);
VDBG(" Bitpool Calibrationg mode %d",data->sbc.mode);
VDBG(" Bitpool Calibrationg max_bitpool %d",data->sbc_capabilities.max_bitpool);
VDBG(" Bitpool Calibrationg min_bitpool %d",data->sbc_capabilities.min_bitpool);
VDBG(" origin bitpool %d",data->bitpool);
// VDBG(" Bitpool Calibrationg to %d",data->sbc.bitpool);
// VDBG(" Bitpool Calibrationg to %d",data->sbc.bitpool);
if( increase ){
data->sbc.bitpool=data->sbc_capabilities.max_bitpool;
DBG(" Bitpool increased to %d",data->sbc.bitpool);
} else { // if we are decreasing bitpool, using middle quality values
if( data->sbc.bitpool != data->sbc_capabilities.max_bitpool ) return;
skip_avdtp_flow_check=1;
if(data->sbc_capabilities.max_bitpool <= 35 ){
if ((data->sbc.mode == SBC_MODE_JOINT_STEREO) ||
(data->sbc.mode == SBC_MODE_STEREO )){
switch( data->sbc.frequency ){
case BT_SBC_SAMPLING_FREQ_44100:
data->sbc.bitpool = MAX(19,data->sbc_capabilities.min_bitpool);
break;
case BT_SBC_SAMPLING_FREQ_48000:
data->sbc.bitpool = MAX(18, data->sbc_capabilities.min_bitpool);
break;
}
} else { // for other modes
switch ( data->sbc.frequency) {
case BT_SBC_SAMPLING_FREQ_44100:
data->sbc.bitpool = MAX(19,data->sbc_capabilities.min_bitpool);
break;
case BT_SBC_SAMPLING_FREQ_48000:
data->sbc.bitpool = MAX(18, data->sbc_capabilities.min_bitpool);
break;
}
}
}else{
data->sbc.bitpool=data->sbc_capabilities.max_bitpool-20;
}
DBG(" Bitpool decreased to %d",data->sbc.bitpool);
}
}
void
MulticastTransport::stop_accepting_or_connecting(TransportClient* client,
const RepoId& remote_id)
{
VDBG((LM_DEBUG, "(%P|%t) MulticastTransport::stop_accepting_or_connecting\n"));
GuardThreadType guard(this->connections_lock_);
for (PendConnMap::iterator it = this->pending_connections_.begin();
it != this->pending_connections_.end(); ++it) {
for (size_t i = 0; i < it->second.size(); ++i) {
if (it->second[i].first == client && it->second[i].second == remote_id) {
it->second.erase(it->second.begin() + i);
break;
}
}
if (it->second.empty()) {
this->pending_connections_.erase(it);
return;
}
}
}
/**
* Run a script. argv[2] is already NULL.
*/
static int run(char *argv[3], struct in_addr *ip)
{
int status;
char *addr = addr; /* for gcc */
const char *fmt = "%s %s %s" + 3;
VDBG("%s run %s %s\n", intf, argv[0], argv[1]);
if (ip) {
addr = inet_ntoa(*ip);
setenv("ip", addr, 1);
fmt -= 3;
}
bb_info_msg(fmt, argv[1], intf, addr);
status = wait4pid(spawn(argv));
if (status < 0) {
bb_perror_msg("%s %s %s" + 3, argv[1], intf);
return -errno;
}
if (status != 0)
bb_error_msg("script %s %s failed, exitcode=%d", argv[0], argv[1], status);
return status;
}
static void tx_complete(struct usb_ep *ep, struct usb_request *req)
{
struct printer_dev *dev = ep->driver_data;
switch (req->status) {
default:
VDBG(dev, "tx err %d\n", req->status);
case -ECONNRESET:
case -ESHUTDOWN:
break;
case 0:
break;
}
spin_lock(&dev->lock);
list_del_init(&req->list);
list_add(&req->list, &dev->tx_reqs);
wake_up_interruptible(&dev->tx_wait);
if (likely(list_empty(&dev->tx_reqs_active)))
wake_up_interruptible(&dev->tx_flush_wait);
spin_unlock(&dev->lock);
}
/* Called when entering a state */
int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
{
state_changed = 1;
if (fsm->transceiver->state == new_state)
return 0;
VDBG("chg state to %s", state_string(new_state));
otg_leave_state(fsm, fsm->transceiver->state);
switch (new_state) {
case OTG_STATE_B_IDLE:
otg_drv_vbus(fsm, 0);
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_UNDEF);
otg_add_timer(fsm, b_se0_srp_tmr);
break;
case OTG_STATE_B_SRP_INIT:
otg_start_pulse(fsm);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_UNDEF);
otg_add_timer(fsm, b_srp_fail_tmr);
break;
case OTG_STATE_B_PERIPHERAL:
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 1);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_GADGET);
break;
case OTG_STATE_B_WAIT_ACON:
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, b_ase0_brst_tmr);
fsm->a_bus_suspend = 0;
break;
case OTG_STATE_B_HOST:
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 1);
otg_set_protocol(fsm, PROTO_HOST);
usb_bus_start_enum(fsm->transceiver->host,
fsm->transceiver->host->otg_port);
break;
case OTG_STATE_A_IDLE:
otg_drv_vbus(fsm, 0);
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
break;
case OTG_STATE_A_WAIT_VRISE:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, a_wait_vrise_tmr);
break;
case OTG_STATE_A_WAIT_BCON:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, a_wait_bcon_tmr);
break;
case OTG_STATE_A_HOST:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 1);
otg_set_protocol(fsm, PROTO_HOST);
/* When HNP is triggered while a_bus_req = 0, a_host will
* suspend too fast to complete a_set_b_hnp_en
*/
if (!fsm->a_bus_req || fsm->a_suspend_req)
otg_add_timer(fsm, a_wait_enum_tmr);
break;
case OTG_STATE_A_SUSPEND:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, a_aidl_bdis_tmr);
break;
case OTG_STATE_A_PERIPHERAL:
otg_loc_conn(fsm, 1);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_GADGET);
otg_drv_vbus(fsm, 1);
break;
case OTG_STATE_A_WAIT_VFALL:
otg_drv_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
break;
case OTG_STATE_A_VBUS_ERR:
otg_drv_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_UNDEF);
break;
default:
break;
}
fsm->transceiver->state = new_state;
return 0;
}
static int __init devices_bind(struct usb_gadget *gadget,
struct psfreedom_device *dev)
{
struct usb_ep *out_ep;
struct usb_ep *in_ep;
gadget_for_each_ep (out_ep, gadget) {
if (0 == strcmp (out_ep->name,
psfreedom_get_endpoint_name (&jig_out_endpoint_desc)))
break;
}
if (!out_ep) {
ERROR (dev, "%s: can't find %s on %s\n",
psfreedom_get_endpoint_name (&jig_out_endpoint_desc),
shortname, gadget->name);
return -ENODEV;
}
out_ep->driver_data = out_ep; /* claim */
gadget_for_each_ep (in_ep, gadget) {
if (0 == strcmp (in_ep->name,
psfreedom_get_endpoint_name (&jig_in_endpoint_desc)))
break;
}
if (!in_ep) {
ERROR (dev, "%s: can't find %s on %s\n",
psfreedom_get_endpoint_name (&jig_in_endpoint_desc),
shortname, gadget->name);
return -ENODEV;
}
in_ep->driver_data = in_ep; /* claim */
/* ok, we made sense of the hardware ... */
dev->in_ep = in_ep;
dev->out_ep = out_ep;
/* If the machine specific code changed our descriptors,
change the ones we'll send too */
memcpy (port5_config_desc + USB_DT_CONFIG_SIZE + USB_DT_INTERFACE_SIZE,
&jig_out_endpoint_desc,
USB_DT_ENDPOINT_SIZE);
memcpy (port5_config_desc + USB_DT_CONFIG_SIZE + \
USB_DT_INTERFACE_SIZE + USB_DT_ENDPOINT_SIZE,
&jig_in_endpoint_desc,
USB_DT_ENDPOINT_SIZE);
INFO(dev, "using %s, EP IN %s (0x%X)\n", gadget->name, in_ep->name,
jig_in_endpoint_desc.bEndpointAddress);
INFO(dev, "using %s, EP OUT %s (0x%X)\n", gadget->name, out_ep->name,
jig_out_endpoint_desc.bEndpointAddress);
/* the max packet size of all the devices must be the same as the ep0 max
packet size, otherwise it won't work */
((struct usb_device_descriptor *)port1_device_desc)->bMaxPacketSize0 = \
((struct usb_device_descriptor *)port2_device_desc)->bMaxPacketSize0 = \
((struct usb_device_descriptor *)port3_device_desc)->bMaxPacketSize0 = \
((struct usb_device_descriptor *)port4_device_desc)->bMaxPacketSize0 = \
((struct usb_device_descriptor *)port5_device_desc)->bMaxPacketSize0 = \
((struct usb_device_descriptor *)port6_device_desc)->bMaxPacketSize0 = \
gadget->ep0->maxpacket;
VDBG(dev, "devices_bind finished ok\n");
return 0;
}
static void tx_complete(struct usb_ep *ep, struct usb_request *req)
{
struct sk_buff *skb = req->context;
struct eth_dev *dev = ep->driver_data;
struct net_device *net = dev->net;
struct usb_request *new_req;
struct usb_ep *in;
int length;
int retval;
switch (req->status) {
default:
dev->net->stats.tx_errors++;
VDBG(dev, "tx err %d\n", req->status);
/* FALLTHROUGH */
case -ECONNRESET: /* unlink */
case -ESHUTDOWN: /* disconnect etc */
break;
case 0:
if (!req->zero)
dev->net->stats.tx_bytes += req->length-1;
else
dev->net->stats.tx_bytes += req->length;
}
dev->net->stats.tx_packets++;
spin_lock(&dev->req_lock);
list_add_tail(&req->list, &dev->tx_reqs);
if (dev->port_usb->multi_pkt_xfer) {
dev->no_tx_req_used--;
req->length = 0;
in = dev->port_usb->in_ep;
if (!list_empty(&dev->tx_reqs)) {
new_req = container_of(dev->tx_reqs.next,
struct usb_request, list);
list_del(&new_req->list);
spin_unlock(&dev->req_lock);
if (new_req->length > 0) {
length = new_req->length;
/* NCM requires no zlp if transfer is
* dwNtbInMaxSize */
if (dev->port_usb->is_fixed &&
length == dev->port_usb->fixed_in_len &&
(length % in->maxpacket) == 0)
new_req->zero = 0;
else
new_req->zero = 1;
/* use zlp framing on tx for strict CDC-Ether
* conformance, though any robust network rx
* path ignores extra padding. and some hardware
* doesn't like to write zlps.
*/
if (new_req->zero && !dev->zlp &&
(length % in->maxpacket) == 0) {
new_req->zero = 0;
length++;
}
new_req->length = length;
retval = usb_ep_queue(in, new_req, GFP_ATOMIC);
switch (retval) {
default:
DBG(dev, "tx queue err %d\n", retval);
break;
case 0:
spin_lock(&dev->req_lock);
dev->no_tx_req_used++;
spin_unlock(&dev->req_lock);
net->trans_start = jiffies;
}
} else {
spin_lock(&dev->req_lock);
list_add(&new_req->list, &dev->tx_reqs);
spin_unlock(&dev->req_lock);
}
} else {
int zcip_main(int argc UNUSED_PARAM, char **argv)
{
int state;
char *r_opt;
unsigned opts;
// ugly trick, but I want these zeroed in one go
struct {
const struct in_addr null_ip;
const struct ether_addr null_addr;
struct in_addr ip;
struct ifreq ifr;
int timeout_ms; /* must be signed */
unsigned conflicts;
unsigned nprobes;
unsigned nclaims;
int ready;
} L;
#define null_ip (L.null_ip )
#define null_addr (L.null_addr )
#define ip (L.ip )
#define ifr (L.ifr )
#define timeout_ms (L.timeout_ms)
#define conflicts (L.conflicts )
#define nprobes (L.nprobes )
#define nclaims (L.nclaims )
#define ready (L.ready )
memset(&L, 0, sizeof(L));
INIT_G();
#define FOREGROUND (opts & 1)
#define QUIT (opts & 2)
// parse commandline: prog [options] ifname script
// exactly 2 args; -v accumulates and implies -f
opt_complementary = "=2:vv:vf";
opts = getopt32(argv, "fqr:p:v", &r_opt, &pidfile, &verbose);
#if !BB_MMU
// on NOMMU reexec early (or else we will rerun things twice)
if (!FOREGROUND)
bb_daemonize_or_rexec(0 /*was: DAEMON_CHDIR_ROOT*/, argv);
#endif
// open an ARP socket
// (need to do it before openlog to prevent openlog from taking
// fd 3 (sock_fd==3))
xmove_fd(xsocket(AF_PACKET, SOCK_PACKET, htons(ETH_P_ARP)), sock_fd);
if (!FOREGROUND) {
// do it before all bb_xx_msg calls
openlog(applet_name, 0, LOG_DAEMON);
logmode |= LOGMODE_SYSLOG;
}
if (opts & 4) { // -r n.n.n.n
if (inet_aton(r_opt, &ip) == 0
|| (ntohl(ip.s_addr) & IN_CLASSB_NET) != LINKLOCAL_ADDR
) {
bb_error_msg_and_die("invalid link address");
}
}
argv += optind - 1;
/* Now: argv[0]:junk argv[1]:intf argv[2]:script argv[3]:NULL */
/* We need to make space for script argument: */
argv[0] = argv[1];
argv[1] = argv[2];
/* Now: argv[0]:intf argv[1]:script argv[2]:junk argv[3]:NULL */
#define argv_intf (argv[0])
xsetenv("interface", argv_intf);
// initialize the interface (modprobe, ifup, etc)
if (run(argv, "init", NULL))
return EXIT_FAILURE;
// initialize saddr
// saddr is: { u16 sa_family; u8 sa_data[14]; }
//memset(&saddr, 0, sizeof(saddr));
//TODO: are we leaving sa_family == 0 (AF_UNSPEC)?!
safe_strncpy(saddr.sa_data, argv_intf, sizeof(saddr.sa_data));
// bind to the interface's ARP socket
xbind(sock_fd, &saddr, sizeof(saddr));
// get the interface's ethernet address
//memset(&ifr, 0, sizeof(ifr));
strncpy_IFNAMSIZ(ifr.ifr_name, argv_intf);
xioctl(sock_fd, SIOCGIFHWADDR, &ifr);
memcpy(ð_addr, &ifr.ifr_hwaddr.sa_data, ETH_ALEN);
// start with some stable ip address, either a function of
// the hardware address or else the last address we used.
// we are taking low-order four bytes, as top-order ones
// aren't random enough.
// NOTE: the sequence of addresses we try changes only
// depending on when we detect conflicts.
{
uint32_t t;
move_from_unaligned32(t, ((char *)ð_addr + 2));
srand(t);
}
if (ip.s_addr == 0)
ip.s_addr = pick();
// FIXME cases to handle:
// - zcip already running!
// - link already has local address... just defend/update
// daemonize now; don't delay system startup
if (!FOREGROUND) {
#if BB_MMU
bb_daemonize(0 /*was: DAEMON_CHDIR_ROOT*/);
#endif
if (verbose)
bb_info_msg("start, interface %s", argv_intf);
}
write_pidfile(pidfile);
bb_signals(BB_FATAL_SIGS, cleanup);
// run the dynamic address negotiation protocol,
// restarting after address conflicts:
// - start with some address we want to try
// - short random delay
// - arp probes to see if another host uses it
// - arp announcements that we're claiming it
// - use it
// - defend it, within limits
// exit if:
// - address is successfully obtained and -q was given:
// run "<script> config", then exit with exitcode 0
// - poll error (when does this happen?)
// - read error (when does this happen?)
// - sendto error (in arp()) (when does this happen?)
// - revents & POLLERR (link down). run "<script> deconfig" first
state = PROBE;
while (1) {
struct pollfd fds[1];
unsigned deadline_us;
struct arp_packet p;
int source_ip_conflict;
int target_ip_conflict;
fds[0].fd = sock_fd;
fds[0].events = POLLIN;
fds[0].revents = 0;
// poll, being ready to adjust current timeout
if (!timeout_ms) {
timeout_ms = random_delay_ms(PROBE_WAIT);
// FIXME setsockopt(sock_fd, SO_ATTACH_FILTER, ...) to
// make the kernel filter out all packets except
// ones we'd care about.
}
// set deadline_us to the point in time when we timeout
deadline_us = MONOTONIC_US() + timeout_ms * 1000;
VDBG("...wait %d %s nprobes=%u, nclaims=%u\n",
timeout_ms, argv_intf, nprobes, nclaims);
switch (safe_poll(fds, 1, timeout_ms)) {
default:
//bb_perror_msg("poll"); - done in safe_poll
cleanup(EXIT_FAILURE);
// timeout
case 0:
VDBG("state = %d\n", state);
switch (state) {
case PROBE:
// timeouts in the PROBE state mean no conflicting ARP packets
// have been received, so we can progress through the states
if (nprobes < PROBE_NUM) {
nprobes++;
VDBG("probe/%u %s@%s\n",
nprobes, argv_intf, inet_ntoa(ip));
arp(/* ARPOP_REQUEST, */
/* ð_addr, */ null_ip,
&null_addr, ip);
timeout_ms = PROBE_MIN * 1000;
timeout_ms += random_delay_ms(PROBE_MAX - PROBE_MIN);
}
else {
// Switch to announce state.
state = ANNOUNCE;
nclaims = 0;
VDBG("announce/%u %s@%s\n",
nclaims, argv_intf, inet_ntoa(ip));
arp(/* ARPOP_REQUEST, */
/* ð_addr, */ ip,
ð_addr, ip);
timeout_ms = ANNOUNCE_INTERVAL * 1000;
}
break;
case RATE_LIMIT_PROBE:
// timeouts in the RATE_LIMIT_PROBE state mean no conflicting ARP packets
// have been received, so we can move immediately to the announce state
state = ANNOUNCE;
nclaims = 0;
VDBG("announce/%u %s@%s\n",
nclaims, argv_intf, inet_ntoa(ip));
arp(/* ARPOP_REQUEST, */
/* ð_addr, */ ip,
ð_addr, ip);
timeout_ms = ANNOUNCE_INTERVAL * 1000;
break;
case ANNOUNCE:
// timeouts in the ANNOUNCE state mean no conflicting ARP packets
// have been received, so we can progress through the states
if (nclaims < ANNOUNCE_NUM) {
nclaims++;
VDBG("announce/%u %s@%s\n",
nclaims, argv_intf, inet_ntoa(ip));
arp(/* ARPOP_REQUEST, */
/* ð_addr, */ ip,
ð_addr, ip);
timeout_ms = ANNOUNCE_INTERVAL * 1000;
}
else {
// Switch to monitor state.
state = MONITOR;
// link is ok to use earlier
// FIXME update filters
run(argv, "config", &ip);
ready = 1;
conflicts = 0;
timeout_ms = -1; // Never timeout in the monitor state.
// NOTE: all other exit paths
// should deconfig ...
if (QUIT)
cleanup(EXIT_SUCCESS);
}
break;
case DEFEND:
// We won! No ARP replies, so just go back to monitor.
state = MONITOR;
timeout_ms = -1;
conflicts = 0;
break;
default:
// Invalid, should never happen. Restart the whole protocol.
state = PROBE;
ip.s_addr = pick();
timeout_ms = 0;
nprobes = 0;
nclaims = 0;
break;
} // switch (state)
break; // case 0 (timeout)
// packets arriving, or link went down
case 1:
// We need to adjust the timeout in case we didn't receive
// a conflicting packet.
if (timeout_ms > 0) {
unsigned diff = deadline_us - MONOTONIC_US();
if ((int)(diff) < 0) {
// Current time is greater than the expected timeout time.
// Should never happen.
VDBG("missed an expected timeout\n");
timeout_ms = 0;
} else {
VDBG("adjusting timeout\n");
timeout_ms = (diff / 1000) | 1; /* never 0 */
}
}
if ((fds[0].revents & POLLIN) == 0) {
if (fds[0].revents & POLLERR) {
// FIXME: links routinely go down;
// this shouldn't necessarily exit.
bb_error_msg("iface %s is down", argv_intf);
if (ready) {
run(argv, "deconfig", &ip);
}
cleanup(EXIT_FAILURE);
}
continue;
}
// read ARP packet
if (safe_read(sock_fd, &p, sizeof(p)) < 0) {
bb_perror_msg(bb_msg_read_error);
cleanup(EXIT_FAILURE);
}
if (p.eth.ether_type != htons(ETHERTYPE_ARP))
continue;
#ifdef DEBUG
{
struct ether_addr *sha = (struct ether_addr *) p.arp.arp_sha;
struct ether_addr *tha = (struct ether_addr *) p.arp.arp_tha;
struct in_addr *spa = (struct in_addr *) p.arp.arp_spa;
struct in_addr *tpa = (struct in_addr *) p.arp.arp_tpa;
VDBG("%s recv arp type=%d, op=%d,\n",
argv_intf, ntohs(p.eth.ether_type),
ntohs(p.arp.arp_op));
VDBG("\tsource=%s %s\n",
ether_ntoa(sha),
inet_ntoa(*spa));
VDBG("\ttarget=%s %s\n",
ether_ntoa(tha),
inet_ntoa(*tpa));
}
#endif
if (p.arp.arp_op != htons(ARPOP_REQUEST)
&& p.arp.arp_op != htons(ARPOP_REPLY))
continue;
source_ip_conflict = 0;
target_ip_conflict = 0;
if (memcmp(p.arp.arp_spa, &ip.s_addr, sizeof(struct in_addr)) == 0
&& memcmp(&p.arp.arp_sha, ð_addr, ETH_ALEN) != 0
) {
source_ip_conflict = 1;
}
if (p.arp.arp_op == htons(ARPOP_REQUEST)
&& memcmp(p.arp.arp_tpa, &ip.s_addr, sizeof(struct in_addr)) == 0
&& memcmp(&p.arp.arp_tha, ð_addr, ETH_ALEN) != 0
) {
target_ip_conflict = 1;
}
VDBG("state = %d, source ip conflict = %d, target ip conflict = %d\n",
state, source_ip_conflict, target_ip_conflict);
switch (state) {
case PROBE:
case ANNOUNCE:
// When probing or announcing, check for source IP conflicts
// and other hosts doing ARP probes (target IP conflicts).
if (source_ip_conflict || target_ip_conflict) {
conflicts++;
if (conflicts >= MAX_CONFLICTS) {
VDBG("%s ratelimit\n", argv_intf);
timeout_ms = RATE_LIMIT_INTERVAL * 1000;
state = RATE_LIMIT_PROBE;
}
// restart the whole protocol
ip.s_addr = pick();
timeout_ms = 0;
nprobes = 0;
nclaims = 0;
}
break;
case MONITOR:
// If a conflict, we try to defend with a single ARP probe.
if (source_ip_conflict) {
VDBG("monitor conflict -- defending\n");
state = DEFEND;
timeout_ms = DEFEND_INTERVAL * 1000;
arp(/* ARPOP_REQUEST, */
/* ð_addr, */ ip,
ð_addr, ip);
}
break;
case DEFEND:
// Well, we tried. Start over (on conflict).
if (source_ip_conflict) {
state = PROBE;
VDBG("defend conflict -- starting over\n");
ready = 0;
run(argv, "deconfig", &ip);
// restart the whole protocol
ip.s_addr = pick();
timeout_ms = 0;
nprobes = 0;
nclaims = 0;
}
break;
default:
// Invalid, should never happen. Restart the whole protocol.
VDBG("invalid state -- starting over\n");
state = PROBE;
ip.s_addr = pick();
timeout_ms = 0;
nprobes = 0;
nclaims = 0;
break;
} // switch state
break; // case 1 (packets arriving)
} // switch poll
} // while (1)
#undef argv_intf
}
static int avdtp_write(struct bluetooth_data *data)
{
int ret = 0;
struct rtp_header *header;
struct rtp_payload *payload;
uint64_t now;
long duration = data->frame_duration * data->frame_count;
#ifdef ENABLE_TIMING
uint64_t begin, end, begin2, end2;
begin = get_microseconds();
#endif
header = (struct rtp_header *)data->buffer;
payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
payload->frame_count = data->frame_count;
header->v = 2;
header->pt = 1;
header->sequence_number = htons(data->seq_num);
header->timestamp = htonl(data->nsamples);
header->ssrc = htonl(1);
data->stream.revents = 0;
#ifdef ENABLE_TIMING
begin2 = get_microseconds();
#endif
ret = poll(&data->stream, 1, POLL_TIMEOUT);
#ifdef ENABLE_TIMING
end2 = get_microseconds();
print_time("poll Time Taken", begin2, end2);
#endif
if (ret == 1 && data->stream.revents == POLLOUT) {
long ahead = 0;
now = get_microseconds();
if (data->next_write) {
ahead = data->next_write - now;
#ifdef ENABLE_TIMING
DBG("duration: %ld, ahead: %ld", duration, ahead);
#endif
if (ahead > 0) {
/* too fast, need to throttle */
usleep(ahead);
}
} else {
data->next_write = now;
}
//DBG("duration: %ld, ahead: %ld", duration, ahead);
//DBG("decrease_bitpool %d",decrease_bitpool);
if(ahead <= -30*1000){
decrease_bitpool++;
if(decrease_bitpool > 2 ) {
DBG("duration: %ld, ahead: %ld", duration, ahead);
VDBG("calibrating Bitpool Decrease");
calibrate_bitpool(data,0);
decrease_bitpool = 0;
avdtp_good_counter=0;
}
} else if(ahead >= 10000){
if(data->sbc.bitpool != data->sbc_capabilities.max_bitpool){
avdtp_good_counter++;
if(avdtp_good_counter > 1000){//about 10 secs
DBG("duration: %ld, ahead: %ld avdtp_good_counter: %ld", duration, ahead, avdtp_good_counter);
calibrate_bitpool(data,1);
skip_avdtp_flow_check=0;
}
}
decrease_bitpool = 0;
}
if (ahead <= -CATCH_UP_TIMEOUT * 1000) {
/* fallen too far behind, don't try to catch up */
DBG("ahead < %d, reseting next_write timestamp", -CATCH_UP_TIMEOUT * 1000);
data->next_write = 0;
} else {
data->next_write += duration;
}
#ifdef ENABLE_TIMING
begin2 = get_microseconds();
#endif
pthread_mutex_lock(&data->lock);
if (data->stream.fd == -1) {
pthread_mutex_unlock(&data->lock);
return EBADF;
} else {
ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL);
}
pthread_mutex_unlock(&data->lock);
#ifdef ENABLE_TIMING
end2 = get_microseconds();
print_time("send", begin2, end2);
#endif
if (ret < 0) {
/* can happen during normal remote disconnect */
VDBG("send() failed: %d (errno %s)", ret, strerror(errno));
}
if (ret == -EPIPE) {
bluetooth_close(data);
}
} else {
/* can happen during normal remote disconnect */
VDBG("poll() failed: %d (revents = %d, errno %s)",
ret, data->stream.revents, strerror(errno));
data->next_write = 0;
}
/* Reset buffer of data to send */
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
data->frame_count = 0;
data->samples = 0;
data->seq_num++;
#ifdef ENABLE_TIMING
end = get_microseconds();
print_time("avdtp_write", begin, end);
#endif
return 0; /* always return success */
}
static void rx_complete(struct usb_ep *ep, struct usb_request *req)
{
struct sk_buff *skb = req->context;
struct eth_dev *dev = ep->driver_data;
int status = req->status;
bool queue = 0;
switch (status) {
/* normal completion */
case 0:
skb_put(skb, req->actual);
if (dev->unwrap) {
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (dev->port_usb) {
status = dev->unwrap(dev->port_usb,
skb,
&dev->rx_frames);
if (status == -EINVAL)
dev->net->stats.rx_errors++;
else if (status == -EOVERFLOW)
dev->net->stats.rx_over_errors++;
} else {
dev_kfree_skb_any(skb);
status = -ENOTCONN;
}
spin_unlock_irqrestore(&dev->lock, flags);
} else {
skb_queue_tail(&dev->rx_frames, skb);
}
if (!status)
queue = 1;
break;
/* software-driven interface shutdown */
case -ECONNRESET: /* unlink */
case -ESHUTDOWN: /* disconnect etc */
VDBG(dev, "rx shutdown, code %d\n", status);
goto quiesce;
/* for hardware automagic (such as pxa) */
case -ECONNABORTED: /* endpoint reset */
DBG(dev, "rx %s reset\n", ep->name);
defer_kevent(dev, WORK_RX_MEMORY);
quiesce:
dev_kfree_skb_any(skb);
goto clean;
/* data overrun */
case -EOVERFLOW:
dev->net->stats.rx_over_errors++;
/* FALLTHROUGH */
default:
queue = 1;
dev_kfree_skb_any(skb);
dev->net->stats.rx_errors++;
DBG(dev, "rx status %d\n", status);
break;
}
clean:
spin_lock(&dev->req_lock);
list_add(&req->list, &dev->rx_reqs);
spin_unlock(&dev->req_lock);
if (queue)
queue_work(uether_wq, &dev->rx_work);
}
/* Called when entering a state */
static int otg_set_state(struct otg_fsm *fsm, enum usb_otg_state new_state)
{
state_changed = 1;
if (fsm->otg->phy->state == new_state)
return 0;
VDBG("Set state: %s\n", usb_otg_state_string(new_state));
otg_leave_state(fsm, fsm->otg->phy->state);
switch (new_state) {
case OTG_STATE_B_IDLE:
otg_drv_vbus(fsm, 0);
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
/*
* Driver is responsible for starting ADP probing
* if ADP sensing times out.
*/
otg_start_adp_sns(fsm);
otg_set_protocol(fsm, PROTO_UNDEF);
otg_add_timer(fsm, B_SE0_SRP);
break;
case OTG_STATE_B_SRP_INIT:
otg_start_pulse(fsm);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_UNDEF);
otg_add_timer(fsm, B_SRP_FAIL);
break;
case OTG_STATE_B_PERIPHERAL:
otg_chrg_vbus(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_GADGET);
otg_loc_conn(fsm, 1);
break;
case OTG_STATE_B_WAIT_ACON:
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, B_ASE0_BRST);
fsm->a_bus_suspend = 0;
break;
case OTG_STATE_B_HOST:
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 1);
otg_set_protocol(fsm, PROTO_HOST);
usb_bus_start_enum(fsm->otg->host,
fsm->otg->host->otg_port);
break;
case OTG_STATE_A_IDLE:
otg_drv_vbus(fsm, 0);
otg_chrg_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_start_adp_prb(fsm);
otg_set_protocol(fsm, PROTO_HOST);
break;
case OTG_STATE_A_WAIT_VRISE:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, A_WAIT_VRISE);
break;
case OTG_STATE_A_WAIT_BCON:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, A_WAIT_BCON);
break;
case OTG_STATE_A_HOST:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 1);
otg_set_protocol(fsm, PROTO_HOST);
/*
* When HNP is triggered while a_bus_req = 0, a_host will
* suspend too fast to complete a_set_b_hnp_en
*/
if (!fsm->a_bus_req || fsm->a_suspend_req_inf)
otg_add_timer(fsm, A_WAIT_ENUM);
break;
case OTG_STATE_A_SUSPEND:
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, A_AIDL_BDIS);
break;
case OTG_STATE_A_PERIPHERAL:
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_GADGET);
otg_drv_vbus(fsm, 1);
otg_loc_conn(fsm, 1);
otg_add_timer(fsm, A_BIDL_ADIS);
break;
case OTG_STATE_A_WAIT_VFALL:
otg_drv_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_HOST);
otg_add_timer(fsm, A_WAIT_VFALL);
break;
case OTG_STATE_A_VBUS_ERR:
otg_drv_vbus(fsm, 0);
otg_loc_conn(fsm, 0);
otg_loc_sof(fsm, 0);
otg_set_protocol(fsm, PROTO_UNDEF);
break;
default:
break;
}
fsm->otg->phy->state = new_state;
return 0;
}
int a2dp_write(a2dpData d, const void* buffer, int count)
{
struct bluetooth_data* data = (struct bluetooth_data*)d;
uint8_t* src = (uint8_t *)buffer;
int codesize;
int err, ret = 0;
long frames_left = count;
int encoded;
unsigned int written;
const char *buff;
int did_configure = 0;
#ifdef ENABLE_TIMING
uint64_t begin, end;
DBG("********** a2dp_write **********");
begin = get_microseconds();
#endif
err = wait_for_start(data, WRITE_TIMEOUT);
if (err < 0)
return err;
codesize = data->codesize;
while (frames_left >= codesize) {
/* Enough data to encode (sbc wants 512 byte blocks) */
encoded = sbc_encode(&(data->sbc), src, codesize,
data->buffer + data->count,
sizeof(data->buffer) - data->count,
&written);
if (encoded <= 0) {
ERR("Encoding error %d", encoded);
goto done;
}
VDBG("sbc_encode returned %d, codesize: %d, written: %d\n",
encoded, codesize, written);
src += encoded;
data->count += written;
data->frame_count++;
data->samples += encoded;
data->nsamples += encoded;
/* No space left for another frame then send */
if ((data->count + written >= data->link_mtu) ||
(data->count + written >= BUFFER_SIZE)) {
VDBG("sending packet %d, count %d, link_mtu %u",
data->seq_num, data->count,
data->link_mtu);
err = avdtp_write(data);
if (err < 0)
return err;
}
ret += encoded;
frames_left -= encoded;
}
if (frames_left > 0)
ERR("%ld bytes left at end of a2dp_write\n", frames_left);
done:
#ifdef ENABLE_TIMING
end = get_microseconds();
print_time("a2dp_write total", begin, end);
#endif
return ret;
}
/* State change judgement */
int otg_statemachine(struct otg_fsm *fsm)
{
enum usb_otg_state state;
unsigned long flags;
spin_lock_irqsave(&fsm->lock, flags);
state = fsm->transceiver->state;
state_changed = 0;
/* State machine state change judgement */
VDBG("top: curr state=%s", state_string(state));
switch (state) {
case OTG_STATE_UNDEFINED:
VDBG("fsm->id = %d", fsm->id);
if (fsm->id)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else
otg_set_state(fsm, OTG_STATE_A_IDLE);
break;
case OTG_STATE_B_IDLE:
VDBG("gadget: %p", fsm->transceiver->gadget);
if (!fsm->id)
otg_set_state(fsm, OTG_STATE_A_IDLE);
else if (fsm->b_sess_vld && fsm->transceiver->gadget)
otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
else if (fsm->b_bus_req && fsm->b_sess_end && fsm->b_se0_srp)
otg_set_state(fsm, OTG_STATE_B_SRP_INIT);
break;
case OTG_STATE_B_SRP_INIT:
if (!fsm->id || fsm->b_srp_done)
otg_set_state(fsm, OTG_STATE_B_IDLE);
break;
case OTG_STATE_B_PERIPHERAL:
if (!fsm->id || !fsm->b_sess_vld)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (fsm->b_bus_req &&
fsm->transceiver->gadget->b_hnp_enable &&
fsm->a_bus_suspend)
otg_set_state(fsm, OTG_STATE_B_WAIT_ACON);
break;
case OTG_STATE_B_WAIT_ACON:
if (fsm->a_conn)
otg_set_state(fsm, OTG_STATE_B_HOST);
else if (!fsm->id || !fsm->b_sess_vld)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (fsm->a_bus_resume || fsm->b_ase0_brst_tmout) {
fsm->b_ase0_brst_tmout = 0;
otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
}
break;
case OTG_STATE_B_HOST:
if (!fsm->id || !fsm->b_sess_vld)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (!fsm->b_bus_req || !fsm->a_conn)
otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
break;
case OTG_STATE_A_IDLE:
if (fsm->id)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (!fsm->a_bus_drop && (fsm->a_bus_req || fsm->a_srp_det))
otg_set_state(fsm, OTG_STATE_A_WAIT_VRISE);
break;
case OTG_STATE_A_WAIT_VRISE:
if (fsm->id || fsm->a_bus_drop || fsm->a_vbus_vld ||
fsm->a_wait_vrise_tmout) {
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
}
break;
case OTG_STATE_A_WAIT_BCON:
if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
else if (fsm->b_conn)
otg_set_state(fsm, OTG_STATE_A_HOST);
else if (fsm->id | fsm->a_bus_drop | fsm->a_wait_bcon_tmout)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
break;
case OTG_STATE_A_HOST:
if ((!fsm->a_bus_req || fsm->a_suspend_req) &&
fsm->transceiver->host->b_hnp_enable)
otg_set_state(fsm, OTG_STATE_A_SUSPEND);
else if (fsm->id || !fsm->b_conn || fsm->a_bus_drop)
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
else if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
break;
case OTG_STATE_A_SUSPEND:
if (!fsm->b_conn && fsm->transceiver->host->b_hnp_enable)
otg_set_state(fsm, OTG_STATE_A_PERIPHERAL);
else if (!fsm->b_conn && !fsm->transceiver->host->b_hnp_enable)
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
else if (fsm->a_bus_req || fsm->b_bus_resume)
otg_set_state(fsm, OTG_STATE_A_HOST);
else if (fsm->id || fsm->a_bus_drop || fsm->a_aidl_bdis_tmout)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
else if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
break;
case OTG_STATE_A_PERIPHERAL:
if (fsm->id || fsm->a_bus_drop)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
else if (fsm->b_bus_suspend)
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
else if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
break;
case OTG_STATE_A_WAIT_VFALL:
if (fsm->id || fsm->a_bus_req ||
(!fsm->a_sess_vld && !fsm->b_conn))
otg_set_state(fsm, OTG_STATE_A_IDLE);
break;
case OTG_STATE_A_VBUS_ERR:
if (fsm->id || fsm->a_bus_drop || fsm->a_clr_err)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
break;
default:
break;
}
spin_unlock_irqrestore(&fsm->lock, flags);
return state_changed;
}
/*
* The setup() callback implements all the ep0 functionality that's
* not handled lower down, in hardware or the hardware driver (like
* device and endpoint feature flags, and their status). It's all
* housekeeping for the gadget function we're implementing. Most of
* the work is in config-specific setup.
*/
static int psfreedom_setup(struct usb_gadget *gadget,
const struct usb_ctrlrequest *ctrl)
{
struct psfreedom_device *dev = get_gadget_data(gadget);
struct usb_request *req = dev->req;
int value = -EOPNOTSUPP;
u16 w_index = le16_to_cpu(ctrl->wIndex);
u16 w_value = le16_to_cpu(ctrl->wValue);
u16 w_length = le16_to_cpu(ctrl->wLength);
u8 address = psfreedom_get_address (dev->gadget);
unsigned long flags;
u16 request = (ctrl->bRequestType << 8) | ctrl->bRequest;
spin_lock_irqsave (&dev->lock, flags);
VDBG (dev, "Setup called %d (%d) -- %d -- %d. Myaddr :%d\n", ctrl->bRequest,
ctrl->bRequestType, w_value, w_index, address);
req->zero = 0;
/* Enable the timer if it's not already enabled */
if (timer_added == 0)
add_timer (&psfreedom_state_machine_timer);
timer_added = 1;
/* Set the address of the port */
if (address)
dev->port_address[dev->current_port] = address;
/* Setup the hub or the devices */
if (dev->current_port == 0)
value = hub_setup (gadget, ctrl, request, w_index, w_value, w_length);
else
value = devices_setup (gadget, ctrl, request, w_index, w_value, w_length);
#ifdef NO_DELAYED_PORT_SWITCHING
if (dev->switch_to_port_delayed >= 0)
switch_to_port (dev, dev->switch_to_port_delayed);
dev->switch_to_port_delayed = -1;
#endif
DBG (dev, "%s Setup called %s (%d - %d) -> %d (w_length=%d)\n",
STATUS_STR (dev->status), REQUEST_STR (request), w_value, w_index,
value, w_length);
/* respond with data transfer before status phase? */
if (value >= 0) {
req->length = value;
req->zero = value < w_length;
value = usb_ep_queue(gadget->ep0, req, GFP_ATOMIC);
if (value < 0) {
DBG(dev, "ep_queue --> %d\n", value);
req->status = 0;
spin_unlock_irqrestore (&dev->lock, flags);
psfreedom_setup_complete(gadget->ep0, req);
return value;
}
}
spin_unlock_irqrestore (&dev->lock, flags);
/* device either stalls (value < 0) or reports success */
return value;
}
/*
* The setup() callback implements all the ep0 functionality that's
* not handled lower down, in hardware or the hardware driver (like
* device and endpoint feature flags, and their status). It's all
* housekeeping for the gadget function we're implementing. Most of
* the work is in config-specific setup.
*/
static int hub_setup(struct usb_gadget *gadget,
const struct usb_ctrlrequest *ctrl, u16 request,
u16 w_index, u16 w_value, u16 w_length)
{
struct psfreedom_device *dev = get_gadget_data(gadget);
struct usb_request *req = dev->req;
int value = -EOPNOTSUPP;
/* usually this stores reply data in the pre-allocated ep0 buffer,
* but config change events will reconfigure hardware.
*/
switch (ctrl->bRequest) {
case USB_REQ_GET_DESCRIPTOR:
if ((ctrl->bRequestType & USB_DIR_IN) == 0) {
goto unknown;
}
if ((ctrl->bRequestType & USB_TYPE_CLASS) == USB_TYPE_CLASS) {
/* GET_HUB_DESCRIPTOR Class specific request */
value = min(w_length, (u16) sizeof(hub_header_desc));
memcpy(req->buf, &hub_header_desc, value);
if (value >= 0)
value = min(w_length, (u16)value);
} else {
switch (w_value >> 8) {
case USB_DT_DEVICE:
value = min(w_length, (u16) sizeof(hub_device_desc));
memcpy(req->buf, &hub_device_desc, value);
break;
case USB_DT_CONFIG:
memcpy(req->buf, &hub_config_desc, sizeof(hub_config_desc));
value = sizeof(hub_config_desc);
memcpy (req->buf + value, &hub_interface_desc,
sizeof(hub_interface_desc));
value += sizeof(hub_interface_desc);
memcpy (req->buf + value, &hub_endpoint_desc,
sizeof(hub_endpoint_desc));
value += sizeof(hub_endpoint_desc);
if (value >= 0)
value = min(w_length, (u16)value);
break;
case USB_DT_STRING:
value = 0;
break;
}
}
break;
case USB_REQ_SET_CONFIGURATION:
if (ctrl->bRequestType != 0) {
goto unknown;
}
value = hub_set_config(dev, w_value);
break;
case USB_REQ_GET_CONFIGURATION:
if (ctrl->bRequestType != USB_DIR_IN) {
goto unknown;
}
*(u8 *)req->buf = 0;
value = min(w_length, (u16)1);
break;
case USB_REQ_SET_INTERFACE:
if (ctrl->bRequestType != USB_RECIP_INTERFACE) {
goto unknown;
}
value = 0;
break;
case USB_REQ_GET_INTERFACE:
if (ctrl->bRequestType != (USB_DIR_IN|USB_RECIP_INTERFACE)) {
goto unknown;
}
if (w_index >= 1) {
value = -EDOM;
break;
}
*(u8 *)req->buf = 0;
value = min(w_length, (u16)1);
break;
case USB_REQ_SET_FEATURE:
if ((ctrl->bRequestType & USB_TYPE_CLASS) == USB_TYPE_CLASS) {
switch (ctrl->bRequestType & USB_RECIP_MASK) {
/* SET_HUB_FEATURE */
case USB_RECIP_DEVICE:
switch (w_value) {
case 0: /* C_HUB_LOCAL_POWER */
case 1: /* C_HUB_OVER_CURRENT */
VDBG (dev, "SetHubFeature called\n");
value = 0;
break;
default:
value = -EINVAL;
break;
}
break;
case USB_RECIP_OTHER:
/* SET_PORT_FEATURE */
if (w_index == 0 || w_index > 6) {
DBG (dev, "SetPortFeature: invalid port index %d\n", w_index);
value = -EINVAL;
break;
}
switch (w_value) {
case 4: /* PORT_RESET */
DBG (dev, "SetPortFeature PORT_RESET called\n");
dev->hub_ports[w_index-1].change |= PORT_STAT_C_RESET;
hub_port_changed (dev);
value = 0;
break;
case 8: /* PORT_POWER */
DBG (dev, "SetPortFeature PORT_POWER called\n");
dev->hub_ports[w_index-1].status |= PORT_STAT_POWER;
if (dev->status == INIT && w_index == 6) {
dev->status = HUB_READY;
SET_TIMER (150);
}
value = 0;
break;
case 0: /* PORT_CONNECTION */
case 1: /* PORT_ENABLE */
case 2: /* PORT_SUSPEND */
case 3: /* PORT_OVER_CURRENT */
case 9: /* PORT_LOW_SPEED */
case 16: /* C_PORT_CONNECTION */
case 17: /* C_PORT_ENABLE */
case 18: /* C_PORT_SUSPEND */
case 19: /* C_PORT_OVER_CURRENT */
case 20: /* C_PORT_RESET */
case 21: /* PORT_TEST */
case 22: /* PORT_INDICATOR */
DBG (dev, "SetPortFeature called\n");
value = 0;
break;
default:
value = -EINVAL;
break;
}
break;
}
}
break;
case USB_REQ_CLEAR_FEATURE:
if ((ctrl->bRequestType & USB_TYPE_CLASS) == USB_TYPE_CLASS) {
switch (ctrl->bRequestType & USB_RECIP_MASK) {
/* CLEAR_HUB_FEATURE */
case USB_RECIP_DEVICE:
switch (w_value) {
case 0: /* C_HUB_LOCAL_POWER */
case 1: /* C_HUB_OVER_CURRENT */
VDBG (dev, "ClearHubFeature called\n");
value = 0;
break;
default:
value = -EINVAL;
break;
}
break;
case USB_RECIP_OTHER:
/* CLEAR_PORT_FEATURE */
if (w_index == 0 || w_index > 6) {
DBG (dev, "ClearPortFeature: invalid port index %d\n", w_index);
value = -EINVAL;
break;
}
switch (w_value) {
case 0: /* PORT_CONNECTION */
case 1: /* PORT_ENABLE */
case 2: /* PORT_SUSPEND */
case 3: /* PORT_OVER_CURRENT */
case 4: /* PORT_RESET */
case 8: /* PORT_POWER */
case 9: /* PORT_LOW_SPEED */
value = 0;
break;
case 16: /* C_PORT_CONNECTION */
DBG (dev, "ClearPortFeature C_PORT_CONNECTION called\n");
dev->hub_ports[w_index-1].change &= ~PORT_STAT_C_CONNECTION;
switch (dev->status) {
case DEVICE1_WAIT_DISCONNECT:
dev->status = DEVICE1_DISCONNECTED;
SET_TIMER (200);
break;
case DEVICE2_WAIT_DISCONNECT:
dev->status = DEVICE2_DISCONNECTED;
SET_TIMER (170);
break;
case DEVICE3_WAIT_DISCONNECT:
dev->status = DEVICE3_DISCONNECTED;
SET_TIMER (450);
break;
case DEVICE4_WAIT_DISCONNECT:
dev->status = DEVICE4_DISCONNECTED;
SET_TIMER (200);
break;
case DEVICE5_WAIT_DISCONNECT:
dev->status = DEVICE5_DISCONNECTED;
SET_TIMER (200);
break;
default:
break;
}
value = 0;
break;
case 20: /* C_PORT_RESET */
DBG (dev, "ClearPortFeature C_PORT_RESET called\n");
dev->hub_ports[w_index-1].change &= ~PORT_STAT_C_RESET;
switch (dev->status) {
case DEVICE1_WAIT_READY:
if (w_index == 1)
dev->switch_to_port_delayed = w_index;
break;
case DEVICE2_WAIT_READY:
if (w_index == 2)
dev->switch_to_port_delayed = w_index;
break;
case DEVICE3_WAIT_READY:
if (w_index == 3)
dev->switch_to_port_delayed = w_index;
break;
case DEVICE4_WAIT_READY:
if (w_index == 4)
dev->switch_to_port_delayed = w_index;
break;
case DEVICE5_WAIT_READY:
if (w_index == 5)
dev->switch_to_port_delayed = w_index;
break;
case DEVICE6_WAIT_READY:
if (w_index == 6)
dev->switch_to_port_delayed = w_index;
break;
default:
break;
}
/* Delay switching the port because we first need to response
to this request with the proper address */
if (dev->switch_to_port_delayed >= 0)
SET_TIMER (0);
value = 0;
break;
case 17: /* C_PORT_ENABLE */
case 18: /* C_PORT_SUSPEND */
case 19: /* C_PORT_OVER_CURRENT */
case 21: /* PORT_TEST */
case 22: /* PORT_INDICATOR */
DBG (dev, "ClearPortFeature called\n");
value = 0;
break;
default:
value = -EINVAL;
break;
}
break;
}
}
break;
case USB_REQ_GET_STATUS:
if ((ctrl->bRequestType & USB_TYPE_CLASS) == USB_TYPE_CLASS) {
u16 status = 0;
u16 change = 0;
value = 2 * sizeof (u16);
switch (ctrl->bRequestType & USB_RECIP_MASK) {
case USB_RECIP_DEVICE:
/* GET_HUB_STATUS */
status = 0;
change = 0;
break;
case USB_RECIP_OTHER:
/* GET_PORT_STATUS */
if (w_index == 0 || w_index > 6) {
DBG (dev, "GetPortstatus : invalid port index %d\n", w_index);
value = -EINVAL;
break;
}
status = dev->hub_ports[w_index -1].status;
change = dev->hub_ports[w_index -1].change;
break;
default:
goto unknown;
}
if (value > 0) {
DBG (dev, "GetHub/PortStatus: transmtiting status %d change %d\n",
status, change);
status = cpu_to_le16 (status);
change = cpu_to_le16 (change);
memcpy(req->buf, &status, sizeof(u16));
memcpy(req->buf + sizeof(u16), &change, sizeof(u16));
}
}
break;
default:
unknown:
ERROR (dev, "unknown control req%02x.%02x v%04x i%04x l%d\n",
ctrl->bRequestType, ctrl->bRequest,
w_value, w_index, w_length);
}
/* device either stalls (value < 0) or reports success */
return value;
}
/* State change judgement */
int otg_statemachine(struct otg_fsm *fsm)
{
enum usb_otg_state state;
mutex_lock(&fsm->lock);
state = fsm->otg->phy->state;
state_changed = 0;
/* State machine state change judgement */
switch (state) {
case OTG_STATE_UNDEFINED:
VDBG("fsm->id = %d\n", fsm->id);
if (fsm->id)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else
otg_set_state(fsm, OTG_STATE_A_IDLE);
break;
case OTG_STATE_B_IDLE:
if (!fsm->id)
otg_set_state(fsm, OTG_STATE_A_IDLE);
else if (fsm->b_sess_vld && fsm->otg->gadget)
otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
else if ((fsm->b_bus_req || fsm->adp_change || fsm->power_up) &&
fsm->b_ssend_srp && fsm->b_se0_srp)
otg_set_state(fsm, OTG_STATE_B_SRP_INIT);
break;
case OTG_STATE_B_SRP_INIT:
if (!fsm->id || fsm->b_srp_done)
otg_set_state(fsm, OTG_STATE_B_IDLE);
break;
case OTG_STATE_B_PERIPHERAL:
if (!fsm->id || !fsm->b_sess_vld)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (fsm->b_bus_req && fsm->otg->
gadget->b_hnp_enable && fsm->a_bus_suspend)
otg_set_state(fsm, OTG_STATE_B_WAIT_ACON);
break;
case OTG_STATE_B_WAIT_ACON:
if (fsm->a_conn)
otg_set_state(fsm, OTG_STATE_B_HOST);
else if (!fsm->id || !fsm->b_sess_vld)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (fsm->a_bus_resume || fsm->b_ase0_brst_tmout) {
fsm->b_ase0_brst_tmout = 0;
otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
}
break;
case OTG_STATE_B_HOST:
if (!fsm->id || !fsm->b_sess_vld)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (!fsm->b_bus_req || !fsm->a_conn || fsm->test_device)
otg_set_state(fsm, OTG_STATE_B_PERIPHERAL);
break;
case OTG_STATE_A_IDLE:
if (fsm->id)
otg_set_state(fsm, OTG_STATE_B_IDLE);
else if (!fsm->a_bus_drop && (fsm->a_bus_req ||
fsm->a_srp_det || fsm->adp_change || fsm->power_up))
otg_set_state(fsm, OTG_STATE_A_WAIT_VRISE);
break;
case OTG_STATE_A_WAIT_VRISE:
if (fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
else if (fsm->id || fsm->a_bus_drop ||
fsm->a_wait_vrise_tmout)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
break;
case OTG_STATE_A_WAIT_BCON:
if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
else if (fsm->b_conn)
otg_set_state(fsm, OTG_STATE_A_HOST);
else if (fsm->id || fsm->a_bus_drop || fsm->a_wait_bcon_tmout)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
break;
case OTG_STATE_A_HOST:
if (fsm->id || fsm->a_bus_drop)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
else if ((!fsm->a_bus_req || fsm->a_suspend_req_inf) &&
fsm->otg->host->b_hnp_enable)
otg_set_state(fsm, OTG_STATE_A_SUSPEND);
else if (!fsm->b_conn)
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
else if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
break;
case OTG_STATE_A_SUSPEND:
if (!fsm->b_conn && fsm->otg->host->b_hnp_enable)
otg_set_state(fsm, OTG_STATE_A_PERIPHERAL);
else if (!fsm->b_conn && !fsm->otg->host->b_hnp_enable)
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
else if (fsm->a_bus_req || fsm->b_bus_resume)
otg_set_state(fsm, OTG_STATE_A_HOST);
else if (fsm->id || fsm->a_bus_drop || fsm->a_aidl_bdis_tmout)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
else if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
break;
case OTG_STATE_A_PERIPHERAL:
if (fsm->id || fsm->a_bus_drop)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
else if (fsm->a_bidl_adis_tmout || fsm->b_bus_suspend)
otg_set_state(fsm, OTG_STATE_A_WAIT_BCON);
else if (!fsm->a_vbus_vld)
otg_set_state(fsm, OTG_STATE_A_VBUS_ERR);
break;
case OTG_STATE_A_WAIT_VFALL:
if (fsm->a_wait_vfall_tmout)
otg_set_state(fsm, OTG_STATE_A_IDLE);
break;
case OTG_STATE_A_VBUS_ERR:
if (fsm->id || fsm->a_bus_drop || fsm->a_clr_err)
otg_set_state(fsm, OTG_STATE_A_WAIT_VFALL);
break;
default:
break;
}
mutex_unlock(&fsm->lock);
VDBG("quit statemachine, changed = %d\n", state_changed);
return state_changed;
}
static int acc_function_setup(struct usb_function *f,
const struct usb_ctrlrequest *ctrl)
{
struct acc_dev *dev = func_to_dev(f);
struct usb_composite_dev *cdev = dev->cdev;
int value = -EOPNOTSUPP;
u8 b_requestType = ctrl->bRequestType;
u8 b_request = ctrl->bRequest;
u16 w_index = le16_to_cpu(ctrl->wIndex);
u16 w_value = le16_to_cpu(ctrl->wValue);
u16 w_length = le16_to_cpu(ctrl->wLength);
/*
printk(KERN_INFO "acc_function_setup "
"%02x.%02x v%04x i%04x l%u\n",
b_requestType, b_request,
w_value, w_index, w_length);
*/
if (dev->function.hidden) {
if (b_requestType == (USB_DIR_OUT | USB_TYPE_VENDOR)) {
if (b_request == ACCESSORY_START) {
schedule_delayed_work(
&dev->work, msecs_to_jiffies(10));
value = 0;
} else if (b_request == ACCESSORY_SEND_STRING) {
dev->string_index = w_index;
cdev->gadget->ep0->driver_data = dev;
cdev->req->complete = acc_complete_set_string;
value = w_length;
}
} else if (b_requestType == (USB_DIR_IN | USB_TYPE_VENDOR)) {
if (b_request == ACCESSORY_GET_PROTOCOL) {
*((u16 *)cdev->req->buf) = PROTOCOL_VERSION;
value = sizeof(u16);
/* clear any strings left over from a previous session */
memset(dev->manufacturer, 0, sizeof(dev->manufacturer));
memset(dev->model, 0, sizeof(dev->model));
memset(dev->description, 0, sizeof(dev->description));
memset(dev->version, 0, sizeof(dev->version));
memset(dev->uri, 0, sizeof(dev->uri));
memset(dev->serial, 0, sizeof(dev->serial));
}
}
}
if (value >= 0) {
cdev->req->zero = 0;
cdev->req->length = value;
value = usb_ep_queue(cdev->gadget->ep0, cdev->req, GFP_ATOMIC);
if (value < 0)
ERROR(cdev, "%s setup response queue error\n",
__func__);
}
if (value == -EOPNOTSUPP)
VDBG(cdev,
"(%s) unknown class-specific control req "
"%02x.%02x v%04x i%04x l%u\n", f->name,
ctrl->bRequestType, ctrl->bRequest,
w_value, w_index, w_length);
return value;
}
static int read_fifo(struct goku_ep *ep, struct goku_request *req)
{
struct goku_udc_regs __iomem *regs;
u32 size, set;
u8 *buf;
unsigned bufferspace, is_short, dbuff;
regs = ep->dev->regs;
top:
buf = req->req.buf + req->req.actual;
prefetchw(buf);
if (unlikely(ep->num == 0 && ep->dev->ep0state != EP0_OUT))
return -EL2HLT;
dbuff = (ep->num == 1 || ep->num == 2);
do {
/* ack dataset irq matching the status we'll handle */
if (ep->num != 0)
writel(~INT_EPxDATASET(ep->num), ®s->int_status);
set = readl(®s->DataSet) & DATASET_AB(ep->num);
size = readl(®s->EPxSizeLA[ep->num]);
bufferspace = req->req.length - req->req.actual;
/* usually do nothing without an OUT packet */
if (likely(ep->num != 0 || bufferspace != 0)) {
if (unlikely(set == 0))
break;
/* use ep1/ep2 double-buffering for OUT */
if (!(size & PACKET_ACTIVE))
size = readl(®s->EPxSizeLB[ep->num]);
if (!(size & PACKET_ACTIVE)) /* "can't happen" */
break;
size &= DATASIZE; /* EPxSizeH == 0 */
/* ep0out no-out-data case for set_config, etc */
} else
size = 0;
/* read all bytes from this packet */
req->req.actual += size;
is_short = (size < ep->ep.maxpacket);
#ifdef USB_TRACE
VDBG(ep->dev, "read %s %u bytes%s OUT req %p %u/%u\n",
ep->ep.name, size, is_short ? "/S" : "",
req, req->req.actual, req->req.length);
#endif
while (likely(size-- != 0)) {
u8 byte = (u8) readl(ep->reg_fifo);
if (unlikely(bufferspace == 0)) {
/* this happens when the driver's buffer
* is smaller than what the host sent.
* discard the extra data in this packet.
*/
if (req->req.status != -EOVERFLOW)
DBG(ep->dev, "%s overflow %u\n",
ep->ep.name, size);
req->req.status = -EOVERFLOW;
} else {
*buf++ = byte;
bufferspace--;
}
}
/* completion */
if (unlikely(is_short || req->req.actual == req->req.length)) {
if (unlikely(ep->num == 0)) {
/* non-control endpoints now usable? */
if (ep->dev->req_config)
writel(ep->dev->configured
? USBSTATE_CONFIGURED
: 0,
®s->UsbState);
/* ep0out status stage */
writel(~(1<<0), ®s->EOP);
ep->stopped = 1;
ep->dev->ep0state = EP0_STATUS;
}
done(ep, req, 0);
/* empty the second buffer asap */
if (dbuff && !list_empty(&ep->queue)) {
req = list_entry(ep->queue.next,
struct goku_request, queue);
goto top;
}
return 1;
}
} while (dbuff);
return 0;
}
static int avdtp_write(struct bluetooth_data *data)
{
int ret = 0;
struct rtp_header *header;
struct rtp_payload *payload;
uint64_t now;
long duration = data->frame_duration * data->frame_count;
#ifdef ENABLE_TIMING
uint64_t begin, end, begin2, end2;
begin = get_microseconds();
#endif
header = (struct rtp_header *)data->buffer;
#ifdef TN_JPN_NTT_BT_SCMS-T
payload = (struct rtp_payload *)(data->buffer + sizeof(*header) + data->sizeof_scms_t);
memset(data->buffer, 0, sizeof(*header) + sizeof(*payload) + data->sizeof_scms_t);
if (data->sizeof_scms_t) {
/*
* In theory, this should be setable on the fly to 0x01 to "protect"
* the stream and 0x00 to allow capture by remote device. In practice,
* all I've ever seem is "protect", or 0x01.
*/
data->buffer[sizeof(*header)] = SCMS_T_COPY_NOT_ALLOWED;
}
#else
payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
#endif
payload->frame_count = data->frame_count;
header->v = 2;
header->pt = 1;
header->sequence_number = htons(data->seq_num);
header->timestamp = htonl(data->nsamples);
header->ssrc = htonl(1);
data->stream.revents = 0;
#ifdef ENABLE_TIMING
begin2 = get_microseconds();
#endif
ret = poll(&data->stream, 1, POLL_TIMEOUT);
#ifdef ENABLE_TIMING
end2 = get_microseconds();
print_time("poll", begin2, end2);
#endif
if (ret == 1 && data->stream.revents == POLLOUT) {
long ahead = 0;
now = get_microseconds();
if (data->next_write) {
ahead = data->next_write - now;
#ifdef ENABLE_TIMING
DBG("duration: %ld, ahead: %ld", duration, ahead);
#endif
if (ahead > 0) {
/* too fast, need to throttle */
usleep(ahead);
}
} else {
data->next_write = now;
}
if (ahead <= -CATCH_UP_TIMEOUT * 1000) {
/* fallen too far behind, don't try to catch up */
VDBG("ahead < %d, reseting next_write timestamp", -CATCH_UP_TIMEOUT * 1000);
data->next_write = 0;
} else {
data->next_write += duration;
}
#ifdef ENABLE_TIMING
begin2 = get_microseconds();
#endif
ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL);
#ifdef ENABLE_TIMING
end2 = get_microseconds();
print_time("send", begin2, end2);
#endif
if (ret < 0) {
/* can happen during normal remote disconnect */
VDBG("send() failed: %d (errno %s)", ret, strerror(errno));
}
if (ret == -EPIPE) {
bluetooth_close(data);
}
} else {
/* can happen during normal remote disconnect */
VDBG("poll() failed: %d (revents = %d, errno %s)",
ret, data->stream.revents, strerror(errno));
data->next_write = 0;
}
/* Reset buffer of data to send */
#ifdef TN_JPN_NTT_BT_SCMS-T
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload) + data->sizeof_scms_t;
#else
data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
#endif
data->frame_count = 0;
data->samples = 0;
data->seq_num++;
#ifdef ENABLE_TIMING
end = get_microseconds();
print_time("avdtp_write", begin, end);
#endif
return 0; /* always return success */
}
static int hidg_setup(struct usb_function *f,
const struct usb_ctrlrequest *ctrl)
{
struct f_hidg *hidg = func_to_hidg(f);
struct usb_composite_dev *cdev = f->config->cdev;
struct usb_request *req = cdev->req;
int status = 0;
__u16 value, length;
value = __le16_to_cpu(ctrl->wValue);
length = __le16_to_cpu(ctrl->wLength);
VDBG(cdev, "hid_setup crtl_request : bRequestType:0x%x bRequest:0x%x "
"Value:0x%x\n", ctrl->bRequestType, ctrl->bRequest, value);
switch ((ctrl->bRequestType << 8) | ctrl->bRequest) {
case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8
| HID_REQ_GET_REPORT):
VDBG(cdev, "get_report\n");
/* send an empty report */
length = min_t(unsigned, length, hidg->report_length);
memset(req->buf, 0x0, length);
goto respond;
break;
case ((USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8
| HID_REQ_GET_PROTOCOL):
VDBG(cdev, "get_protocol\n");
length = min_t(unsigned, length, 1);
if (hidg->bInterfaceSubClass == USB_INTERFACE_SUBCLASS_BOOT)
((u8 *) req->buf)[0] = 0; /* Boot protocol */
else
((u8 *) req->buf)[0] = 1; /* Report protocol */
goto respond;
break;
case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8
| HID_REQ_SET_REPORT):
VDBG(cdev, "set_report | wLenght=%d\n", ctrl->wLength);
req->context = hidg;
req->complete = hidg_set_report_complete;
goto respond;
break;
case ((USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE) << 8
| HID_REQ_SET_PROTOCOL):
VDBG(cdev, "set_protocol\n");
length = 0;
if (hidg->bInterfaceSubClass == USB_INTERFACE_SUBCLASS_BOOT) {
if (value == 0) /* Boot protocol */
goto respond;
} else {
if (value == 1) /* Report protocol */
goto respond;
}
goto stall;
break;
case ((USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_INTERFACE) << 8
| USB_REQ_GET_DESCRIPTOR):
switch (value >> 8) {
case HID_DT_HID:
VDBG(cdev, "USB_REQ_GET_DESCRIPTOR: HID\n");
length = min_t(unsigned short, length,
hidg_desc.bLength);
memcpy(req->buf, &hidg_desc, length);
goto respond;
break;
case HID_DT_REPORT:
VDBG(cdev, "USB_REQ_GET_DESCRIPTOR: REPORT\n");
length = min_t(unsigned short, length,
hidg->report_desc_length);
memcpy(req->buf, hidg->report_desc, length);
goto respond;
break;
default:
VDBG(cdev, "Unknown decriptor request 0x%x\n",
value >> 8);
goto stall;
break;
}
break;
default:
VDBG(cdev, "Unknown request 0x%x\n",
ctrl->bRequest);
goto stall;
break;
}
stall:
return -EOPNOTSUPP;
respond:
req->zero = 0;
req->length = length;
status = usb_ep_queue(cdev->gadget->ep0, req, GFP_ATOMIC);
if (status < 0)
ERROR(cdev, "usb_ep_queue error on ep0 %d\n", value);
return status;
}
static void rx_complete(struct usb_ep *ep, struct usb_request *req)
{
struct sk_buff *skb = req->context, *skb2;
struct eth_dev *dev = ep->driver_data;
int status = req->status;
switch (status) {
/* normal completion */
case 0:
skb_put(skb, req->actual);
if (dev->unwrap) {
unsigned long flags;
spin_lock_irqsave(&dev->lock, flags);
if (dev->port_usb) {
status = dev->unwrap(dev->port_usb,
skb,
&dev->rx_frames);
} else {
dev_kfree_skb_any(skb);
status = -ENOTCONN;
}
spin_unlock_irqrestore(&dev->lock, flags);
} else {
skb_queue_tail(&dev->rx_frames, skb);
}
skb = NULL;
skb2 = skb_dequeue(&dev->rx_frames);
while (skb2) {
if (status < 0
|| ETH_HLEN > skb2->len
|| skb2->len > ETH_FRAME_LEN) {
dev->net->stats.rx_errors++;
dev->net->stats.rx_length_errors++;
DBG(dev, "rx length %d\n", skb2->len);
dev_kfree_skb_any(skb2);
goto next_frame;
}
skb2->protocol = eth_type_trans(skb2, dev->net);
dev->net->stats.rx_packets++;
dev->net->stats.rx_bytes += skb2->len;
/* no buffer copies needed, unless hardware can't
* use skb buffers.
*/
status = netif_rx(skb2);
next_frame:
skb2 = skb_dequeue(&dev->rx_frames);
}
break;
/* software-driven interface shutdown */
case -ECONNRESET: /* unlink */
case -ESHUTDOWN: /* disconnect etc */
VDBG(dev, "rx shutdown, code %d\n", status);
goto quiesce;
/* for hardware automagic (such as pxa) */
case -ECONNABORTED: /* endpoint reset */
DBG(dev, "rx %s reset\n", ep->name);
defer_kevent(dev, WORK_RX_MEMORY);
quiesce:
dev_kfree_skb_any(skb);
goto clean;
/* data overrun */
case -EOVERFLOW:
dev->net->stats.rx_over_errors++;
/* FALLTHROUGH */
default:
dev->net->stats.rx_errors++;
DBG(dev, "rx status %d\n", status);
break;
}
if (skb)
dev_kfree_skb_any(skb);
if (!netif_running(dev->net)) {
clean:
spin_lock(&dev->req_lock);
list_add(&req->list, &dev->rx_reqs);
spin_unlock(&dev->req_lock);
req = NULL;
}
if (req)
rx_submit(dev, req, GFP_ATOMIC);
}
int
DDS_TEST::run(int num_messages, int msg_size)
{
VDBG((LM_DEBUG, "(%P|%t) DBG: "
"Build the DataSampleElementList\n"));
this->num_messages_delivered_ = 0;
this->num_messages_sent_ = num_messages;
// Set up the SendStateDataSampleList
OpenDDS::DCPS::SendStateDataSampleList samples;
samples.size_ = num_messages;
// Now we can create the DataSampleHeader struct and set its fields.
OpenDDS::DCPS::DataSampleHeader header;
// The +1 makes the null terminator ('\0') get placed into the block.
header.message_id_ = 1;
header.publication_id_ = this->pub_id_;
OpenDDS::DCPS::DataSampleElement* prev_element = 0;
OpenDDS::DCPS::DataSampleElementAllocator allocator(num_messages);
Cleanup cleanup(allocator, samples);
OpenDDS::DCPS::TransportSendElementAllocator trans_allocator(num_messages, sizeof (OpenDDS::DCPS::TransportSendElement));
std::string data = "Hello World!";
if (msg_size) {
data.clear();
for (int j = 1; j <= msg_size; ++j) {
data += char(1 + (j % 255));
}
}
for (int i = 1; i <= num_messages; ++i) {
// This is what goes in the "Data Block".
std::ostringstream ostr;
ostr << data << " [" << i << "]";
std::string data_str = msg_size ? data : ostr.str();
ssize_t num_data_bytes = data_str.length() + 1;
header.message_length_ = static_cast<ACE_UINT32>(num_data_bytes);
header.sequence_ = i;
// The DataSampleHeader is what goes in the "Header Block".
ACE_Message_Block* header_block =
new ACE_Message_Block(header.max_marshaled_size());
*header_block << header;
ACE_Message_Block* data_block = new ACE_Message_Block(num_data_bytes);
data_block->copy(data_str.c_str());
// Chain the "Data Block" to the "Header Block"
header_block->cont(data_block);
// Create the DataSampleElement now.
OpenDDS::DCPS::DataSampleElement* element;
ACE_NEW_MALLOC_RETURN(element,
static_cast<OpenDDS::DCPS::DataSampleElement*>(allocator.malloc(sizeof (OpenDDS::DCPS::DataSampleElement))),
OpenDDS::DCPS::DataSampleElement(this->pub_id_, this, 0, &trans_allocator, 0), 1);
// The Sample Element will hold on to the chain of blocks (header + data).
element->sample_ = header_block;
if (prev_element == 0) {
samples.head_ = element;
} else {
prev_element->set_next_send_sample(element);
}
prev_element = element;
samples.tail_ = element;
}
VDBG((LM_DEBUG, "(%P|%t) DBG: "
"Send the SendStateDataSampleList (samples).\n"));
ACE_Time_Value start = ACE_OS::gettimeofday();
this->send(samples);
ACE_Time_Value finished = ACE_OS::gettimeofday();
ACE_Time_Value total = finished - start;
ACE_DEBUG((LM_INFO,
"(%P|%t) Publisher total time required was %d.%d seconds.\n",
total.sec(),
total.usec() % 1000000));
VDBG((LM_DEBUG, "(%P|%t) DBG: "
"The Publisher has finished sending the samples.\n"));
if (msg_size) {
ACE_OS::sleep(15);
}
return 0;
}
static void tx_complete(struct usb_ep *ep, struct usb_request *req)
{
struct sk_buff *skb;
struct eth_dev *dev;
struct net_device *net;
struct usb_request *new_req;
struct usb_ep *in;
int length;
int retval;
if (!ep->driver_data) {
usb_ep_free_request(ep, req);
return;
}
dev = ep->driver_data;
net = dev->net;
if (!dev->port_usb) {
usb_ep_free_request(ep, req);
return;
}
switch (req->status) {
default:
dev->net->stats.tx_errors++;
VDBG(dev, "tx err %d\n", req->status);
/* FALLTHROUGH */
case -ECONNRESET: /* unlink */
case -ESHUTDOWN: /* disconnect etc */
break;
case 0:
if (!req->zero)
dev->net->stats.tx_bytes += req->length-1;
else
dev->net->stats.tx_bytes += req->length;
}
dev->net->stats.tx_packets++;
spin_lock(&dev->req_lock);
list_add_tail(&req->list, &dev->tx_reqs);
if (dev->port_usb->multi_pkt_xfer) {
dev->no_tx_req_used--;
req->length = 0;
in = dev->port_usb->in_ep;
if (!list_empty(&dev->tx_reqs)) {
new_req = container_of(dev->tx_reqs.next,
struct usb_request, list);
list_del(&new_req->list);
spin_unlock(&dev->req_lock);
if (new_req->length > 0) {
length = new_req->length;
/* NCM requires no zlp if transfer is
* dwNtbInMaxSize */
if (dev->port_usb->is_fixed &&
length == dev->port_usb->fixed_in_len &&
(length % in->maxpacket) == 0)
new_req->zero = 0;
else
new_req->zero = 1;
/* use zlp framing on tx for strict CDC-Ether
* conformance, though any robust network rx
* path ignores extra padding. and some hardware
* doesn't like to write zlps.
*/
if (new_req->zero && !dev->zlp &&
(length % in->maxpacket) == 0) {
new_req->zero = 0;
length++;
}
new_req->length = length;
retval = usb_ep_queue(in, new_req, GFP_ATOMIC);
switch (retval) {
default:
#ifndef CONFIG_USB_NCM_SUPPORT_MTU_CHANGE
DBG(dev, "tx queue err %d\n", retval);
#else
printk(KERN_ERR"usb:%s tx queue err %d\n",__func__, retval);
#endif
new_req->length = 0;
spin_lock(&dev->req_lock);
list_add_tail(&new_req->list,
&dev->tx_reqs);
spin_unlock(&dev->req_lock);
break;
case 0:
spin_lock(&dev->req_lock);
dev->no_tx_req_used++;
spin_unlock(&dev->req_lock);
net->trans_start = jiffies;
}
} else {
spin_lock(&dev->req_lock);
/*
* Put the idle request at the back of the
* queue. The xmit function will put the
* unfinished request at the beginning of the
* queue.
*/
list_add_tail(&new_req->list, &dev->tx_reqs);
spin_unlock(&dev->req_lock);
}
} else {
/*
* The setup() callback implements all the ep0 functionality that's
* not handled lower down, in hardware or the hardware driver (like
* device and endpoint feature flags, and their status). It's all
* housekeeping for the gadget function we're implementing. Most of
* the work is in config-specific setup.
*/
static int
zero_setup(struct usb_gadget *gadget, const struct usb_ctrlrequest *ctrl)
{
struct zero_dev *dev = get_gadget_data(gadget);
struct usb_request *req = dev->req;
int value = -EOPNOTSUPP;
u16 w_index = le16_to_cpu(ctrl->wIndex);
u16 w_value = le16_to_cpu(ctrl->wValue);
u16 w_length = le16_to_cpu(ctrl->wLength);
/* usually this stores reply data in the pre-allocated ep0 buffer,
* but config change events will reconfigure hardware.
*/
req->zero = 0;
switch (ctrl->bRequest) {
case USB_REQ_GET_DESCRIPTOR:
if (ctrl->bRequestType != USB_DIR_IN)
goto unknown;
switch (w_value >> 8) {
case USB_DT_DEVICE:
value = min(w_length, (u16) sizeof device_desc);
memcpy(req->buf, &device_desc, value);
break;
case USB_DT_DEVICE_QUALIFIER:
if (!gadget_is_dualspeed(gadget))
break;
value = min(w_length, (u16) sizeof dev_qualifier);
memcpy(req->buf, &dev_qualifier, value);
break;
case USB_DT_OTHER_SPEED_CONFIG:
if (!gadget_is_dualspeed(gadget))
break;
// FALLTHROUGH
case USB_DT_CONFIG:
value = config_buf(gadget, req->buf,
w_value >> 8,
w_value & 0xff);
if (value >= 0)
value = min(w_length, (u16) value);
break;
case USB_DT_STRING:
/* wIndex == language code.
* this driver only handles one language, you can
* add string tables for other languages, using
* any UTF-8 characters
*/
value = usb_gadget_get_string(&stringtab,
w_value & 0xff, req->buf);
if (value >= 0)
value = min(w_length, (u16) value);
break;
}
break;
/* currently two configs, two speeds */
case USB_REQ_SET_CONFIGURATION:
if (ctrl->bRequestType != 0)
goto unknown;
if (gadget->a_hnp_support)
DBG(dev, "HNP available\n");
else if (gadget->a_alt_hnp_support)
DBG(dev, "HNP needs a different root port\n");
else
VDBG(dev, "HNP inactive\n");
spin_lock(&dev->lock);
value = zero_set_config(dev, w_value);
spin_unlock(&dev->lock);
break;
case USB_REQ_GET_CONFIGURATION:
if (ctrl->bRequestType != USB_DIR_IN)
goto unknown;
*(u8 *)req->buf = dev->config;
value = min(w_length, (u16) 1);
break;
/* until we add altsetting support, or other interfaces,
* only 0/0 are possible. pxa2xx only supports 0/0 (poorly)
* and already killed pending endpoint I/O.
*/
case USB_REQ_SET_INTERFACE:
if (ctrl->bRequestType != USB_RECIP_INTERFACE)
goto unknown;
spin_lock(&dev->lock);
if (dev->config && w_index == 0 && w_value == 0) {
u8 config = dev->config;
/* resets interface configuration, forgets about
* previous transaction state (queued bufs, etc)
* and re-inits endpoint state (toggle etc)
* no response queued, just zero status == success.
* if we had more than one interface we couldn't
* use this "reset the config" shortcut.
*/
zero_reset_config(dev);
zero_set_config(dev, config);
value = 0;
}
spin_unlock(&dev->lock);
break;
case USB_REQ_GET_INTERFACE:
if (ctrl->bRequestType != (USB_DIR_IN|USB_RECIP_INTERFACE))
goto unknown;
if (!dev->config)
break;
if (w_index != 0) {
value = -EDOM;
break;
}
*(u8 *)req->buf = 0;
value = min(w_length, (u16) 1);
break;
/*
* These are the same vendor-specific requests supported by
* Intel's USB 2.0 compliance test devices. We exceed that
* device spec by allowing multiple-packet requests.
*/
case 0x5b: /* control WRITE test -- fill the buffer */
if (ctrl->bRequestType != (USB_DIR_OUT|USB_TYPE_VENDOR))
goto unknown;
if (w_value || w_index)
break;
/* just read that many bytes into the buffer */
if (w_length > USB_BUFSIZ)
break;
value = w_length;
break;
case 0x5c: /* control READ test -- return the buffer */
if (ctrl->bRequestType != (USB_DIR_IN|USB_TYPE_VENDOR))
goto unknown;
if (w_value || w_index)
break;
/* expect those bytes are still in the buffer; send back */
if (w_length > USB_BUFSIZ
|| w_length != req->length)
break;
value = w_length;
break;
default:
unknown:
VDBG(dev,
"unknown control req%02x.%02x v%04x i%04x l%d\n",
ctrl->bRequestType, ctrl->bRequest,
w_value, w_index, w_length);
}
/* respond with data transfer before status phase? */
if (value >= 0) {
req->length = value;
req->zero = value < w_length;
value = usb_ep_queue(gadget->ep0, req, GFP_ATOMIC);
if (value < 0) {
DBG(dev, "ep_queue --> %d\n", value);
req->status = 0;
zero_setup_complete(gadget->ep0, req);
}
}
/* device either stalls (value < 0) or reports success */
return value;
}