midibuf.c 5.4 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Line 6 Linux USB driver
  4. *
  5. * Copyright (C) 2004-2010 Markus Grabner ([email protected])
  6. */
  7. #include <linux/slab.h>
  8. #include "midibuf.h"
  9. static int midibuf_message_length(unsigned char code)
  10. {
  11. int message_length;
  12. if (code < 0x80)
  13. message_length = -1;
  14. else if (code < 0xf0) {
  15. static const int length[] = { 3, 3, 3, 3, 2, 2, 3 };
  16. message_length = length[(code >> 4) - 8];
  17. } else {
  18. static const int length[] = { -1, 2, 2, 2, -1, -1, 1, 1, 1, -1,
  19. 1, 1, 1, -1, 1, 1
  20. };
  21. message_length = length[code & 0x0f];
  22. }
  23. return message_length;
  24. }
  25. static int midibuf_is_empty(struct midi_buffer *this)
  26. {
  27. return (this->pos_read == this->pos_write) && !this->full;
  28. }
  29. static int midibuf_is_full(struct midi_buffer *this)
  30. {
  31. return this->full;
  32. }
  33. void line6_midibuf_reset(struct midi_buffer *this)
  34. {
  35. this->pos_read = this->pos_write = this->full = 0;
  36. this->command_prev = -1;
  37. }
  38. int line6_midibuf_init(struct midi_buffer *this, int size, int split)
  39. {
  40. this->buf = kmalloc(size, GFP_KERNEL);
  41. if (this->buf == NULL)
  42. return -ENOMEM;
  43. this->size = size;
  44. this->split = split;
  45. line6_midibuf_reset(this);
  46. return 0;
  47. }
  48. int line6_midibuf_bytes_free(struct midi_buffer *this)
  49. {
  50. return
  51. midibuf_is_full(this) ?
  52. 0 :
  53. (this->pos_read - this->pos_write + this->size - 1) % this->size +
  54. 1;
  55. }
  56. int line6_midibuf_bytes_used(struct midi_buffer *this)
  57. {
  58. return
  59. midibuf_is_empty(this) ?
  60. 0 :
  61. (this->pos_write - this->pos_read + this->size - 1) % this->size +
  62. 1;
  63. }
  64. int line6_midibuf_write(struct midi_buffer *this, unsigned char *data,
  65. int length)
  66. {
  67. int bytes_free;
  68. int length1, length2;
  69. int skip_active_sense = 0;
  70. if (midibuf_is_full(this) || (length <= 0))
  71. return 0;
  72. /* skip trailing active sense */
  73. if (data[length - 1] == 0xfe) {
  74. --length;
  75. skip_active_sense = 1;
  76. }
  77. bytes_free = line6_midibuf_bytes_free(this);
  78. if (length > bytes_free)
  79. length = bytes_free;
  80. if (length > 0) {
  81. length1 = this->size - this->pos_write;
  82. if (length < length1) {
  83. /* no buffer wraparound */
  84. memcpy(this->buf + this->pos_write, data, length);
  85. this->pos_write += length;
  86. } else {
  87. /* buffer wraparound */
  88. length2 = length - length1;
  89. memcpy(this->buf + this->pos_write, data, length1);
  90. memcpy(this->buf, data + length1, length2);
  91. this->pos_write = length2;
  92. }
  93. if (this->pos_write == this->pos_read)
  94. this->full = 1;
  95. }
  96. return length + skip_active_sense;
  97. }
  98. int line6_midibuf_read(struct midi_buffer *this, unsigned char *data,
  99. int length, int read_type)
  100. {
  101. int bytes_used;
  102. int length1, length2;
  103. int command;
  104. int midi_length;
  105. int repeat = 0;
  106. int i;
  107. /* we need to be able to store at least a 3 byte MIDI message */
  108. if (length < 3)
  109. return -EINVAL;
  110. if (midibuf_is_empty(this))
  111. return 0;
  112. bytes_used = line6_midibuf_bytes_used(this);
  113. if (length > bytes_used)
  114. length = bytes_used;
  115. length1 = this->size - this->pos_read;
  116. command = this->buf[this->pos_read];
  117. /*
  118. PODxt always has status byte lower nibble set to 0010,
  119. when it means to send 0000, so we correct if here so
  120. that control/program changes come on channel 1 and
  121. sysex message status byte is correct
  122. */
  123. if (read_type == LINE6_MIDIBUF_READ_RX) {
  124. if (command == 0xb2 || command == 0xc2 || command == 0xf2) {
  125. unsigned char fixed = command & 0xf0;
  126. this->buf[this->pos_read] = fixed;
  127. command = fixed;
  128. }
  129. }
  130. /* check MIDI command length */
  131. if (command & 0x80) {
  132. midi_length = midibuf_message_length(command);
  133. this->command_prev = command;
  134. } else {
  135. if (this->command_prev > 0) {
  136. int midi_length_prev =
  137. midibuf_message_length(this->command_prev);
  138. if (midi_length_prev > 1) {
  139. midi_length = midi_length_prev - 1;
  140. repeat = 1;
  141. } else
  142. midi_length = -1;
  143. } else
  144. midi_length = -1;
  145. }
  146. if (midi_length < 0) {
  147. /* search for end of message */
  148. if (length < length1) {
  149. /* no buffer wraparound */
  150. for (i = 1; i < length; ++i)
  151. if (this->buf[this->pos_read + i] & 0x80)
  152. break;
  153. midi_length = i;
  154. } else {
  155. /* buffer wraparound */
  156. length2 = length - length1;
  157. for (i = 1; i < length1; ++i)
  158. if (this->buf[this->pos_read + i] & 0x80)
  159. break;
  160. if (i < length1)
  161. midi_length = i;
  162. else {
  163. for (i = 0; i < length2; ++i)
  164. if (this->buf[i] & 0x80)
  165. break;
  166. midi_length = length1 + i;
  167. }
  168. }
  169. if (midi_length == length)
  170. midi_length = -1; /* end of message not found */
  171. }
  172. if (midi_length < 0) {
  173. if (!this->split)
  174. return 0; /* command is not yet complete */
  175. } else {
  176. if (length < midi_length)
  177. return 0; /* command is not yet complete */
  178. length = midi_length;
  179. }
  180. if (length < length1) {
  181. /* no buffer wraparound */
  182. memcpy(data + repeat, this->buf + this->pos_read, length);
  183. this->pos_read += length;
  184. } else {
  185. /* buffer wraparound */
  186. length2 = length - length1;
  187. memcpy(data + repeat, this->buf + this->pos_read, length1);
  188. memcpy(data + repeat + length1, this->buf, length2);
  189. this->pos_read = length2;
  190. }
  191. if (repeat)
  192. data[0] = this->command_prev;
  193. this->full = 0;
  194. return length + repeat;
  195. }
  196. int line6_midibuf_ignore(struct midi_buffer *this, int length)
  197. {
  198. int bytes_used = line6_midibuf_bytes_used(this);
  199. if (length > bytes_used)
  200. length = bytes_used;
  201. this->pos_read = (this->pos_read + length) % this->size;
  202. this->full = 0;
  203. return length;
  204. }
  205. void line6_midibuf_destroy(struct midi_buffer *this)
  206. {
  207. kfree(this->buf);
  208. this->buf = NULL;
  209. }