diff --git a/sdo/download.go b/sdo/download.go
index 4e05591..3748603 100644
--- a/sdo/download.go
+++ b/sdo/download.go
@@ -1,22 +1,22 @@
 package sdo
 
 import (
+	"github.com/brutella/can"
+	"github.com/brutella/canopen"
+
 	"bytes"
 	"encoding/binary"
 	"errors"
 	"fmt"
-	"github.com/brutella/can"
-	"github.com/brutella/canopen"
-	"log"
 	"time"
 )
 
 const (
-	ClientIntiateDownload = 0x20 // 0010 0000
-	ClientSegmentDownload = 0x00 // 0110 0000
+	DownloadInitiateRequest  = 0x20 // 0010 0000
+	DownloadInitiateResponse = 0x60 // 0110 0000
 
-	ServerInitiateDownload = 0x60 // 0110 0000
-	ServerSegmentDownload  = 0x20 // 0010 0000
+	DownloadSegmentRequest  = 0x00 // 0000 0000
+	DownloadSegmentResponse = 0x20 // 0010 0000
 )
 
 // Download represents a SDO download process to write data to a CANopen
@@ -30,94 +30,141 @@ type Download struct {
 }
 
 func (download Download) Do(bus *can.Bus) error {
-	c := &canopen.Client{bus, time.Second * 2}
-	data := download.Data
-
-	e := TransferExpedited
-	s := TransferSizeIndicated
-	n := byte(0)
-	if size := int32(len(data)); size > 4 {
-		e = 0
-		n = 0
-
-		var buf bytes.Buffer
-		if err := binary.Write(&buf, binary.LittleEndian, size); err != nil {
-			return err
-		} else {
-			data = buf.Bytes()
-		}
-	} else {
-		n = byte(size)
+	if err := download.doInit(bus); err != nil {
+		return err
 	}
 
-	bytes := []byte{
-		byte(ClientIntiateDownload | e | s | ((int(n) << 2) & TransferMaskSize)),
-		download.ObjectIndex.Index.B0, download.ObjectIndex.Index.B1,
-		download.ObjectIndex.SubIndex,
-	}
+	return download.doSegments(bus)
+}
 
-	// Initiate
-	frame := canopen.Frame{
-		CobID: download.RequestCobID,
-		Data:  append(bytes[:], data[:]...),
+func (download Download) doInit(bus *can.Bus) error {
+	frame, err := download.initFrame()
+	if err != nil {
+		return err
 	}
 
 	req := canopen.NewRequest(frame, uint32(download.ResponseCobID))
+	c := &canopen.Client{bus, time.Second * 2}
 	resp, err := c.Do(req)
 	if err != nil {
-		log.Print(err)
 		return err
 	}
 
 	frame = resp.Frame
-	b0 := frame.Data[0] // == 0100 nnes
-	scs := b0 & TransferMaskCommandSpecifier
-	switch scs {
-	case ServerInitiateDownload:
-		break
-	case TransferAbort:
-		return errors.New("Server aborted download")
+	switch scs := frame.Data[0] >> 5; scs {
+	case 3: // response
+		return nil
+	case 4: // abort
+		return errors.New("server aborted download initialization")
 	default:
-		log.Fatalf("Unexpected server command specifier %X", scs)
+		return fmt.Errorf("unexpected server command specifier %X (expected %X)", scs, 3)
 	}
+}
 
-	if e == 0 {
-		junks := splitN(download.Data, 7)
-		t := 0
-		for i, junk := range junks {
-			cmd := byte(ClientSegmentDownload)
+// initFrame returns the initial frame of the download.
+// If the download data is less than 4 bytes, the init frame data contains all download data.
+// If the download data is more than 4 bytes, the init frame data contains the overall length of the download data.
+func (download Download) initFrame() (frame canopen.Frame, err error) {
+	fdata := make([]byte, 8)
+
+	// css = 1 (download init request)
+	fdata[0] = setBit(fdata[0], 5)
+	fdata[1] = download.ObjectIndex.Index.B0
+	fdata[2] = download.ObjectIndex.Index.B1
+	fdata[3] = download.ObjectIndex.SubIndex
+
+	n := uint8(len(download.Data) & 0x3)
+	if n <= 4 { // does download data fit into one frame?
+		// e = 1 (expedited)
+		fdata[0] = setBit(fdata[0], 1)
+		// s = 1
+		fdata[0] = setBit(fdata[0], 0)
+		// n = number of unused bytes in frame.Data
+		fdata[0] |= (4 - n) << 2
+		// copy all download data into frame data
+		copy(fdata[3:], download.Data)
+	} else {
+		// e = 0
+		// n = 0 (frame.Data contains the overall )
+		// s = 1
+		fdata[0] = setBit(fdata[0], 0)
 
-			if t%2 == 1 {
-				cmd |= TransferSegmentToggle
-			}
+		var buf bytes.Buffer
+		if err = binary.Write(&buf, binary.LittleEndian, uint32(n)); err != nil {
+			return
+		}
 
-			t += 1
+		// copy overall length of download data into frame data
+		copy(fdata[:4], buf.Bytes())
+	}
 
-			// is last segmented
-			if i == len(junks)-1 {
-				cmd |= 0x1
-			}
+	frame.CobID = download.RequestCobID
+	frame.Data = fdata
 
-			frame = canopen.Frame{
-				CobID: download.RequestCobID,
-				Data:  append([]byte{cmd}, junk[:]...),
-			}
+	return
+}
 
-			req = canopen.NewRequest(frame, uint32(download.ResponseCobID))
-			resp, err = c.Do(req)
+func (download Download) doSegments(bus *can.Bus) error {
+	frames := download.segmentFrames()
 
-			if err != nil {
-				return err
-			}
+	c := &canopen.Client{bus, time.Second * 2}
+	for _, frame := range frames {
+		req := canopen.NewRequest(frame, uint32(download.ResponseCobID))
+		resp, err := c.Do(req)
+		if err != nil {
+			return err
+		}
 
-			// Segment response
-			frame := resp.Frame
-			if scs := frame.Data[0] & TransferMaskCommandSpecifier; scs != ServerSegmentDownload {
-				return fmt.Errorf("Invalid scs %X != %X\n", scs, ServerSegmentDownload)
-			}
+		switch scs := resp.Frame.Data[0] >> 5; scs {
+		case 1:
+			break
+		case 4:
+			return errors.New("server aborted download")
+		default:
+			return fmt.Errorf("unexpected server command specifier %X (expected %X)", scs, 1)
 		}
 
+		// check toggle bit
+		if hasBit(frame.Data[0], 4) != hasBit(resp.Frame.Data[0], 4) {
+			return fmt.Errorf("unexpected toggle bit %t", hasBit(resp.Frame.Data[0], 4))
+		}
 	}
 
 	return nil
 }
+
+func (download Download) segmentFrames() (frames []canopen.Frame) {
+	if len(download.Data) <= 4 {
+		return
+	}
+
+	junks := splitN(download.Data, 7)
+	for i, junk := range junks {
+		fdata := make([]byte, 8)
+
+		if len(junk) < 7 {
+			fdata[0] |= uint8(7-len(junk)) << 1
+		}
+
+		if i%2 == 1 {
+			// toggle bit 5
+			fdata[0] = setBit(fdata[0], 4)
+		}
+
+		if i == len(junks)-1 {
+			// c = 1 (no more segments to download)
+			fdata[0] = setBit(fdata[0], 0)
+		}
+
+		copy(fdata[1:], junk)
+
+		frame := canopen.Frame{
+			CobID: download.RequestCobID,
+			Data:  fdata,
+		}
+
+		frames = append(frames, frame)
+	}
+
+	return
+}
diff --git a/sdo/download_test.go b/sdo/download_test.go
index e9ea898..103a6f9 100644
--- a/sdo/download_test.go
+++ b/sdo/download_test.go
@@ -22,10 +22,10 @@ func (rw *downloadReadWriteCloser) Write(b []byte) (n int, err error) {
 		return 0, err
 	}
 
-	switch frm.Data[0] & TransferMaskCommandSpecifier {
-	case ClientIntiateDownload:
+	switch frm.Data[0] >> 5 {
+	case 1: // initiate
 		frm = downloadInitiateFrame
-	case ClientSegmentDownload:
+	case 0: // segment
 		frm = downloadSegmentFrame
 	default:
 		log.Fatal("Unknown command")
@@ -49,7 +49,7 @@ var downloadInitiateFrame = can.Frame{
 	Res0:   0x0,
 	Res1:   0x0,
 	Data: [can.MaxFrameDataLength]uint8{
-		ServerInitiateDownload,
+		3 << 5,
 		0xBB, 0xAA,
 		0xCC,
 		0x0, 0x0, 0x0, 0x0},
@@ -62,7 +62,7 @@ var downloadSegmentFrame = can.Frame{
 	Res0:   0x0,
 	Res1:   0x0,
 	Data: [can.MaxFrameDataLength]uint8{
-		ServerSegmentDownload,
+		1 << 5, /* scs */
 		0xBB, 0xAA,
 		0xCC,
 		0x0, 0x0, 0x0, 0x0},
diff --git a/sdo/upload.go b/sdo/upload.go
index 920a2fb..44628e3 100644
--- a/sdo/upload.go
+++ b/sdo/upload.go
@@ -1,21 +1,23 @@
 package sdo
 
 import (
+	"github.com/brutella/can"
+	"github.com/brutella/canopen"
+
 	"bytes"
 	"encoding/binary"
 	"errors"
-	"github.com/brutella/can"
-	"github.com/brutella/canopen"
+	"fmt"
 	"log"
 	"time"
 )
 
 const (
-	ClientIntiateUpload = 0x40 // 0100 0000
-	ClientSegmentUpload = 0x60 // 0110 0000
+	UploadInitiateRequest  = 0x40 // 0100 0000
+	UploadInitiateResponse = 0x40 // 0100 0000
 
-	ServerInitiateUpload = 0x40 // 0100 0000
-	ServerSegmentUpload  = 0x00 // 0000 0000
+	UploadSegmentRequest  = 0x60 // 0110 0000
+	UploadSegmentResponse = 0x00 // 0000 0000
 )
 
 // Upload represents a SDO upload process to read data from a CANopen
@@ -33,7 +35,7 @@ func (upload Upload) Do(bus *can.Bus) ([]byte, error) {
 	frame := canopen.Frame{
 		CobID: upload.RequestCobID,
 		Data: []byte{
-			byte(ClientIntiateUpload),
+			byte(UploadInitiateRequest),
 			upload.ObjectIndex.Index.B0, upload.ObjectIndex.Index.B1,
 			upload.ObjectIndex.SubIndex,
 			0x0, 0x0, 0x0, 0x0,
@@ -48,10 +50,8 @@ func (upload Upload) Do(bus *can.Bus) ([]byte, error) {
 	}
 
 	frame = resp.Frame
-	b0 := frame.Data[0] // == 0100 nnes
-	scs := b0 & TransferMaskCommandSpecifier
-	switch scs {
-	case ServerInitiateUpload:
+	switch scs := frame.Data[0] >> 5; scs {
+	case 2:
 		break
 	case TransferAbort:
 		return nil, errors.New("Server aborted upload")
@@ -59,11 +59,11 @@ func (upload Upload) Do(bus *can.Bus) ([]byte, error) {
 		log.Fatalf("Unexpected server command specifier %X", scs)
 	}
 
-	if isExpedited(frame) {
+	if hasBit(frame.Data[0], 1) { // e = 1?
 		// number of segment bytes with no data
-		n := 0
-		if isSizeIndicated(frame) {
-			n = sizeValue(frame)
+		var n uint8
+		if hasBit(frame.Data[0], 0) { // s = 1?
+			n = (frame.Data[0] >> 2) & 0x3
 		}
 		return frame.Data[4 : 8-n], nil
 	}
@@ -75,57 +75,43 @@ func (upload Upload) Do(bus *can.Bus) ([]byte, error) {
 		return nil, err
 	}
 
+	var i int
 	var buf bytes.Buffer
-	t := 0
 	for {
-		// Upload segment
-		cmd := byte(ClientSegmentUpload)
-		if t%2 == 1 {
-			cmd |= TransferSegmentToggle
+		data := make([]byte, 8)
+
+		// ccs = 3
+		data[0] |= 3 << 5
+
+		if i%2 == 1 {
+			// t = 1
+			data[0] = setBit(data[0], 4)
 		}
 
-		t += 1
+		i += 1
 
 		frame = canopen.Frame{
 			CobID: upload.RequestCobID,
-			Data: []byte{
-				cmd,
-				0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-			},
+			Data:  data,
 		}
 
 		req = canopen.NewRequest(frame, uint32(upload.ResponseCobID))
 		resp, err = c.Do(req)
 		if err != nil {
-			log.Fatal(err)
+			return nil, err
 		}
 
-		// Segment response
-		frame := resp.Frame
-		b0 = frame.Data[0]
+		if hasBit(frame.Data[0], 4) != hasBit(resp.Frame.Data[0], 4) {
+			return nil, fmt.Errorf("unexpected toggle bit %t", hasBit(resp.Frame.Data[0], 4))
+		}
 
-		n := int((b0 & 0xE) >> 1)
+		n := (resp.Frame.Data[0] >> 1) & 0x7
 		buf.Write(resp.Frame.Data[1 : 8-n])
-		if isLast(frame) {
+
+		if hasBit(resp.Frame.Data[0], 0) { // c = 1?
 			break
 		}
 	}
 
 	return buf.Bytes(), nil
 }
-
-func isExpedited(frame canopen.Frame) bool {
-	return frame.Data[0]&TransferExpedited == TransferExpedited
-}
-
-func isSizeIndicated(frame canopen.Frame) bool {
-	return frame.Data[0]&TransferSizeIndicated == TransferSizeIndicated
-}
-
-func isLast(frame canopen.Frame) bool {
-	return frame.Data[0]&0x1 == 0x1
-}
-
-func sizeValue(frame canopen.Frame) int {
-	return int(frame.Data[0] & TransferMaskSize >> 2)
-}
diff --git a/sdo/util.go b/sdo/util.go
new file mode 100644
index 0000000..3da50b2
--- /dev/null
+++ b/sdo/util.go
@@ -0,0 +1,11 @@
+package sdo
+
+func hasBit(n uint8, pos uint) bool {
+	val := n & (1 << pos)
+	return (val > 0)
+}
+
+func setBit(n uint8, pos uint) uint8 {
+	n |= (1 << pos)
+	return n
+}