path: root/net/bluetooth/sco.c

/*
   BlueZ - Bluetooth protocol stack for Linux
   Copyright (C) 2000-2001 Qualcomm Incorporated

   Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License version 2 as
   published by the Free Software Foundation;

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
   IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
   CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
   WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
   ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
   OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
   SOFTWARE IS DISCLAIMED.
*/

/* Bluetooth SCO sockets. */

#include <linux/module.h>

#include <linux/types.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/slab.h>
#include <linux/poll.h>
#include <linux/fcntl.h>
#include <linux/init.h>
#include <linux/interrupt.h>
#include <linux/socket.h>
#include <linux/skbuff.h>
#include <linux/device.h>
#include <linux/list.h>
#include <net/sock.h>

#include <asm/system.h>
#include <asm/uaccess.h>

#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/sco.h>

#ifndef CONFIG_BT_SCO_DEBUG
#undef  BT_DBG
#define BT_DBG(D...)
#endif

#define VERSION "0.5"

static const struct proto_ops sco_sock_ops;

static struct bt_sock_list sco_sk_list = {
	.lock = RW_LOCK_UNLOCKED
};

static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent);
static void sco_chan_del(struct sock *sk, int err);

static int  sco_conn_del(struct hci_conn *conn, int err);

static void sco_sock_close(struct sock *sk);
static void sco_sock_kill(struct sock *sk);

/* ---- SCO timers ---- */
static void sco_sock_timeout(unsigned long arg)
{
	struct sock *sk = (struct sock *) arg;

	BT_DBG("sock %p state %d", sk, sk->sk_state);

	bh_lock_sock(sk);
	sk->sk_err = ETIMEDOUT;
	sk->sk_state_change(sk);
	bh_unlock_sock(sk);

	sco_sock_kill(sk);
	sock_put(sk);
}

static void sco_sock_set_timer(struct sock *sk, long timeout)
{
	BT_DBG("sock %p state %d timeout %ld", sk, sk->sk_state, timeout);
	sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout);
}

static void sco_sock_clear_timer(struct sock *sk)
{
	BT_DBG("sock %p state %d", sk, sk->sk_state);
	sk_stop_timer(sk, &sk->sk_timer);
}

static void sco_sock_init_timer(struct sock *sk)
{
	init_timer(&sk->sk_timer);
	sk->sk_timer.function = sco_sock_timeout;
	sk->sk_timer.data = (unsigned long)sk;
}

/* ---- SCO connections ---- */
static struct sco_conn *sco_conn_add(struct hci_conn *hcon, __u8 status)
{
	struct hci_dev *hdev = hcon->hdev;
	struct sco_conn *conn = hcon->sco_data;

	if (conn || status)
		return conn;

	conn = kzalloc(sizeof(struct sco_conn), GFP_ATOMIC);
	if (!conn)
		return NULL;

	spin_lock_init(&conn->lock);

	hcon->sco_data = conn;
	conn->hcon = hcon;

	conn->src = &hdev->bdaddr;
	conn->dst = &hcon->dst;

	if (hdev->sco_mtu > 0)
		conn->mtu = hdev->sco_mtu;
	else
		conn->mtu = 60;

	BT_DBG("hcon %p conn %p", hcon, conn);

	return conn;
}

static inline struct sock *sco_chan_get(struct sco_conn *conn)
{
	struct sock *sk = NULL;
	sco_conn_lock(conn);
	sk = conn->sk;
	sco_conn_unlock(conn);
	return sk;
}

static int sco_conn_del(struct hci_conn *hcon, int err)
{
	struct sco_conn *conn;
	struct sock *sk;

	if (!(conn = hcon->sco_data))
		return 0;

	BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);

	/* Kill socket */
	if ((sk = sco_chan_get(conn))) {
		bh_lock_sock(sk);
		sco_sock_clear_timer(sk);
		sco_chan_del(sk, err);
		bh_unlock_sock(sk);
		sco_sock_kill(sk);
	}

	hcon->sco_data = NULL;
	kfree(conn);
	return 0;
}

static inline int sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent)
{
	int err = 0;

	sco_conn_lock(conn);
	if (conn->sk) {
		err = -EBUSY;
	} else {
		__sco_chan_add(conn, sk, parent);
	}
	sco_conn_unlock(conn);
	return err;
}

static int sco_connect(struct sock *sk)
{
	bdaddr_t *src = &bt_sk(sk)->src;
	bdaddr_t *dst = &bt_sk(sk)->dst;
	struct sco_conn *conn;
	struct hci_conn *hcon;
	struct hci_dev  *hdev;
	int err = 0;

	BT_DBG("%s -> %s", batostr(src), batostr(dst));

	if (!(hdev = hci_get_route(dst, src)))
		return -EHOSTUNREACH;

	hci_dev_lock_bh(hdev);

	err = -ENOMEM;

	hcon = hci_connect(hdev, SCO_LINK, dst);
	if (!hcon)
		goto done;

	conn = sco_conn_add(hcon, 0);
	if (!conn) {
		hci_conn_put(hcon);
		goto done;
	}

	/* Update source addr of the socket */
	bacpy(src, conn->src);

	err = sco_chan_add(conn, sk, NULL);
	if (err)
		goto done;

	if (hcon->state == BT_CONNECTED) {
		sco_sock_clear_timer(sk);
		sk->sk_state = BT_CONNECTED;
	} else {
		sk->sk_state = BT_CONNECT;
		sco_sock_set_timer(sk, sk->sk_sndtimeo);
	}
done:
	hci_dev_unlock_bh(hdev);
	hci_dev_put(hdev);
	return err;
}

static inline int sco_send_frame(struct sock *sk, struct msghdr *msg, int len)
{
	struct sco_conn *conn = sco_pi(sk)->conn;
	struct sk_buff *skb;
	int err, count;

	/* Check outgoing MTU */
	if (len > conn->mtu)
		return -EINVAL;

	BT_DBG("sk %p len %d", sk, len);

	count = min_t(unsigned int, conn->mtu, len);
	if (!(skb = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err)))
		return err;

	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
		err = -EFAULT;
		goto fail;
	}

	if ((err = hci_send_sco(conn->hcon, skb)) < 0)
		return err;

	return count;

fail:
	kfree_skb(skb);
	return err;
}

static inline void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb)
{
	struct sock *sk = sco_chan_get(conn);

	if (!sk)
		goto drop;

	BT_DBG("sk %p len %d", sk, skb->len);

	if (sk->sk_state != BT_CONNECTED)
		goto drop;

	if (!sock_queue_rcv_skb(sk, skb))
		return;

drop:
	kfree_skb(skb);
	return;
}

/* -------- Socket interface ---------- */
static struct sock *__sco_get_sock_by_addr(bdaddr_t *ba)
{
	struct sock *sk;
	struct hlist_node *node;

	sk_for_each(sk, node, &sco_sk_list.head)
		if (!bacmp(&bt_sk(sk)->src, ba))
			goto found;
	sk = NULL;
found:
	return sk;
}

/* Find socket listening on source bdaddr.
 * Returns closest match.
 */
static struct sock *sco_get_sock_listen(bdaddr_t *src)
{
	struct sock *sk = NULL, *sk1 = NULL;
	struct hlist_node *node;

	read_lock(&sco_sk_list.lock);

	sk_for_each(sk, node, &sco_sk_list.head) {
		if (sk->sk_state != BT_LISTEN)
			continue;

		/* Exact match. */
		if (!bacmp(&bt_sk(sk)->src, src))
			break;

		/* Closest match */
		if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
			sk1 = sk;
	}

	read_unlock(&sco_sk_list.lock);

	return node ? sk : sk1;
}

static void sco_sock_destruct(struct sock *sk)
{
	BT_DBG("sk %p", sk);

	skb_queue_purge(&sk->sk_receive_queue);
	skb_queue_purge(&sk->sk_write_queue);
}

static void sco_sock_cleanup_listen(struct sock *parent)
{
	struct sock *sk;

	BT_DBG("parent %p", parent);

	/* Close not yet accepted channels */
	while ((sk = bt_accept_dequeue(parent, NULL))) {
		sco_sock_close(sk);
		sco_sock_kill(sk);
	}

	parent->sk_state  = BT_CLOSED;
	sock_set_flag(parent, SOCK_ZAPPED);
}

/* Kill socket (only if zapped and orphan)
 * Must be called on unlocked socket.
 */
static void sco_sock_kill(struct sock *sk)
{
	if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket)
		return;

	BT_DBG("sk %p state %d", sk, sk->sk_state);

	/* Kill poor orphan */
	bt_sock_unlink(&sco_sk_list, sk);
	sock_set_flag(sk, SOCK_DEAD);
	sock_put(sk);
}

/* Close socket.
 * Must be called on unlocked socket.
 */
static void sco_sock_close(struct sock *sk)
{
	struct sco_conn *conn;

	sco_sock_clear_timer(sk);

	lock_sock(sk);

	conn = sco_pi(sk)->conn;

	BT_DBG("sk %p state %d conn %p socket %p", sk, sk->sk_state, conn, sk->sk_socket);

	switch (sk->sk_state) {
	case BT_LISTEN:
		sco_sock_cleanup_listen(sk);
		break;

	case BT_CONNECTED:
	case BT_CONFIG:
	case BT_CONNECT:
	case BT_DISCONN:
		sco_chan_del(sk, ECONNRESET);
		break;

	default:
		sock_set_flag(sk, SOCK_ZAPPED);
		break;
	};

	release_sock(sk);

	sco_sock_kill(sk);
}

static void sco_sock_init(struct sock *sk, struct sock *parent)
{
	BT_DBG("sk %p", sk);

	if (parent)
		sk->sk_type = parent->sk_type;
}

static struct proto sco_proto = {
	.name		= "SCO",
	.owner		= THIS_MODULE,
	.obj_size	= sizeof(struct sco_pinfo)
};

static struct sock *sco_sock_alloc(struct socket *sock, int proto, gfp_t prio)
{
	struct sock *sk;

	sk = sk_alloc(PF_BLUETOOTH, prio, &sco_proto, 1);
	if (!sk)
		return NULL;

	sock_init_data(sock, sk);
	INIT_LIST_HEAD(&bt_sk(sk)->accept_q);

	sk->sk_destruct = sco_sock_destruct;
	sk->sk_sndtimeo = SCO_CONN_TIMEOUT;

	sock_reset_flag(sk, SOCK_ZAPPED);

	sk->sk_protocol = proto;
	sk->sk_state    = BT_OPEN;

	sco_sock_init_timer(sk);

	bt_sock_link(&sco_sk_list, sk);
	return sk;
}

static int sco_sock_create(struct socket *sock, int protocol)
{
	struct sock *sk;

	BT_DBG("sock %p", sock);

	sock->state = SS_UNCONNECTED;

	if (sock->type != SOCK_SEQPACKET)
		return -ESOCKTNOSUPPORT;

	sock->ops = &sco_sock_ops;

	sk = sco_sock_alloc(sock, protocol, GFP_ATOMIC);
	if (!sk)
		return -ENOMEM;

	sco_sock_init(sk, NULL);
	return 0;
}

static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
{
	struct sockaddr_sco *sa = (struct sockaddr_sco *) addr;
	struct sock *sk = sock->sk;
	bdaddr_t *src = &sa->sco_bdaddr;
	int err = 0;

	BT_DBG("sk %p %s", sk, batostr(&sa->sco_bdaddr));

	if (!addr || addr->sa_family != AF_BLUETOOTH)
		return -EINVAL;

	lock_sock(sk);

	if (sk->sk_state != BT_OPEN) {
		err = -EBADFD;
		goto done;
	}

	write_lock_bh(&sco_sk_list.lock);

	if (bacmp(src, BDADDR_ANY) && __sco_get_sock_by_addr(src)) {
		err = -EADDRINUSE;
	} else {
		/* Save source address */
		bacpy(&bt_sk(sk)->src, &sa->sco_bdaddr);
		sk->sk_state = BT_BOUND;
	}

	write_unlock_bh(&sco_sk_list.lock);

done:
	release_sock(sk);
	return err;
}
class="hl num">1;
static volatile int fdc_nested;
static DECLARE_WAIT_QUEUE_HEAD(fdc_wait);
 
static DECLARE_WAIT_QUEUE_HEAD(motor_wait);

static volatile int selected = -1;	/* currently selected drive */

static int writepending;
static int writefromint;
static char *raw_buf;

static DEFINE_SPINLOCK(amiflop_lock);

#define RAW_BUF_SIZE 30000  /* size of raw disk data */

/*
 * These are global variables, as that's the easiest way to give
 * information to interrupts. They are the data used for the current
 * request.
 */
static volatile char block_flag;
static DECLARE_WAIT_QUEUE_HEAD(wait_fd_block);

/* MS-Dos MFM Coding tables (should go quick and easy) */
static unsigned char mfmencode[16]={
	0x2a, 0x29, 0x24, 0x25, 0x12, 0x11, 0x14, 0x15,
	0x4a, 0x49, 0x44, 0x45, 0x52, 0x51, 0x54, 0x55
};
static unsigned char mfmdecode[128];

/* floppy internal millisecond timer stuff */
static volatile int ms_busy = -1;
static DECLARE_WAIT_QUEUE_HEAD(ms_wait);
#define MS_TICKS ((amiga_eclock+50)/1000)

/*
 * Note that MAX_ERRORS=X doesn't imply that we retry every bad read
 * max X times - some types of errors increase the errorcount by 2 or
 * even 3, so we might actually retry only X/2 times before giving up.
 */
#define MAX_ERRORS 12

/* Prevent "aliased" accesses. */
static int fd_ref[4] = { 0,0,0,0 };
static int fd_device[4] = { 0, 0, 0, 0 };

/*
 * Here come the actual hardware access and helper functions.
 * They are not reentrant and single threaded because all drives
 * share the same hardware and the same trackbuffer.
 */

/* Milliseconds timer */

static irqreturn_t ms_isr(int irq, void *dummy, struct pt_regs *fp)
{
	ms_busy = -1;
	wake_up(&ms_wait);
	return IRQ_HANDLED;
}

/* all waits are queued up 
   A more generic routine would do a schedule a la timer.device */
static void ms_delay(int ms)
{
	unsigned long flags;
	int ticks;
	if (ms > 0) {
		local_irq_save(flags);
		while (ms_busy == 0)
			sleep_on(&ms_wait);
		ms_busy = 0;
		local_irq_restore(flags);
		ticks = MS_TICKS*ms-1;
		ciaa.tblo=ticks%256;
		ciaa.tbhi=ticks/256;
		ciaa.crb=0x19; /*count eclock, force load, one-shoot, start */
		sleep_on(&ms_wait);
	}
}

/* Hardware semaphore */

/* returns true when we would get the semaphore */
static inline int try_fdc(int drive)
{
	drive &= 3;
	return ((fdc_busy < 0) || (fdc_busy == drive));
}

static void get_fdc(int drive)
{
	unsigned long flags;

	drive &= 3;
#ifdef DEBUG
	printk("get_fdc: drive %d  fdc_busy %d  fdc_nested %d\n",drive,fdc_busy,fdc_nested);
#endif
	local_irq_save(flags);
	while (!try_fdc(drive))
		sleep_on(&fdc_wait);
	fdc_busy = drive;
	fdc_nested++;
	local_irq_restore(flags);
}

static inline void rel_fdc(void)
{
#ifdef DEBUG
	if (fdc_nested == 0)
		printk("fd: unmatched rel_fdc\n");
	printk("rel_fdc: fdc_busy %d fdc_nested %d\n",fdc_busy,fdc_nested);
#endif
	fdc_nested--;
	if (fdc_nested == 0) {
		fdc_busy = -1;
		wake_up(&fdc_wait);
	}
}

static void fd_select (int drive)
{
	unsigned char prb = ~0;

	drive&=3;
#ifdef DEBUG
	printk("selecting %d\n",drive);
#endif
	if (drive == selected)
		return;
	get_fdc(drive);
	selected = drive;

	if (unit[drive].track % 2 != 0)
		prb &= ~DSKSIDE;
	if (unit[drive].motor == 1)
		prb &= ~DSKMOTOR;
	ciab.prb |= (SELMASK(0)|SELMASK(1)|SELMASK(2)|SELMASK(3));
	ciab.prb = prb;
	prb &= ~SELMASK(drive);
	ciab.prb = prb;
	rel_fdc();
}

static void fd_deselect (int drive)
{
	unsigned char prb;
	unsigned long flags;

	drive&=3;
#ifdef DEBUG
	printk("deselecting %d\n",drive);
#endif
	if (drive != selected) {
		printk(KERN_WARNING "Deselecting drive %d while %d was selected!\n",drive,selected);
		return;
	}

	get_fdc(drive);
	local_irq_save(flags);

	selected = -1;

	prb = ciab.prb;
	prb |= (SELMASK(0)|SELMASK(1)|SELMASK(2)|SELMASK(3));
	ciab.prb = prb;

	local_irq_restore (flags);
	rel_fdc();

}

static void motor_on_callback(unsigned long nr)
{
	if (!(ciaa.pra & DSKRDY) || --on_attempts == 0) {
		wake_up (&motor_wait);
	} else {
		motor_on_timer.expires = jiffies + HZ/10;
		add_timer(&motor_on_timer);
	}
}

static int fd_motor_on(int nr)
{
	nr &= 3;

	del_timer(motor_off_timer + nr);

	if (!unit[nr].motor) {
		unit[nr].motor = 1;
		fd_select(nr);

		motor_on_timer.data = nr;
		mod_timer(&motor_on_timer, jiffies + HZ/2);

		on_attempts = 10;
		sleep_on (&motor_wait);
		fd_deselect(nr);
	}

	if (on_attempts == 0) {
		on_attempts = -1;
#if 0
		printk (KERN_ERR "motor_on failed, turning motor off\n");
		fd_motor_off (nr);
		return 0;
#else
		printk (KERN_WARNING "DSKRDY not set after 1.5 seconds - assuming drive is spinning notwithstanding\n");
#endif
	}

	return 1;
}

static void fd_motor_off(unsigned long drive)
{
	long calledfromint;
#ifdef MODULE
	long decusecount;

	decusecount = drive & 0x40000000;
#endif
	calledfromint = drive & 0x80000000;
	drive&=3;
	if (calledfromint && !try_fdc(drive)) {
		/* We would be blocked in an interrupt, so try again later */
		motor_off_timer[drive].expires = jiffies + 1;
		add_timer(motor_off_timer + drive);
		return;
	}
	unit[drive].motor = 0;
	fd_select(drive);
	udelay (1);
	fd_deselect(drive);
}

static void floppy_off (unsigned int nr)
{
	int drive;

	drive = nr & 3;
	/* called this way it is always from interrupt */
	motor_off_timer[drive].data = nr | 0x80000000;
	mod_timer(motor_off_timer + drive, jiffies + 3*HZ);
}

static int fd_calibrate(int drive)
{
	unsigned char prb;
	int n;

	drive &= 3;
	get_fdc(drive);
	if (!fd_motor_on (drive))
		return 0;
	fd_select (drive);
	prb = ciab.prb;
	prb |= DSKSIDE;
	prb &= ~DSKDIREC;
	ciab.prb = prb;
	for (n = unit[drive].type->tracks/2; n != 0; --n) {
		if (ciaa.pra & DSKTRACK0)
			break;
		prb &= ~DSKSTEP;
		ciab.prb = prb;
		prb |= DSKSTEP;
		udelay (2);
		ciab.prb = prb;
		ms_delay(unit[drive].type->step_delay);
	}
	ms_delay (unit[drive].type->settle_time);
	prb |= DSKDIREC;
	n = unit[drive].type->tracks + 20;
	for (;;) {
		prb &= ~DSKSTEP;
		ciab.prb = prb;
		prb |= DSKSTEP;
		udelay (2);
		ciab.prb = prb;
		ms_delay(unit[drive].type->step_delay + 1);
		if ((ciaa.pra & DSKTRACK0) == 0)
			break;
		if (--n == 0) {
			printk (KERN_ERR "fd%d: calibrate failed, turning motor off\n", drive);
			fd_motor_off (drive);
			unit[drive].track = -1;
			rel_fdc();
			return 0;
		}
	}
	unit[drive].track = 0;
	ms_delay(unit[drive].type->settle_time);

	rel_fdc();
	fd_deselect(drive);
	return 1;
}

static int fd_seek(int drive, int track)
{
	unsigned char prb;
	int cnt;

#ifdef DEBUG
	printk("seeking drive %d to track %d\n",drive,track);
#endif
	drive &= 3;
	get_fdc(drive);
	if (unit[drive].track == track) {
		rel_fdc();
		return 1;
	}
	if (!fd_motor_on(drive)) {
		rel_fdc();
		return 0;
	}
	if (unit[drive].track < 0 && !fd_calibrate(drive)) {
		rel_fdc();
		return 0;
	}

	fd_select (drive);
	cnt = unit[drive].track/2 - track/2;
	prb = ciab.prb;
	prb |= DSKSIDE | DSKDIREC;
	if (track % 2 != 0)
		prb &= ~DSKSIDE;
	if (cnt < 0) {
		cnt = - cnt;
		prb &= ~DSKDIREC;
	}
	ciab.prb = prb;
	if (track % 2 != unit[drive].track % 2)
		ms_delay (unit[drive].type->side_time);
	unit[drive].track = track;
	if (cnt == 0) {
		rel_fdc();
		fd_deselect(drive);
		return 1;
	}
	do {
		prb &= ~DSKSTEP;
		ciab.prb = prb;
		prb |= DSKSTEP;
		udelay (1);
		ciab.prb = prb;
		ms_delay (unit[drive].type->step_delay);
	} while (--cnt != 0);
	ms_delay (unit[drive].type->settle_time);

	rel_fdc();
	fd_deselect(drive);
	return 1;
}

static unsigned long fd_get_drive_id(int drive)
{
	int i;
	ulong id = 0;

  	drive&=3;
  	get_fdc(drive);
	/* set up for ID */
	MOTOR_ON;
	udelay(2);
	SELECT(SELMASK(drive));
	udelay(2);
	DESELECT(SELMASK(drive));
	udelay(2);
	MOTOR_OFF;
	udelay(2);
	SELECT(SELMASK(drive));
	udelay(2);
	DESELECT(SELMASK(drive));
	udelay(2);

	/* loop and read disk ID */
	for (i=0; i<32; i++) {
		SELECT(SELMASK(drive));
		udelay(2);

		/* read and store value of DSKRDY */
		id <<= 1;
		id |= (ciaa.pra & DSKRDY) ? 0 : 1;	/* cia regs are low-active! */

		DESELECT(SELMASK(drive));
	}

	rel_fdc();

        /*
         * RB: At least A500/A2000's df0: don't identify themselves.
         * As every (real) Amiga has at least a 3.5" DD drive as df0:
         * we default to that if df0: doesn't identify as a certain
         * type.
         */
        if(drive == 0 && id == FD_NODRIVE)
	{
                id = fd_def_df0;
                printk(KERN_NOTICE "fd: drive 0 didn't identify, setting default %08lx\n", (ulong)fd_def_df0);
	}
	/* return the ID value */
	return (id);
}

static irqreturn_t fd_block_done(int irq, void *dummy, struct pt_regs *fp)
{
	if (block_flag)
		custom.dsklen = 0x4000;

	if (block_flag == 2) { /* writing */
		writepending = 2;
		post_write_timer.expires = jiffies + 1; /* at least 2 ms */
		post_write_timer.data = selected;
		add_timer(&post_write_timer);
	}
	else {                /* reading */
		block_flag = 0;
		wake_up (&wait_fd_block);
	}
	return IRQ_HANDLED;
}

static void raw_read(int drive)
{
	drive&=3;
	get_fdc(drive);
	while (block_flag)
		sleep_on(&wait_fd_block);
	fd_select(drive);
	/* setup adkcon bits correctly */
	custom.adkcon = ADK_MSBSYNC;
	custom.adkcon = ADK_SETCLR|ADK_WORDSYNC|ADK_FAST;

	custom.dsksync = MFM_SYNC;

	custom.dsklen = 0;
	custom.dskptr = (u_char *)ZTWO_PADDR((u_char *)raw_buf);
	custom.dsklen = unit[drive].type->read_size/sizeof(short) | DSKLEN_DMAEN;
	custom.dsklen = unit[drive].type->read_size/sizeof(short) | DSKLEN_DMAEN;

	block_flag = 1;

	while (block_flag)
		sleep_on (&wait_fd_block);

	custom.dsklen = 0;
	fd_deselect(drive);
	rel_fdc();
}

static int raw_write(int drive)
{
	ushort adk;

	drive&=3;
	get_fdc(drive); /* corresponds to rel_fdc() in post_write() */
	if ((ciaa.pra & DSKPROT) == 0) {
		rel_fdc();
		return 0;
	}
	while (block_flag)
		sleep_on(&wait_fd_block);
	fd_select(drive);
	/* clear adkcon bits */
	custom.adkcon = ADK_PRECOMP1|ADK_PRECOMP0|ADK_WORDSYNC|ADK_MSBSYNC;
	/* set appropriate adkcon bits */
	adk = ADK_SETCLR|ADK_FAST;
	if ((ulong)unit[drive].track >= unit[drive].type->precomp2)
		adk |= ADK_PRECOMP1;
	else if ((ulong)unit[drive].track >= unit[drive].type->precomp1)
		adk |= ADK_PRECOMP0;
	custom.adkcon = adk;

	custom.dsklen = DSKLEN_WRITE;
	custom.dskptr = (u_char *)ZTWO_PADDR((u_char *)raw_buf);
	custom.dsklen = unit[drive].type->write_size/sizeof(short) | DSKLEN_DMAEN|DSKLEN_WRITE;
	custom.dsklen = unit[drive].type->write_size/sizeof(short) | DSKLEN_DMAEN|DSKLEN_WRITE;

	block_flag = 2;
	return 1;
}

/*
 * to be called at least 2ms after the write has finished but before any
 * other access to the hardware.
 */
static void post_write (unsigned long drive)
{
#ifdef DEBUG
	printk("post_write for drive %ld\n",drive);
#endif
	drive &= 3;
	custom.dsklen = 0;
	block_flag = 0;
	writepending = 0;
	writefromint = 0;
	unit[drive].dirty = 0;
	wake_up(&wait_fd_block);
	fd_deselect(drive);
	rel_fdc(); /* corresponds to get_fdc() in raw_write */
}


/*
 * The following functions are to convert the block contents into raw data
 * written to disk and vice versa.
 * (Add other formats here ;-))
 */

static unsigned long scan_sync(unsigned long raw, unsigned long end)
{
	ushort *ptr = (ushort *)raw, *endp = (ushort *)end;

	while (ptr < endp && *ptr++ != 0x4489)
		;
	if (ptr < endp) {
		while (*ptr == 0x4489 && ptr < endp)
			ptr++;
		return (ulong)ptr;
	}
	return 0;
}

static inline unsigned long checksum(unsigned long *addr, int len)
{
	unsigned long csum = 0;

	len /= sizeof(*addr);
	while (len-- > 0)
		csum ^= *addr++;
	csum = ((csum>>1) & 0x55555555)  ^  (csum & 0x55555555);

	return csum;
}

static unsigned long decode (unsigned long *data, unsigned long *raw,
			     int len)
{
	ulong *odd, *even;

	/* convert length from bytes to longwords */
	len >>= 2;
	odd = raw;
	even = odd + len;

	/* prepare return pointer */
	raw += len * 2;

	do {
		*data++ = ((*odd++ & 0x55555555) << 1) | (*even++ & 0x55555555);
	} while (--len != 0);

	return (ulong)raw;
}

struct header {
	unsigned char magic;
	unsigned char track;
	unsigned char sect;
	unsigned char ord;
	unsigned char labels[16];
	unsigned long hdrchk;
	unsigned long datachk;
};

static int amiga_read(int drive)
{
	unsigned long raw;
	unsigned long end;
	int scnt;
	unsigned long csum;
	struct header hdr;

	drive&=3;
	raw = (long) raw_buf;
	end = raw + unit[drive].type->read_size;

	for (scnt = 0;scnt < unit[drive].dtype->sects * unit[drive].type->sect_mult; scnt++) {
		if (!(raw = scan_sync(raw, end))) {
			printk (KERN_INFO "can't find sync for sector %d\n", scnt);
			return MFM_NOSYNC;
		}

		raw = decode ((ulong *)&hdr.magic, (ulong *)raw, 4);
		raw = decode ((ulong *)&hdr.labels, (ulong *)raw, 16);
		raw = decode ((ulong *)&hdr.hdrchk, (ulong *)raw, 4);
		raw = decode ((ulong *)&hdr.datachk, (ulong *)raw, 4);
		csum = checksum((ulong *)&hdr,
				(char *)&hdr.hdrchk-(char *)&hdr);

#ifdef DEBUG
		printk ("(%x,%d,%d,%d) (%lx,%lx,%lx,%lx) %lx %lx\n",
			hdr.magic, hdr.track, hdr.sect, hdr.ord,
			*(ulong *)&hdr.labels[0], *(ulong *)&hdr.labels[4],
			*(ulong *)&hdr.labels[8], *(ulong *)&hdr.labels[12],
			hdr.hdrchk, hdr.datachk);
#endif

		if (hdr.hdrchk != csum) {
			printk(KERN_INFO "MFM_HEADER: %08lx,%08lx\n", hdr.hdrchk, csum);
			return MFM_HEADER;
		}

		/* verify track */
		if (hdr.track != unit[drive].track) {
			printk(KERN_INFO "MFM_TRACK: %d, %d\n", hdr.track, unit[drive].track);
			return MFM_TRACK;
		}

		raw = decode ((ulong *)(unit[drive].trackbuf + hdr.sect*512),
			      (ulong *)raw, 512);
		csum = checksum((ulong *)(unit[drive].trackbuf + hdr.sect*512), 512);

		if (hdr.datachk != csum) {
			printk(KERN_INFO "MFM_DATA: (%x:%d:%d:%d) sc=%d %lx, %lx\n",
			       hdr.magic, hdr.track, hdr.sect, hdr.ord, scnt,
			       hdr.datachk, csum);
			printk (KERN_INFO "data=(%lx,%lx,%lx,%lx)\n",
				((ulong *)(unit[drive].trackbuf+hdr.sect*512))[0],
				((ulong *)(unit[drive].trackbuf+hdr.sect*512))[1],
				((ulong *)(unit[drive].trackbuf+hdr.sect*512))[2],
				((ulong *)(unit[drive].trackbuf+hdr.sect*512))[3]);
			return MFM_DATA;
		}
	}

	return 0;
}

static void encode(unsigned long data, unsigned long *dest)
{
	unsigned long data2;

	data &= 0x55555555;
	data2 = data ^ 0x55555555;
	data |= ((data2 >> 1) | 0x80000000) & (data2 << 1);

	if (*(dest - 1) & 0x00000001)
		data &= 0x7FFFFFFF;

	*dest = data;
}

static void encode_block(unsigned long *dest, unsigned long *src, int len)
{
	int cnt, to_cnt = 0;
	unsigned long data;

	/* odd bits */
	for (cnt = 0; cnt < len / 4; cnt++) {
		data = src[cnt] >> 1;
		encode(data, dest + to_cnt++);
	}

	/* even bits */
	for (cnt = 0; cnt < len / 4; cnt++) {
		data = src[cnt];
		encode(data, dest + to_cnt++);
	}
}

static unsigned long *putsec(int disk, unsigned long *raw, int cnt)
{
	struct header hdr;
	int i;

	disk&=3;
	*raw = (raw[-1]&1) ? 0x2AAAAAAA : 0xAAAAAAAA;
	raw++;
	*raw++ = 0x44894489;

	hdr.magic = 0xFF;
	hdr.track = unit[disk].track;
	hdr.sect = cnt;
	hdr.ord = unit[disk].dtype->sects * unit[disk].type->sect_mult - cnt;
	for (i = 0; i < 16; i++)
		hdr.labels[i] = 0;
	hdr.hdrchk = checksum((ulong *)&hdr,
			      (char *)&hdr.hdrchk-(char *)&hdr);
	hdr.datachk = checksum((ulong *)(unit[disk].trackbuf+cnt*512), 512);

	encode_block(raw, (ulong *)&hdr.magic, 4);
	raw += 2;
	encode_block(raw, (ulong *)&hdr.labels, 16);
	raw += 8;
	encode_block(raw, (ulong *)&hdr.hdrchk, 4);
	raw += 2;
	encode_block(raw, (ulong *)&hdr.datachk, 4);
	raw += 2;
	encode_block(raw, (ulong *)(unit[disk].trackbuf+cnt*512), 512);
	raw += 256;

	return raw;
}

static void amiga_write(int disk)
{
	unsigned int cnt;
	unsigned long *ptr = (unsigned long *)raw_buf;

	disk&=3;
	/* gap space */
	for (cnt = 0; cnt < 415 * unit[disk].type->sect_mult; cnt++)
		*ptr++ = 0xaaaaaaaa;

	/* sectors */
	for (cnt = 0; cnt < unit[disk].dtype->sects * unit[disk].type->sect_mult; cnt++)
		ptr = putsec (disk, ptr, cnt);
	*(ushort *)ptr = (ptr[-1]&1) ? 0x2AA8 : 0xAAA8;
}


struct dos_header {
	unsigned char track,   /* 0-80 */
		side,    /* 0-1 */
		sec,     /* 0-...*/
		len_desc;/* 2 */
	unsigned short crc;     /* on 68000 we got an alignment problem, 
				   but this compiler solves it  by adding silently 
				   adding a pad byte so data won't fit
				   and this took about 3h to discover.... */
	unsigned char gap1[22];     /* for longword-alignedness (0x4e) */
};

/* crc routines are borrowed from the messydos-handler  */

/* excerpt from the messydos-device           
; The CRC is computed not only over the actual data, but including
; the SYNC mark (3 * $a1) and the 'ID/DATA - Address Mark' ($fe/$fb).
; As we don't read or encode these fields into our buffers, we have to
; preload the registers containing the CRC with the values they would have
; after stepping over these fields.
;
; How CRCs "really" work:
;
; First, you should regard a bitstring as a series of coefficients of
; polynomials. We calculate with these polynomials in modulo-2
; arithmetic, in which both add and subtract are done the same as
; exclusive-or. Now, we modify our data (a very long polynomial) in
; such a way that it becomes divisible by the CCITT-standard 16-bit
;		 16   12   5
; polynomial:	x  + x	+ x + 1, represented by $11021. The easiest
; way to do this would be to multiply (using proper arithmetic) our
; datablock with $11021. So we have:
;   data * $11021		 =
;   data * ($10000 + $1021)      =
;   data * $10000 + data * $1021
; The left part of this is simple: Just add two 0 bytes. But then
; the right part (data $1021) remains difficult and even could have
; a carry into the left part. The solution is to use a modified
; multiplication, which has a result that is not correct, but with
; a difference of any multiple of $11021. We then only need to keep
; the 16 least significant bits of the result.
;
; The following algorithm does this for us:
;
;   unsigned char *data, c, crclo, crchi;
;   while (not done) {
;	c = *data++ + crchi;
;	crchi = (@ c) >> 8 + crclo;
;	crclo = @ c;
;   }
;
; Remember, + is done with EOR, the @ operator is in two tables (high
; and low byte separately), which is calculated as
;
;      $1021 * (c & $F0)
;  xor $1021 * (c & $0F)
;  xor $1021 * (c >> 4)         (* is regular multiplication)
;
;
; Anyway, the end result is the same as the remainder of the division of
; the data by $11021. I am afraid I need to study theory a bit more...


my only works was to code this from manx to C....

*/

static ushort dos_crc(void * data_a3, int data_d0, int data_d1, int data_d3)
{
	static unsigned char CRCTable1[] = {
		0x00,0x10,0x20,0x30,0x40,0x50,0x60,0x70,0x81,0x91,0xa1,0xb1,0xc1,0xd1,0xe1,0xf1,
		0x12,0x02,0x32,0x22,0x52,0x42,0x72,0x62,0x93,0x83,0xb3,0xa3,0xd3,0xc3,0xf3,0xe3,
		0x24,0x34,0x04,0x14,0x64,0x74,0x44,0x54,0xa5,0xb5,0x85,0x95,0xe5,0xf5,0xc5,0xd5,
		0x36,0x26,0x16,0x06,0x76,0x66,0x56,0x46,0xb7,0xa7,0x97,0x87,0xf7,0xe7,0xd7,0xc7,
		0x48,0x58,0x68,0x78,0x08,0x18,0x28,0x38,0xc9,0xd9,0xe9,0xf9,0x89,0x99,0xa9,0xb9,
		0x5a,0x4a,0x7a,0x6a,0x1a,0x0a,0x3a,0x2a,0xdb,0xcb,0xfb,0xeb,0x9b,0x8b,0xbb,0xab,
		0x6c,0x7c,0x4c,0x5c,0x2c,0x3c,0x0c,0x1c,0xed,0xfd,0xcd,0xdd,0xad,0xbd,0x8d,0x9d,
		0x7e,0x6e,0x5e,0x4e,0x3e,0x2e,0x1e,0x0e,0xff,0xef,0xdf,0xcf,0xbf,0xaf,0x9f,0x8f,
		0x91,0x81,0xb1,0xa1,0xd1,0xc1,0xf1,0xe1,0x10,0x00,0x30,0x20,0x50,0x40,0x70,0x60,
		0x83,0x93,0xa3,0xb3,0xc3,0xd3,0xe3,0xf3,0x02,0x12,0x22,0x32,0x42,0x52,0x62,0x72,
		0xb5,0xa5,0x95,0x85,0xf5,0xe5,0xd5,0xc5,0x34,0x24,0x14,0x04,0x74,0x64,0x54,0x44,
		0xa7,0xb7,0x87,0x97,0xe7,0xf7,0xc7,0xd7,0x26,0x36,0x06,0x16,0x66,0x76,0x46,0x56,
		0xd9,0xc9,0xf9,0xe9,0x99,0x89,0xb9,0xa9,0x58,0x48,0x78,0x68,0x18,0x08,0x38,0x28,
		0xcb,0xdb,0xeb,0xfb,0x8b,0x9b,0xab,0xbb,0x4a,0x5a,0x6a,0x7a,0x0a,0x1a,0x2a,0x3a,
		0xfd,0xed,0xdd,0xcd,0xbd,0xad,0x9d,0x8d,0x7c,0x6c,0x5c,0x4c,0x3c,0x2c,0x1c,0x0c,
		0xef,0xff,0xcf,0xdf,0xaf,0xbf,0x8f,0x9f,0x6e,0x7e,0x4e,0x5e,0x2e,0x3e,0x0e,0x1e
	};

	static unsigned char CRCTable2[] = {
		0x00,0x21,0x42,0x63,0x84,0xa5,0xc6,0xe7,0x08,0x29,0x4a,0x6b,0x8c,0xad,0xce,0xef,
		0x31,0x10,0x73,0x52,0xb5,0x94,0xf7,0xd6,0x39,0x18,0x7b,0x5a,0xbd,0x9c,0xff,0xde,
		0x62,0x43,0x20,0x01,0xe6,0xc7,0xa4,0x85,0x6a,0x4b,0x28,0x09,0xee,0xcf,0xac,0x8d,
		0x53,0x72,0x11,0x30,0xd7,0xf6,0x95,0xb4,0x5b,0x7a,0x19,0x38,0xdf,0xfe,0x9d,0xbc,
		0xc4,0xe5,0x86,0xa7,0x40,0x61,0x02,0x23,0xcc,0xed,0x8e,0xaf,0x48,0x69,0x0a,0x2b,
		0xf5,0xd4,0xb7,0x96,0x71,0x50,0x33,0x12,0xfd,0xdc,0xbf,0x9e,0x79,0x58,0x3b,0x1a,
		0xa6,0x87,0xe4,0xc5,0x22,0x03,0x60,0x41,0xae,0x8f,0xec,0xcd,0x2a,0x0b,0x68,0x49,
		0x97,0xb6,0xd5,0xf4,0x13,0x32,0x51,0x70,0x9f,0xbe,0xdd,0xfc,0x1b,0x3a,0x59,0x78,
		0x88,0xa9,0xca,0xeb,0x0c,0x2d,0x4e,0x6f,0x80,0xa1,0xc2,0xe3,0x04,0x25,0x46,0x67,
		0xb9,0x98,0xfb,0xda,0x3d,0x1c,0x7f,0x5e,0xb1,0x90,0xf3,0xd2,0x35,0x14,0x77,0x56,
		0xea,0xcb,0xa8,0x89,0x6e,0x4f,0x2c,0x0d,0xe2,0xc3,0xa0,0x81,0x66,0x47,0x24,0x05,
		0xdb,0xfa,0x99,0xb8,0x5f,0x7e,0x1d,0x3c,0xd3,0xf2,0x91,0xb0,0x57,0x76,0x15,0x34,
		0x4c,0x6d,0x0e,0x2f,0xc8,0xe9,0x8a,0xab,0x44,0x65,0x06,0x27,0xc0,0xe1,0x82,0xa3,
		0x7d,0x5c,0x3f,0x1e,0xf9,0xd8,0xbb,0x9a,0x75,0x54,0x37,0x16,0xf1,0xd0,0xb3,0x92,
		0x2e,0x0f,0x6c,0x4d,0xaa,0x8b,0xe8,0xc9,0x26,0x07,0x64,0x45,0xa2,0x83,0xe0,0xc1,
		0x1f,0x3e,0x5d,0x7c,0x9b,0xba,0xd9,0xf8,0x17,0x36,0x55,0x74,0x93,0xb2,0xd1,0xf0
	};

/* look at the asm-code - what looks in C a bit strange is almost as good as handmade */
	register int i;
	register unsigned char *CRCT1, *CRCT2, *data, c, crch, crcl;

	CRCT1=CRCTable1;
	CRCT2=CRCTable2;
	data=data_a3;
	crcl=data_d1;
	crch=data_d0;
	for (i=data_d3; i>=0; i--) {
		c = (*data++) ^ crch;
		crch = CRCT1[c] ^ crcl;
		crcl = CRCT2[c];
	}
	return (crch<<8)|crcl;
}

static inline ushort dos_hdr_crc (struct dos_header *hdr)
{
	return dos_crc(&(hdr->track), 0xb2, 0x30, 3); /* precomputed magic */
}

static inline ushort dos_data_crc(unsigned char *data)
{
	return dos_crc(data, 0xe2, 0x95 ,511); /* precomputed magic */
}

static inline unsigned char dos_decode_byte(ushort word)
{
	register ushort w2;
	register unsigned char byte;
	register unsigned char *dec = mfmdecode;

	w2=word;
	w2>>=8;
	w2&=127;
	byte = dec[w2];
	byte <<= 4;
	w2 = word & 127;
	byte |= dec[w2];
	return byte;
}

static unsigned long dos_decode(unsigned char *data, unsigned short *raw, int len)
{
	int i;

	for (i = 0; i < len; i++)
		*data++=dos_decode_byte(*raw++);
	return ((ulong)raw);
}

#ifdef DEBUG
static void dbg(unsigned long ptr)
{
	printk("raw data @%08lx: %08lx, %08lx ,%08lx, %08lx\n", ptr,
	       ((ulong *)ptr)[0], ((ulong *)ptr)[1],
	       ((ulong *)ptr)[2], ((ulong *)ptr)[3]);
}
#endif

static int dos_read(int drive)
{
	unsigned long end;
	unsigned long raw;
	int scnt;
	unsigned short crc,data_crc[2];
	struct dos_header hdr;

	drive&=3;
	raw = (long) raw_buf;
	end = raw + unit[drive].type->read_size;

	for (scnt=0; scnt < unit[drive].dtype->sects * unit[drive].type->sect_mult; scnt++) {
		do { /* search for the right sync of each sec-hdr */
			if (!(raw = scan_sync (raw, end))) {
				printk(KERN_INFO "dos_read: no hdr sync on "
				       "track %d, unit %d for sector %d\n",
				       unit[drive].track,drive,scnt);
				return MFM_NOSYNC;
			}
#ifdef DEBUG
			dbg(raw);
#endif
		} while (*((ushort *)raw)!=0x5554); /* loop usually only once done */
		raw+=2; /* skip over headermark */
		raw = dos_decode((unsigned char *)&hdr,(ushort *) raw,8);
		crc = dos_hdr_crc(&hdr);

#ifdef DEBUG
		printk("(%3d,%d,%2d,%d) %x\n", hdr.track, hdr.side,
		       hdr.sec, hdr.len_desc, hdr.crc);
#endif

		if (crc != hdr.crc) {
			printk(KERN_INFO "dos_read: MFM_HEADER %04x,%04x\n",
			       hdr.crc, crc);
			return MFM_HEADER;
		}
		if (hdr.track != unit[drive].track/unit[drive].type->heads) {
			printk(KERN_INFO "dos_read: MFM_TRACK %d, %d\n",
			       hdr.track,
			       unit[drive].track/unit[drive].type->heads);
			return MFM_TRACK;
		}

		if (hdr.side != unit[drive].track%unit[drive].type->heads) {
			printk(KERN_INFO "dos_read: MFM_SIDE %d, %d\n",
			       hdr.side,
			       unit[drive].track%unit[drive].type->heads);
			return MFM_TRACK;
		}

		if (hdr.len_desc != 2) {
			printk(KERN_INFO "dos_read: unknown sector len "
			       "descriptor %d\n", hdr.len_desc);
			return MFM_DATA;
		}
#ifdef DEBUG
		printk("hdr accepted\n");
#endif
		if (!(raw = scan_sync (raw, end))) {
			printk(KERN_INFO "dos_read: no data sync on track "
			       "%d, unit %d for sector%d, disk sector %d\n",
			       unit[drive].track, drive, scnt, hdr.sec);
			return MFM_NOSYNC;
		}
#ifdef DEBUG
		dbg(raw);
#endif

		if (*((ushort *)raw)!=0x5545) {
			printk(KERN_INFO "dos_read: no data mark after "
			       "sync (%d,%d,%d,%d) sc=%d\n",
			       hdr.track,hdr.side,hdr.sec,hdr.len_desc,scnt);
			return MFM_NOSYNC;
		}

		raw+=2;  /* skip data mark (included in checksum) */
		raw = dos_decode((unsigned char *)(unit[drive].trackbuf + (hdr.sec - 1) * 512), (ushort *) raw, 512);
		raw = dos_decode((unsigned char  *)data_crc,(ushort *) raw,4);
		crc = dos_data_crc(unit[drive].trackbuf + (hdr.sec - 1) * 512);

		if (crc != data_crc[0]) {
			printk(KERN_INFO "dos_read: MFM_DATA (%d,%d,%d,%d) "
			       "sc=%d, %x %x\n", hdr.track, hdr.side,
			       hdr.sec, hdr.len_desc, scnt,data_crc[0], crc);
			printk(KERN_INFO "data=(%lx,%lx,%lx,%lx,...)\n",
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[0],
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[1],
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[2],
			       ((ulong *)(unit[drive].trackbuf+(hdr.sec-1)*512))[3]);
			return MFM_DATA;
		}
	}
	return 0;
}

static inline ushort dos_encode_byte(unsigned char byte)
{
	register unsigned char *enc, b2, b1;
	register ushort word;

	enc=mfmencode;
	b1=byte;
	b2=b1>>4;
	b1&=15;
	word=enc[b2] <<8 | enc [b1];
	return (word|((word&(256|64)) ? 0: 128));
}

static void dos_encode_block(ushort *dest, unsigned char *src, int len)
{
	int i;

	for (i = 0; i < len; i++) {
		*dest=dos_encode_byte(*src++);
		*dest|=((dest[-1]&1)||(*dest&0x4000))? 0: 0x8000;
		dest++;
	}
}

static unsigned long *ms_putsec(int drive, unsigned long *raw, int cnt)
{
	static struct dos_header hdr={0,0,0,2,0,
	  {78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78,78}};
	int i;
	static ushort crc[2]={0,0x4e4e};

	drive&=3;
/* id gap 1 */
/* the MFM word before is always 9254 */
	for(i=0;i<6;i++)
		*raw++=0xaaaaaaaa;
/* 3 sync + 1 headermark */
	*raw++=0x44894489;
	*raw++=0x44895554;

/* fill in the variable parts of the header */
	hdr.track=unit[drive].track/unit[drive].type->heads;
	hdr.side=unit[drive].track%unit[drive].type->heads;
	hdr.sec=cnt+1;
	hdr.crc=dos_hdr_crc(&hdr);

/* header (without "magic") and id gap 2*/
	dos_encode_block((ushort *)raw,(unsigned char *) &hdr.track,28);
	raw+=14;

/*id gap 3 */
	for(i=0;i<6;i++)
		*raw++=0xaaaaaaaa;

/* 3 syncs and 1 datamark */
	*raw++=0x44894489;
	*raw++=0x44895545;

/* data */
	dos_encode_block((ushort *)raw,
			 (unsigned char *)unit[drive].trackbuf+cnt*512,512);
	raw+=256;

/*data crc + jd's special gap (long words :-/) */
	crc[0]=dos_data_crc(unit[drive].trackbuf+cnt*512);
	dos_encode_block((ushort *) raw,(unsigned char *)crc,4);
	raw+=2;

/* data gap */
	for(i=0;i<38;i++)
		*raw++=0x92549254;

	return raw; /* wrote 652 MFM words */
}

static void dos_write(int disk)
{
	int cnt;
	unsigned long raw = (unsigned long) raw_buf;
	unsigned long *ptr=(unsigned long *)raw;

	disk&=3;
/* really gap4 + indexgap , but we write it first and round it up */
	for (cnt=0;cnt<425;cnt++)
		*ptr++=0x92549254;

/* the following is just guessed */
	if (unit[disk].type->sect_mult==2)  /* check for HD-Disks */
		for(cnt=0;cnt<473;cnt++)
			*ptr++=0x92549254;

/* now the index marks...*/
	for (cnt=0;cnt<20;cnt++)
		*ptr++=0x92549254;
	for (cnt=0;cnt<6;cnt++)
		*ptr++=0xaaaaaaaa;
	*ptr++=0x52245224;
	*ptr++=0x52245552;
	for (cnt=0;cnt<20;cnt++)
		*ptr++=0x92549254;

/* sectors */
	for(cnt = 0; cnt < unit[disk].dtype->sects * unit[disk].type->sect_mult; cnt++)
		ptr=ms_putsec(disk,ptr,cnt);

	*(ushort *)ptr = 0xaaa8; /* MFM word before is always 0x9254 */
}

/*
 * Here comes the high level stuff (i.e. the filesystem interface)
 * and helper functions.
 * Normally this should be the only part that has to be adapted to
 * different kernel versions.
 */

/* FIXME: this assumes the drive is still spinning -
 * which is only true if we complete writing a track within three seconds
 */
static void flush_track_callback(unsigned long nr)
{
	nr&=3;
	writefromint = 1;
	if (!try_fdc(nr)) {
		/* we might block in an interrupt, so try again later */
		flush_track_timer[nr].expires = jiffies + 1;
		add_timer(flush_track_timer + nr);
		return;
	}
	get_fdc(nr);
	(*unit[nr].dtype->write_fkt)(nr);
	if (!raw_write(nr)) {
		printk (KERN_NOTICE "floppy disk write protected\n");
		writefromint = 0;
		writepending = 0;
	}
	rel_fdc();
}

static int non_int_flush_track (unsigned long nr)
{
	unsigned long flags;

	nr&=3;
	writefromint = 0;
	del_timer(&post_write_timer);
	get_fdc(nr);
	if (!fd_motor_on(nr)) {
		writepending = 0;
		rel_fdc();
		return 0;
	}
	local_irq_save(flags);
	if (writepending != 2) {
		local_irq_restore(flags);
		(*unit[nr].dtype->write_fkt)(nr);
		if (!raw_write(nr)) {
			printk (KERN_NOTICE "floppy disk write protected "
				"in write!\n");
			writepending = 0;
			return 0;
		}
		while (block_flag == 2)
			sleep_on (&wait_fd_block);
	}
	else {
		local_irq_restore(flags);
		ms_delay(2); /* 2 ms post_write delay */
		post_write(nr);
	}
	rel_fdc();
	return 1;
}

static int get_track(int drive, int track)
{
	int error, errcnt;

	drive&=3;
	if (unit[drive].track == track)
		return 0;
	get_fdc(drive);
	if (!fd_motor_on(drive)) {
		rel_fdc();
		return -1;
	}

	if (unit[drive].dirty == 1) {
		del_timer (flush_track_timer + drive);
		non_int_flush_track (drive);
	}
	errcnt = 0;
	while (errcnt < MAX_ERRORS) {
		if (!fd_seek(drive, track))
			return -1;
		raw_read(drive);
		error = (*unit[drive].dtype->read_fkt)(drive);
		if (error == 0) {
			rel_fdc();
			return 0;
		}
		/* Read Error Handling: recalibrate and try again */
		unit[drive].track = -1;
		errcnt++;
	}
	rel_fdc();
	return -1;
}

static void redo_fd_request(void)
{
	unsigned int cnt, block, track, sector;
	int drive;
	struct amiga_floppy_struct *floppy;
	char *data;
	unsigned long flags;

 repeat:
	if (!CURRENT) {
		/* Nothing left to do */
		return;
	}

	floppy = CURRENT->rq_disk->private_data;
	drive = floppy - unit;

	/* Here someone could investigate to be more efficient */
	for (cnt = 0; cnt < CURRENT->current_nr_sectors; cnt++) { 
#ifdef DEBUG
		printk("fd: sector %ld + %d requested for %s\n",
		       CURRENT->sector,cnt,
		       (CURRENT->cmd==READ)?"read":"write");
#endif
		block = CURRENT->sector + cnt;
		if ((int)block > floppy->blocks) {
			end_request(CURRENT, 0);
			goto repeat;
		}

		track = block / (floppy->dtype->sects * floppy->type->sect_mult);
		sector = block % (floppy->dtype->sects * floppy->type->sect_mult);
		data = CURRENT->buffer + 512 * cnt;
#ifdef DEBUG
		printk("access to track %d, sector %d, with buffer at "
		       "0x%08lx\n", track, sector, data);
#endif

		if ((rq_data_dir(CURRENT) != READ) && (rq_data_dir(CURRENT) != WRITE)) {
			printk(KERN_WARNING "do_fd_request: unknown command\n");
			end_request(CURRENT, 0);
			goto repeat;
		}
		if (get_track(drive, track) == -1) {
			end_request(CURRENT, 0);
			goto repeat;
		}

		switch (rq_data_dir(CURRENT)) {
		case READ:
			memcpy(data, floppy->trackbuf + sector * 512, 512);
			break;

		case WRITE:
			memcpy(floppy->trackbuf + sector * 512, data, 512);

			/* keep the drive spinning while writes are scheduled */
			if (!fd_motor_on(drive)) {
				end_request(CURRENT, 0);
				goto repeat;
			}
			/*
			 * setup a callback to write the track buffer
			 * after a short (1 tick) delay.
			 */
			local_irq_save(flags);

			floppy->dirty = 1;
		        /* reset the timer */
			mod_timer (flush_track_timer + drive, jiffies + 1);
			local_irq_restore(flags);
			break;
		}
	}
	CURRENT->nr_sectors -= CURRENT->current_nr_sectors;
	CURRENT->sector += CURRENT->current_nr_sectors;

	end_request(CURRENT, 1);
	goto repeat;
}

static void do_fd_request(request_queue_t * q)
{
	redo_fd_request();
}

static int fd_ioctl(struct inode *inode, struct file *filp,
		    unsigned int cmd, unsigned long param)
{
	int drive = iminor(inode) & 3;
	static struct floppy_struct getprm;

	switch(cmd){
	case HDIO_GETGEO:
	{
		struct hd_geometry loc;
		loc.heads = unit[drive].type->heads;
		loc.sectors = unit[drive].dtype->sects * unit[drive].type->sect_mult;
		loc.cylinders = unit[drive].type->tracks;
		loc.start = 0;
		if (copy_to_user((void *)param, (void *)&loc,
				 sizeof(struct hd_geometry)))
			return -EFAULT;
		break;
	}
	case FDFMTBEG:
		get_fdc(drive);
		if (fd_ref[drive] > 1) {
			rel_fdc();
			return -EBUSY;
		}
		fsync_bdev(inode->i_bdev);
		if (fd_motor_on(drive) == 0) {
			rel_fdc();
			return -ENODEV;
		}
		if (fd_calibrate(drive) == 0) {
			rel_fdc();
			return -ENXIO;
		}
		floppy_off(drive);
		rel_fdc();
		break;
	case FDFMTTRK:
		if (param < unit[drive].type->tracks * unit[drive].type->heads)
		{
			get_fdc(drive);
			if (fd_seek(drive,param) != 0){
				memset(unit[drive].trackbuf, FD_FILL_BYTE,
				       unit[drive].dtype->sects * unit[drive].type->sect_mult * 512);
				non_int_flush_track(drive);
			}
			floppy_off(drive);
			rel_fdc();
		}
		else
			return -EINVAL;
		break;
	case FDFMTEND:
		floppy_off(drive);
		invalidate_bdev(inode->i_bdev, 0);
		break;
	case FDGETPRM:
		memset((void *)&getprm, 0, sizeof (getprm));
		getprm.track=unit[drive].type->tracks;
		getprm.head=unit[drive].type->heads;
		getprm.sect=unit[drive].dtype->sects * unit[drive].type->sect_mult;
		getprm.size=unit[drive].blocks;
		if (copy_to_user((void *)param,
				 (void *)&getprm,
				 sizeof(struct floppy_struct)))
			return -EFAULT;
		break;
	case FDSETPRM:
	case FDDEFPRM:
		return -EINVAL;
	case FDFLUSH: /* unconditionally, even if not needed */
		del_timer (flush_track_timer + drive);
		non_int_flush_track(drive);
		break;
#ifdef RAW_IOCTL
	case IOCTL_RAW_TRACK:
		if (copy_to_user((void *)param, raw_buf,
				 unit[drive].type->read_size))
			return -EFAULT;
		else
			return unit[drive].type->read_size;
#endif
	default:
		printk(KERN_DEBUG "fd_ioctl: unknown cmd %d for drive %d.",
		       cmd, drive);
		return -ENOSYS;
	}
	return 0;
}

static void fd_probe(int dev)
{
	unsigned long code;
	int type;
	int drive;

	drive = dev & 3;
	code = fd_get_drive_id(drive);

	/* get drive type */
	for (type = 0; type < num_dr_types; type++)
		if (drive_types[type].code == code)
			break;

	if (type >= num_dr_types) {
		printk(KERN_WARNING "fd_probe: unsupported drive type "
		       "%08lx found\n", code);
		unit[drive].type = &drive_types[num_dr_types-1]; /* FD_NODRIVE */
		return;
	}

	unit[drive].type = drive_types + type;
	unit[drive].track = -1;

	unit[drive].disk = -1;
	unit[drive].motor = 0;
	unit[drive].busy = 0;
	unit[drive].status = -1;
}

/*
 * floppy_open check for aliasing (/dev/fd0 can be the same as
 * /dev/PS0 etc), and disallows simultaneous access to the same
 * drive with different device numbers.
 */
static int floppy_open(struct inode *inode, struct file *filp)
{
	int drive = iminor(inode) & 3;
	int system =  (iminor(inode) & 4) >> 2;
	int old_dev;
	unsigned long flags;

	old_dev = fd_device[drive];

	if (fd_ref[drive] && old_dev != system)
		return -EBUSY;

	if (filp && filp->f_mode & 3) {
		check_disk_change(inode->i_bdev);
		if (filp->f_mode & 2 ) {
			int wrprot;

			get_fdc(drive);
			fd_select (drive);
			wrprot = !(ciaa.pra & DSKPROT);
			fd_deselect (drive);
			rel_fdc();

			if (wrprot)
				return -EROFS;
		}
	}

	local_irq_save(flags);
	fd_ref[drive]++;
	fd_device[drive] = system;
	local_irq_restore(flags);

	unit[drive].dtype=&data_types[system];
	unit[drive].blocks=unit[drive].type->heads*unit[drive].type->tracks*
		data_types[system].sects*unit[drive].type->sect_mult;
	set_capacity(unit[drive].gendisk, unit[drive].blocks);

	printk(KERN_INFO "fd%d: accessing %s-disk with %s-layout\n",drive,
	       unit[drive].type->name, data_types[system].name);

	return 0;
}

static int floppy_release(struct inode * inode, struct file * filp)
{
	int drive = iminor(inode) & 3;

	if (unit[drive].dirty == 1) {
		del_timer (flush_track_timer + drive);
		non_int_flush_track (drive);
	}
  
	if (!fd_ref[drive]--) {
		printk(KERN_CRIT "floppy_release with fd_ref == 0");
		fd_ref[drive] = 0;
	}
#ifdef MODULE
/* the mod_use counter is handled this way */
	floppy_off (drive | 0x40000000);
#endif
	return 0;
}

/*
 * floppy-change is never called from an interrupt, so we can relax a bit
 * here, sleep etc. Note that floppy-on tries to set current_DOR to point
 * to the desired drive, but it will probably not survive the sleep if
 * several floppies are used at the same time: thus the loop.
 */
static int amiga_floppy_change(struct gendisk *disk)
{
	struct amiga_floppy_struct *p = disk->private_data;
	int drive = p - unit;
	int changed;
	static int first_time = 1;

	if (first_time)
		changed = first_time--;
	else {
		get_fdc(drive);
		fd_select (drive);
		changed = !(ciaa.pra & DSKCHANGE);
		fd_deselect (drive);
		rel_fdc();
	}

	if (changed) {
		fd_probe(drive);
		p->track = -1;
		p->dirty = 0;
		writepending = 0; /* if this was true before, too bad! */
		writefromint = 0;
		return 1;
	}
	return 0;
}

static struct block_device_operations floppy_fops = {
	.owner		= THIS_MODULE,
	.open		= floppy_open,
	.release	= floppy_release,
	.ioctl		= fd_ioctl,
	.media_changed	= amiga_floppy_change,
};

void __init amiga_floppy_setup (char *str, int *ints)
{
	printk (KERN_INFO "amiflop: Setting default df0 to %x\n", ints[1]);
	fd_def_df0 = ints[1];
}

static int __init fd_probe_drives(void)
{
	int drive,drives,nomem;

	printk(KERN_INFO "FD: probing units\n" KERN_INFO "found ");
	drives=0;
	nomem=0;
	for(drive=0;drive<FD_MAX_UNITS;drive++) {
		struct gendisk *disk;
		fd_probe(drive);
		if (unit[drive].type->code == FD_NODRIVE)
			continue;
		disk = alloc_disk(1);
		if (!disk) {
			unit[drive].type->code = FD_NODRIVE;
			continue;
		}
		unit[drive].gendisk = disk;
		drives++;
		if ((unit[drive].trackbuf = kmalloc(FLOPPY_MAX_SECTORS * 512, GFP_KERNEL)) == NULL) {
			printk("no mem for ");
			unit[drive].type = &drive_types[num_dr_types - 1]; /* FD_NODRIVE */
			drives--;
			nomem = 1;
		}
		printk("fd%d ",drive);
		disk->major = FLOPPY_MAJOR;
		disk->first_minor = drive;
		disk->fops = &floppy_fops;
		sprintf(disk->disk_name, "fd%d", drive);
		disk->private_data = &unit[drive];
		disk->queue = floppy_queue;
		set_capacity(disk, 880*2);
		add_disk(disk);
	}
	if ((drives > 0) || (nomem == 0)) {
		if (drives == 0)
			printk("no drives");
		printk("\n");
		return drives;
	}
	printk("\n");
	return -ENOMEM;
}
 
static struct kobject *floppy_find(dev_t dev, int *part, void *data)
{
	int drive = *part & 3;
	if (unit[drive].type->code == FD_NODRIVE)
		return NULL;
	*part = 0;
	return get_disk(unit[drive].gendisk);
}

int __init amiga_floppy_init(void)
{
	int i, ret;

	if (!AMIGAHW_PRESENT(AMI_FLOPPY))
		return -ENXIO;

	if (register_blkdev(FLOPPY_MAJOR,"fd"))
		return -EBUSY;

	/*
	 *  We request DSKPTR, DSKLEN and DSKDATA only, because the other
	 *  floppy registers are too spreaded over the custom register space
	 */
	ret = -EBUSY;
	if (!request_mem_region(CUSTOM_PHYSADDR+0x20, 8, "amiflop [Paula]")) {
		printk("fd: cannot get floppy registers\n");
		goto out_blkdev;
	}

	ret = -ENOMEM;
	if ((raw_buf = (char *)amiga_chip_alloc (RAW_BUF_SIZE, "Floppy")) ==
	    NULL) {
		printk("fd: cannot get chip mem buffer\n");
		goto out_memregion;
	}

	ret = -EBUSY;
	if (request_irq(IRQ_AMIGA_DSKBLK, fd_block_done, 0, "floppy_dma", NULL)) {
		printk("fd: cannot get irq for dma\n");
		goto out_irq;
	}

	if (request_irq(IRQ_AMIGA_CIAA_TB, ms_isr, 0, "floppy_timer", NULL)) {
		printk("fd: cannot get irq for timer\n");
		goto out_irq2;
	}

	ret = -ENOMEM;
	floppy_queue = blk_init_queue(do_fd_request, &amiflop_lock);
	if (!floppy_queue)
		goto out_queue;

	ret = -ENXIO;
	if (fd_probe_drives() < 1) /* No usable drives */
		goto out_probe;

	blk_register_region(MKDEV(FLOPPY_MAJOR, 0), 256, THIS_MODULE,
				floppy_find, NULL, NULL);

	/* initialize variables */
	init_timer(&motor_on_timer);
	motor_on_timer.expires = 0;
	motor_on_timer.data = 0;
	motor_on_timer.function = motor_on_callback;
	for (i = 0; i < FD_MAX_UNITS; i++) {
		init_timer(&motor_off_timer[i]);
		motor_off_timer[i].expires = 0;
		motor_off_timer[i].data = i|0x80000000;
		motor_off_timer[i].function = fd_motor_off;
		init_timer(&flush_track_timer[i]);
		flush_track_timer[i].expires = 0;
		flush_track_timer[i].data = i;
		flush_track_timer[i].function = flush_track_callback;

		unit[i].track = -1;
	}

	init_timer(&post_write_timer);
	post_write_timer.expires = 0;
	post_write_timer.data = 0;
	post_write_timer.function = post_write;
  
	for (i = 0; i < 128; i++)
		mfmdecode[i]=255;
	for (i = 0; i < 16; i++)
		mfmdecode[mfmencode[i]]=i;

	/* make sure that disk DMA is enabled */
	custom.dmacon = DMAF_SETCLR | DMAF_DISK;

	/* init ms timer */
	ciaa.crb = 8; /* one-shot, stop */
	return 0;

out_probe:
	blk_cleanup_queue(floppy_queue);
out_queue:
	free_irq(IRQ_AMIGA_CIAA_TB, NULL);
out_irq2:
	free_irq(IRQ_AMIGA_DSKBLK, NULL);
out_irq:
	amiga_chip_free(raw_buf);
out_memregion:
	release_mem_region(CUSTOM_PHYSADDR+0x20, 8);
out_blkdev:
	unregister_blkdev(FLOPPY_MAJOR,"fd");
	return ret;
}

#ifdef MODULE

int init_module(void)
{
	if (!MACH_IS_AMIGA)
		return -ENXIO;
	return amiga_floppy_init();
}

#if 0 /* not safe to unload */
void cleanup_module(void)
{
	int i;

	for( i = 0; i < FD_MAX_UNITS; i++) {
		if (unit[i].type->code != FD_NODRIVE) {
			del_gendisk(unit[i].gendisk);
			put_disk(unit[i].gendisk);
			kfree(unit[i].trackbuf);
		}
	}
	blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
	free_irq(IRQ_AMIGA_CIAA_TB, NULL);
	free_irq(IRQ_AMIGA_DSKBLK, NULL);
	custom.dmacon = DMAF_DISK; /* disable DMA */
	amiga_chip_free(raw_buf);
	blk_cleanup_queue(floppy_queue);
	release_mem_region(CUSTOM_PHYSADDR+0x20, 8);
	unregister_blkdev(FLOPPY_MAJOR, "fd");
}
#endif
#endif