From 7c5b45490e0148136ab3e74b2afd377ed94f32d1 Mon Sep 17 00:00:00 2001
From: dholland <dholland@NetBSD.org>
Date: Fri, 25 Jul 2014 08:22:08 +0000
Subject: [PATCH] Implement d_discard for wd.

---
 sys/dev/ata/wd.c | 67 ++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 56 insertions(+), 11 deletions(-)

diff --git a/sys/dev/ata/wd.c b/sys/dev/ata/wd.c
index f9d5fc7c721e..e994b9bb2928 100644
--- a/sys/dev/ata/wd.c
+++ b/sys/dev/ata/wd.c
@@ -1,4 +1,4 @@
-/*	$NetBSD: wd.c,v 1.409 2014/07/25 08:10:36 dholland Exp $ */
+/*	$NetBSD: wd.c,v 1.410 2014/07/25 08:22:08 dholland Exp $ */
 
 /*
  * Copyright (c) 1998, 2001 Manuel Bouyer.  All rights reserved.
@@ -54,7 +54,7 @@
  */
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: wd.c,v 1.409 2014/07/25 08:10:36 dholland Exp $");
+__KERNEL_RCSID(0, "$NetBSD: wd.c,v 1.410 2014/07/25 08:22:08 dholland Exp $");
 
 #include "opt_ata.h"
 
@@ -140,6 +140,7 @@ dev_type_ioctl(wdioctl);
 dev_type_strategy(wdstrategy);
 dev_type_dump(wddump);
 dev_type_size(wdsize);
+static dev_type_discard(wddiscard);
 
 const struct bdevsw wd_bdevsw = {
 	.d_open = wdopen,
@@ -148,7 +149,7 @@ const struct bdevsw wd_bdevsw = {
 	.d_ioctl = wdioctl,
 	.d_dump = wddump,
 	.d_psize = wdsize,
-	.d_discard = nodiscard,
+	.d_discard = wddiscard,
 	.d_flag = D_DISK
 };
 
@@ -163,7 +164,7 @@ const struct cdevsw wd_cdevsw = {
 	.d_poll = nopoll,
 	.d_mmap = nommap,
 	.d_kqfilter = nokqfilter,
-	.d_discard = nodiscard,
+	.d_discard = wddiscard,
 	.d_flag = D_DISK
 };
 
@@ -195,7 +196,7 @@ void  wdrestart(void *);
 void  wddone(void *);
 int   wd_get_params(struct wd_softc *, u_int8_t, struct ataparams *);
 int   wd_flushcache(struct wd_softc *, int);
-int   wd_trim(struct wd_softc *, int, struct disk_discard_range *);
+int   wd_trim(struct wd_softc *, int, daddr_t, long);
 bool  wd_shutdown(device_t, int);
 
 int   wd_getcache(struct wd_softc *, int *);
@@ -1562,8 +1563,12 @@ wdioctl(dev_t dev, u_long xfer, void *addr, int flag, struct lwp *l)
 		aprint_debug_dev(wd->sc_dev, "TRIM maxsize %ld\n", tp->maxsize);
 		return 0;
 	}
-	case DIOCDISCARD:
-		return wd_trim(wd, WDPART(dev), (struct disk_discard_range *)addr);
+	case DIOCDISCARD: {
+		struct disk_discard_range *dr;
+
+		dr = addr;
+		return wd_trim(wd, WDPART(dev), dr->bno, dr->size);
+	}
 
 	default:
 		return ENOTTY;
@@ -1574,6 +1579,47 @@ wdioctl(dev_t dev, u_long xfer, void *addr, int flag, struct lwp *l)
 #endif
 }
 
+static int
+wddiscard(dev_t dev, off_t pos, off_t len)
+{
+	struct wd_softc *wd = device_lookup_private(&wd_cd, WDUNIT(dev));
+	daddr_t bno;
+	long size, done;
+	long maxatonce, amount;
+	int result;
+
+	if (!(wd->sc_params.atap_ata_major & WDC_VER_ATA7)
+	    || !(wd->sc_params.support_dsm & ATA_SUPPORT_DSM_TRIM)) {
+		/* not supported; ignore request */
+		ATADEBUG_PRINT(("wddiscard (unsupported)\n"), DEBUG_FUNCS);
+		return 0;
+	}
+	maxatonce = 0xffff; /*wd->sc_params.max_dsm_blocks*/
+
+	ATADEBUG_PRINT(("wddiscard\n"), DEBUG_FUNCS);
+
+	if ((wd->sc_flags & WDF_LOADED) == 0)
+		return EIO;
+
+	/* round the start up and the end down */
+	bno = (pos + DEV_BSIZE - 1) >> DEV_BSHIFT;
+	size = ((pos + len) >> DEV_BSHIFT) - bno;
+
+	done = 0;
+	while (done < size) {
+	     amount = size - done;
+	     if (amount > maxatonce) {
+		     amount = maxatonce;
+	     }
+	     result = wd_trim(wd, WDPART(dev), bno + done, amount);
+	     if (result) {
+		     return result;
+	     }
+	     done += amount;
+	}
+	return 0;
+}
+
 #ifdef B_FORMAT
 int
 wdformat(struct buf *bp)
@@ -1940,11 +1986,10 @@ wd_flushcache(struct wd_softc *wd, int flags)
 }
 
 int
-wd_trim(struct wd_softc *wd, int part, struct disk_discard_range *tr)
+wd_trim(struct wd_softc *wd, int part, daddr_t bno, long size)
 {
 	struct ata_command ata_c;
 	unsigned char *req;
-	daddr_t bno = tr->bno;
 
 	if (part != RAW_PART)
 		bno += wd->sc_dk.dk_label->d_partitions[part].p_offset;;
@@ -1956,8 +2001,8 @@ wd_trim(struct wd_softc *wd, int part, struct disk_discard_range *tr)
 	req[3] = (bno >> 24) & 0xff;
 	req[4] = (bno >> 32) & 0xff;
 	req[5] = (bno >> 40) & 0xff;
-	req[6] = tr->size & 0xff;
-	req[7] = (tr->size >> 8) & 0xff;
+	req[6] = size & 0xff;
+	req[7] = (size >> 8) & 0xff;
 
 	memset(&ata_c, 0, sizeof(struct ata_command));
 	ata_c.r_command = ATA_DATA_SET_MANAGEMENT;